/* -------------------------------------------------------------------------
 *  A repertory of multi primitive-to-primitive (MP2P) ICP algorithms in C++
 * Copyright (C) 2018-2021 Jose Luis Blanco, University of Almeria
 * See LICENSE for license information.
 * ------------------------------------------------------------------------- */
/**
 * @file   errorTerms.cpp
 * @brief
 * @author Francisco José Mañas Álvarez, Jose Luis Blanco Claraco
 * @date   Aug 4, 2020
 */

#include <mp2p_icp/errorTerms.h>
#include <mp2p_icp/optimal_tf_gauss_newton.h>
#include <mrpt/math/CVectorFixed.h>
#include <mrpt/math/TPoint3D.h>
#include <mrpt/math/geometry.h>
#include <mrpt/math/ops_containers.h>  // dotProduct()
#include <mrpt/poses/CPose3D.h>
#include <mrpt/poses/Lie/SE.h>

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

using namespace mp2p_icp;
using namespace mrpt::math;

mrpt::math::CVectorFixedDouble<3> mp2p_icp::error_point2point(
    const mrpt::tfest::TMatchingPair&                           pairing,
    const mrpt::poses::CPose3D&                                 relativePose,
    mrpt::optional_ref<mrpt::math::CMatrixFixed<double, 3, 12>> jacobian)
{
    MRPT_START
    mrpt::math::CVectorFixedDouble<3> error;
    const mrpt::math::TPoint3D&       l = pairing.local;

    const mrpt::math::TPoint3D g = relativePose.composePoint(l);

    error[0] = g.x - pairing.global.x;
    error[1] = g.y - pairing.global.y;
    error[2] = g.z - pairing.global.z;

    // It's possible change the error to scalar with the function
    // g.DistanceTo(l) Eval Jacobian:
    if (jacobian)
    {
        mrpt::math::CMatrixFixed<double, 3, 12>& J_aux = jacobian.value().get();
        // clang-format off
        J_aux = (Eigen::Matrix<double, 3, 12>() <<
                 l.x,   0,   0, l.y,   0,    0, l.z,   0,   0,  1,  0,  0,
                   0, l.x,   0,   0, l.y,    0,   0, l.z,   0,  0,  1,  0,
                   0,   0, l.x,   0,   0,  l.y,   0,   0, l.z,  0,  0,  1
                 ).finished();
        // clang-format on
    }

    return error;
    MRPT_END
}

mrpt::math::CVectorFixedDouble<3> mp2p_icp::error_point2line(
    const mp2p_icp::point_line_pair_t&                          pairing,
    const mrpt::poses::CPose3D&                                 relativePose,
    mrpt::optional_ref<mrpt::math::CMatrixFixed<double, 3, 12>> jacobian)
{
    MRPT_START
    mrpt::math::CVectorFixedDouble<3> error;
    const auto&                       lnGlob = pairing.ln_global;

    // local and global point:
    const mrpt::math::TPoint3D l = pairing.pt_local;
    const mrpt::math::TPoint3D g = relativePose.composePoint(l);

    // Module of vector director of line
    const auto& u = lnGlob.director;
    const auto  q = (g - lnGlob.pBase);

    const auto uq = mrpt::math::dotProduct<3, double>(u, q);

    error[0] = q[0] - u.x * uq;
    error[1] = q[1] - u.y * uq;
    error[2] = q[2] - u.z * uq;

    if (jacobian)
    {
        // J1
        const Eigen::Matrix<double, 3, 3> J1 =
            (Eigen::Matrix<double, 3, 3>() << 1 - mrpt::square(u.x), -u.x * u.y,
             -u.x * u.z, -u.y * u.x, 1 - mrpt::square(u.y), -u.y * u.z,
             -u.z * u.x, -u.z * u.y, 1 - mrpt::square(u.z))
                .finished();
        // J2
        // clang-format off
        Eigen::Matrix<double, 3, 12> J2 =
            (Eigen::Matrix<double, 3, 12>() <<
             l.x,   0,   0, l.y,   0,    0, l.z,   0,   0,  1,  0,  0,
               0, l.x,   0,   0, l.y,    0,   0, l.z,   0,  0,  1,  0,
               0,   0, l.x,   0,   0,  l.y,   0,   0, l.z,  0,  0,  1
             ).finished();
        // clang-format on
        mrpt::math::CMatrixFixed<double, 3, 12>& J_aux = jacobian.value().get();

        J_aux = J1 * J2;
    }
    return error;
    MRPT_END
}

mrpt::math::CVectorFixedDouble<3> mp2p_icp::error_point2plane(
    const mp2p_icp::point_plane_pair_t&                         pairing,
    const mrpt::poses::CPose3D&                                 relativePose,
    mrpt::optional_ref<mrpt::math::CMatrixFixed<double, 3, 12>> jacobian)
{
    MRPT_START
    mrpt::math::CVectorFixedDouble<3> error;
    const auto&                       p      = pairing.pt_local;
    const auto&                       pl_aux = pairing.pl_global.plane;
    const mrpt::math::TPoint3D        l      = TPoint3D(p.x, p.y, p.z);
    mrpt::math::TPoint3D              g;
    relativePose.composePoint(l, g);
    double mod_n = mrpt::square(pl_aux.coefs[0]) +
                   mrpt::square(pl_aux.coefs[1]) +
                   mrpt::square(pl_aux.coefs[2]);

    error[0] = -(pl_aux.coefs[0] / mod_n) *
               (pl_aux.coefs[0] * g.x + pl_aux.coefs[1] * g.y +
                pl_aux.coefs[2] * g.z + pl_aux.coefs[3]);
    error[1] = -(pl_aux.coefs[1] / mod_n) *
               (pl_aux.coefs[0] * g.x + pl_aux.coefs[1] * g.y +
                pl_aux.coefs[2] * g.z + pl_aux.coefs[3]);
    error[2] = -(pl_aux.coefs[2] / mod_n) *
               (pl_aux.coefs[0] * g.x + pl_aux.coefs[1] * g.y +
                pl_aux.coefs[2] * g.z + pl_aux.coefs[3]);
    if (jacobian)
    {
        // Eval Jacobian:
        // J1
        const Eigen::Matrix<double, 3, 3> J1 =
            (Eigen::Matrix<double, 3, 3>()
                 << -mrpt::square(pl_aux.coefs[0]) / mod_n,
             -pl_aux.coefs[0] * pl_aux.coefs[1] / mod_n,
             -pl_aux.coefs[0] * pl_aux.coefs[2] / mod_n,
             -pl_aux.coefs[1] * pl_aux.coefs[0] / mod_n,
             -mrpt::square(pl_aux.coefs[1]) / mod_n,
             -pl_aux.coefs[1] * pl_aux.coefs[2] / mod_n,
             -pl_aux.coefs[2] * pl_aux.coefs[0] / mod_n,
             -pl_aux.coefs[2] * pl_aux.coefs[1] / mod_n,
             -mrpt::square(pl_aux.coefs[2]) / mod_n)
                .finished();
        // J2
        // clang-format off
        const Eigen::Matrix<double, 3, 12> J2 =
            (Eigen::Matrix<double, 3, 12>() <<
             l.x,   0,   0, l.y,   0,    0, l.z,   0,   0,  1,  0,  0,
               0, l.x,   0,   0, l.y,    0,   0, l.z,   0,  0,  1,  0,
               0,   0, l.x,   0,   0,  l.y,   0,   0, l.z,  0,  0,  1
             ).finished();
        // clang-format on

        mrpt::math::CMatrixFixed<double, 3, 12>& J_aux = jacobian.value().get();
        J_aux                                          = J1 * J2;
    }
    return error;
    MRPT_END
}

mrpt::math::CVectorFixedDouble<4> mp2p_icp::error_line2line(
    const mp2p_icp::matched_line_t&                             pairing,
    const mrpt::poses::CPose3D&                                 relativePose,
    mrpt::optional_ref<mrpt::math::CMatrixFixed<double, 4, 12>> jacobian)
{
    MRPT_START
    mrpt::math::CVectorFixedDouble<4> error;
    mrpt::math::TLine3D               ln_aux;
    mrpt::math::TPoint3D              g;

    const auto& p0 = pairing.ln_local.pBase;
    const auto& u0 = pairing.ln_local.director;
    const auto& p1 = pairing.ln_global.pBase;
    const auto& u1 = pairing.ln_global.director;

    relativePose.composePoint(p0, g);
    ln_aux.pBase = mrpt::math::TPoint3D(g);

    // Homogeneous matrix calculation
    mrpt::math::CMatrixDouble44 aux;
    relativePose.getHomogeneousMatrix(aux);
    const Eigen::Matrix<double, 4, 4> T = aux.asEigen();

    // Projection of the director vector for the new pose
    const Eigen::Matrix<double, 1, 4> U =
        (Eigen::Matrix<double, 1, 4>() << u0[0], u0[1], u0[2], 1).finished();
    const Eigen::Matrix<double, 1, 4> U_T = U * T;
    ln_aux.director                       = {U_T[0], U_T[1], U_T[2]};

    // Angle formed between the lines
    double alfa = getAngle(pairing.ln_global, ln_aux) * 180 / (2 * 3.14159265);
    /*
        std::cout << "\nLine 1:\n"
                  <<  pairing.ln_this << "\nLine 2:\n"
                   << pairing.ln_other << "\nLine 2':\n"
                   << ln_aux << "\nAngle:\n"
                   << alfa << "\nT:\n"
                   <<  T << "\n";
    */

    // p_r0 = (p-r_{0,r}). Ec.20
    const Eigen::Matrix<double, 1, 3> p_r2 =
        (Eigen::Matrix<double, 1, 3>() << ln_aux.pBase.x - p1.x,
         ln_aux.pBase.y - p1.y, ln_aux.pBase.z - p1.z)
            .finished();

    const Eigen::Matrix<double, 1, 3> rv =
        (Eigen::Matrix<double, 1, 3>() << u1[0], u1[1], u1[2]).finished();

    // Relationship between lines
    const double tolerance = 0.01;
    if (abs(alfa) < tolerance)
    {  // Parallel
        // Error: Ec.20
        error[0] = mrpt::square(pairing.ln_global.distance(ln_aux.pBase));
        if (jacobian)
        {
            // Module of vector director of line
            double mod_rv = rv * rv.transpose();

            // J1: Ec.22
            Eigen::Matrix<double, 1, 3> J1 =
                2 * p_r2 - (2 / mod_rv) * (p_r2 * rv.transpose()) * rv;
            // J2: Ec.23
            // clang-format off
            const Eigen::Matrix<double, 3, 12> J2 =
                (Eigen::Matrix<double, 3, 12>() <<
                 p0.x,    0,    0, p0.y,    0,    0, p0.z,    0,    0, 1, 0, 0,
                    0, p0.x,    0,    0, p0.y,    0,    0, p0.z,    0, 0, 1, 0,
                    0,    0, p0.x,    0,    0, p0.y,    0,    0, p0.z, 0, 0, 1
                 ).finished();
            // clang-format on
            // Build Jacobian
            mrpt::math::CMatrixFixed<double, 4, 12>& J_auxp =
                jacobian.value().get();
            J_auxp.block<1, 12>(0, 0) = J1 * J2;
        }
    }
    else
    {  // Rest
        // Error:
        // Cross product (r_u x r_2,v)
        const double rw_x = U_T[1] * u1[2] - U_T[2] * u1[1];
        const double rw_y = -(U_T[0] * u1[2] - U_T[2] * u1[0]);
        const double rw_z = U_T[0] * u1[1] - U_T[1] * u1[0];

        const Eigen::Matrix<double, 1, 3> r_w =
            (Eigen::Matrix<double, 1, 3>() << rw_x, rw_y, rw_z).finished();
        double aux_rw = r_w * r_w.transpose();
        // Error 1. Ec.26
        error[0] = p_r2.dot(r_w) / sqrt(aux_rw);
        // Error 2. Ec.27
        error[1] = U_T[0] - u1[0];
        error[2] = U_T[1] - u1[1];
        error[3] = U_T[2] - u1[2];
        if (jacobian)
        {
            // J1.1: Ec.32
            Eigen::Matrix<double, 1, 3> J1_1 = r_w / sqrt(aux_rw);

            // J1.2:
            // A
            const double A =
                p_r2[0] * r_w[0] + p_r2[1] * r_w[1] + p_r2[2] * r_w[2];
            const double Ax = -u1[2] * p_r2[1] + u1[1] * p_r2[2];
            const double Ay = u1[2] * p_r2[0] - u1[0] * p_r2[2];
            const double Az = -u1[1] * p_r2[0] + u1[0] * p_r2[1];
            // B
            const double B  = sqrt(aux_rw);
            const double Bx = (-u1[2] * r_w[1] + u1[1] * r_w[2]) / B;
            const double By = (u1[2] * r_w[0] + u1[0] * r_w[2]) / B;
            const double Bz = (-u1[1] * r_w[0] + u1[0] * r_w[1]) / B;

#if 0
            std::cout << "\nA: " << A << "\nAx: " << Ax << "\nAy: " << Ay
                      << "\nAz: " << Az << "\nB: " << B << "\nBx: " << Bx
                      << "\nBy: " << By << "\nBz: " << Bz << "\n";
#endif

            // Ec.36
            // clang-format off
            Eigen::Matrix<double, 1, 3> J1_2 =
                (Eigen::Matrix<double, 1, 3>() <<
                 (Ax * B - A * Bx) / mrpt::square(B),
                 (Ay * B - A * By) / mrpt::square(B),
                 (Az * B - A * Bz) / mrpt::square(B)
                 ).finished();
            // clang-format on

            // J1.3: Ec.37-38
            // clang-format off
            Eigen::Matrix<double, 3, 6> J1_3 =
                (Eigen::Matrix<double, 3, 6>() <<
                 0, 0, 0, 1, 0, 0,
                 0, 0, 0, 0, 1, 0,
                 0, 0, 0, 0, 0, 1
                 ).finished();
            // clang-format on

            // J1: Ec.29
            Eigen::Matrix<double, 4, 6> J1;
            J1.block<1, 3>(0, 0) = J1_1;
            J1.block<1, 3>(0, 3) = J1_2;
            J1.block<3, 6>(1, 0) = J1_3;

            // J2: Ec.39-41
            // clang-format off
            const Eigen::Matrix<double, 6, 12> J2 =
                (Eigen::Matrix<double, 6, 12>() <<
                 p0.x,     0,     0,  p0.y,     0,     0,  p0.z,     0,     0, 1, 0, 0,
                    0,  p0.x,     0,     0,  p0.y,     0,     0,  p0.z,     0, 0, 1, 0,
                    0,     0,  p0.x,     0,     0,  p0.y,     0,     0,  p0.z, 0, 0, 1,
                -u0[0],     0,     0, -u0[1],     0,     0, -u0[2],     0,     0, 0, 0, 0,
                    0, -u0[0],     0,     0, -u0[1],     0,     0, -u0[2],     0, 0, 0, 0,
                    0,     0, -u0[0],     0,     0, -u0[1],     0,     0, -u0[2], 0, 0, 0
                ).finished();
            // clang-format on
            // Build Jacobian
            mrpt::math::CMatrixFixed<double, 4, 12>& J_aux =
                jacobian.value().get();
            J_aux.block<4, 12>(0, 0) = J1 * J2;

            std::cout << "\nJ1:\n" << J1 << "\nJ2:\n" << J2 << "\n";
        }
    }
    //    std::cout<<"\nError:\n"<<error;
    return error;
    MRPT_END
}

mrpt::math::CVectorFixedDouble<3> mp2p_icp::error_plane2plane(
    const mp2p_icp::matched_plane_t&                            pairing,
    const mrpt::poses::CPose3D&                                 relativePose,
    mrpt::optional_ref<mrpt::math::CMatrixFixed<double, 3, 12>> jacobian)
{
    MRPT_START
    mrpt::math::CVectorFixedDouble<3> error;

    const auto nl = pairing.p_local.plane.getNormalVector();
    const auto ng = pairing.p_global.plane.getNormalVector();

    const auto p_oplus_nl = relativePose.rotateVector(nl);

    for (int i = 0; i < 3; i++) error[i] = p_oplus_nl[i] - ng[i];

    if (jacobian)
    {
        // Eval Jacobian:

        // df_oplus(A,p)/d_A. Section 7.3.2 tech. report:
        // "A tutorial on SE(3) transformation parameterizations and
        // on-manifold optimization"
        // Modified, to discard the last I_3 block, since this particular
        // cost function is insensible to translations.

        // clang-format off
        mrpt::math::CMatrixFixed<double, 3, 12>& J_aux = jacobian.value().get();
        J_aux = (Eigen::Matrix<double, 3, 12>() <<
                 nl.x,    0,    0, nl.y,    0,    0, nl.z,    0,    0,  0,  0,  0,
                    0, nl.x,    0,    0, nl.y,    0,    0, nl.z,    0,  0,  0,  0,
                    0,    0, nl.x,    0,    0, nl.y,    0,    0, nl.z,  0,  0,  0
                ).finished();
        // clang-format on
    }
    return error;
    MRPT_END
}


/*
 * fusion_algebra.hpp
 *
 * Copyright 2009-2012 Karsten Ahnert
 * Copyright 2009-2012 Mario Mulansky
 *
 * 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)
 */

#ifndef FUSION_ALGEBRA_HPP_
#define FUSION_ALGEBRA_HPP_

#include <boost/array.hpp>

#include <iostream>

template< size_t n >
struct fusion_algebra
{
    template< typename T , size_t dim >
    inline static void foreach( boost::array< T , dim > &x_tmp , const boost::array< T , dim > &x ,
            const boost::array< double , n > &a ,
            const boost::array< T , dim > k_vector[n] , const double dt )
    {
        for( size_t i=0 ; i<dim ; ++i )
        {
            x_tmp[i] = x[i];// + a[0]*dt*k_vector[0][i];
            for( size_t j = 0 ; j<n ; ++j )
                x_tmp[i] += a[j]*dt*k_vector[j][i];
        }
    }

    template< typename T , size_t dim >
    inline static void foreach( boost::array< T , dim > &x_tmp ,
                const boost::array< double , n > &a ,
                const boost::array< T , dim > k_vector[n] , const double dt )
    {
        for( size_t i=0 ; i<dim ; ++i )
        {
            x_tmp[i] = a[0]*dt*k_vector[0][i];
            for( size_t j = 1 ; j<n ; ++j )
                x_tmp[i] += a[j]*dt*k_vector[j][i];
         }
    }

};

/** hand-wise implementation for performance improvement for n = 1..4 **/

/* !!!!!!!   Actually, this is factor 3 slower with intel compiler, so we don'y use it !!!!!
 * Update: It increases performance on msvc 9.0 by about 30%, so it is activated for MSVC
 */

//#ifdef BOOST_MSVC

template<>
struct fusion_algebra< 1 >
{
    template< typename T , size_t dim >
    inline static void foreach( boost::array< T , dim > &x_tmp , const boost::array< T , dim > &x ,
            const boost::array< double , 1 > &a ,
            const boost::array< T , dim > *k_vector , const double dt )
    {
        for( size_t i=0 ; i<dim ; ++i )
        {
            x_tmp[i] = x[i]
                + a[0]*dt*k_vector[0][i];
        }
    }

};

template<>
struct fusion_algebra< 2 >
{

    template< typename T , size_t dim >
    inline static void foreach( boost::array< T , dim > &x_tmp , const boost::array< T , dim > &x ,
            const boost::array< double , 2 > &a ,
            const boost::array< T , dim > *k_vector , const double dt )
    {
        for( size_t i=0 ; i<dim ; ++i )
        {
            x_tmp[i] = x[i]
             + a[0]*dt*k_vector[0][i]
             + a[1]*dt*k_vector[1][i];
        }
    }

};

template<>
struct fusion_algebra< 3 >
{

    template< typename T , size_t dim >
    inline static void foreach( boost::array< T , dim > &x_tmp , const boost::array< T , dim > &x ,
            const boost::array< double , 3 > &a ,
            const boost::array< T , dim > *k_vector , const double dt )
    {
        for( size_t i=0 ; i<dim ; ++i )
        {
            x_tmp[i] = x[i]
             + a[0]*dt*k_vector[0][i]
             + a[1]*dt*k_vector[1][i]
             + a[2]*dt*k_vector[2][i];
        }
    }

};

template<>
struct fusion_algebra< 4 >
{

    template< typename T , size_t dim >
    inline static void foreach( boost::array< T , dim > &x_tmp , const boost::array< T , dim > &x ,
            const boost::array< double , 4 > &a ,
            const boost::array< T , dim > *k_vector , const double dt )
    {
        for( size_t i=0 ; i<dim ; ++i )
        {
            x_tmp[i] = x[i]
             + a[0]*dt*k_vector[0][i]
             + a[1]*dt*k_vector[1][i]
             + a[2]*dt*k_vector[2][i]
             + a[3]*dt*k_vector[3][i];
        }
    }

};

template<>
struct fusion_algebra< 5 >
{

    template< typename T , size_t dim >
    inline static void foreach( boost::array< T , dim > &x_tmp , const boost::array< T , dim > &x ,
            const boost::array< double , 5 > &a ,
            const boost::array< T , dim > *k_vector , const double dt )
    {
        for( size_t i=0 ; i<dim ; ++i )
        {
            x_tmp[i] = x[i]
             + a[0]*dt*k_vector[0][i]
             + a[1]*dt*k_vector[1][i]
             + a[2]*dt*k_vector[2][i]
             + a[3]*dt*k_vector[3][i]
             + a[4]*dt*k_vector[4][i];
        }
    }

};

template<>
struct fusion_algebra< 6 >
{

    template< typename T , size_t dim >
    inline static void foreach( boost::array< T , dim > &x_tmp , const boost::array< T , dim > &x ,
            const boost::array< double , 6 > &a ,
            const boost::array< T , dim > *k_vector , const double dt )
    {
        for( size_t i=0 ; i<dim ; ++i )
        {
            x_tmp[i] = x[i]
             + a[0]*dt*k_vector[0][i]
             + a[1]*dt*k_vector[1][i]
             + a[2]*dt*k_vector[2][i]
             + a[3]*dt*k_vector[3][i]
             + a[4]*dt*k_vector[4][i]
             + a[5]*dt*k_vector[5][i];
        }
    }

    template< typename T , size_t dim >
    inline static void foreach(boost::array<T , dim> &x_tmp ,
            const boost::array<double , 6> &a ,
            const boost::array<T , dim> *k_vector , const double dt)
    {
        for (size_t i = 0 ; i < dim ; ++i)
        {
            x_tmp[i] = a[0] * dt * k_vector[0][i] + a[1] * dt * k_vector[1][i]
                    + a[2] * dt * k_vector[2][i] + a[3] * dt * k_vector[3][i]
                    + a[4] * dt * k_vector[4][i] + a[5] * dt * k_vector[5][i];
        }
    }

};

//#endif /* BOOST_MSVC */

#endif /* FUSION_ALGEBRA_HPP_ */


#include <iostream>
#include <cmath>
#include <algorithm>
#include <vector>
#include <boost/multiprecision/cpp_int.hpp>
using namespace boost::multiprecision;
using namespace std;
int main() {
    int n; cin >> n;
    vector<int> a(n, 0);
    cpp_int sum = 0, ans = 0;
    for (int i = 0; i < n; i++) {
        cin >> a[i]; sum += a[i];
    }
    for (int i = 0; i < n - 1; i++) {
        ans += (sum -= a[i]) * a[i];
    }
    cout << ans % 1000000007 << endl;
}


#include <Eigen/Core>
#include <iostream>
#include "timer.h"
#include "ArpackFun.h"

using Eigen::MatrixXd;
using Eigen::VectorXd;
using Eigen::MatrixXcd;
using Eigen::VectorXcd;
using Eigen::Lower;
typedef Eigen::Map<VectorXd> MapVec;

void eigs_sym_F77(MatrixXd &M, VectorXd &init_resid, int k, int m,
                  double &time_used, double &prec_err, int &nops)
{
    double start, end;
    prec_err = -1.0;
    start = get_wall_time();

    // Begin ARPACK
    //
    // Initial value of ido
    int ido = 0;
    // 'I' means standard eigen value problem, A * x = lambda * x
    char bmat = 'I';
    // dimension of A (n by n)
    int n = M.rows();
    // Specify selection criteria
    // "LM": largest magnitude
    char which[3] = {'L', 'M', '\0'};
    // Number of eigenvalues requested
    int nev = k;
    // Precision
    double tol = 1e-10;
    // Residual vector
    double *resid = new double[n]();
    std::copy(init_resid.data(), init_resid.data() + n, resid);
    // Number of Ritz values used
    int ncv = m;
    // Vector of eigenvalues
    VectorXd evals(nev);
    // Matrix of eigenvectors
    MatrixXd evecs(n, ncv);

    // Store final results of eigenvectors
    // double *V = new double[n * ncv]();
    double *V = evecs.data();
    // Leading dimension of V, required by FORTRAN
    int ldv = n;
    // Control parameters
    int *iparam = new int[11]();
    iparam[1 - 1] = 1;     // ishfts
    iparam[3 - 1] = 1000;  // maxitr
    iparam[7 - 1] = 1;     // mode
    // Some pointers
    int *ipntr = new int[11]();
    /* workd has 3 columns.
     * ipntr[2] - 1 ==> first column to store B * X,
     * ipntr[1] - 1 ==> second to store Y,
     * ipntr[0] - 1 ==> third to store X. */
    double *workd = new double[3 * n]();
    int lworkl = ncv * (ncv + 8);
    double *workl = new double[lworkl]();
    // Error flag. 0 means random initialization,
    // otherwise using resid as initial value
    int info = 1;

    saupd(ido, bmat, n, which,
          nev, tol, resid,
          ncv, V, ldv,
          iparam, ipntr, workd,
          workl, lworkl, info);
    // ido == -1 or ido == 1 means more iterations needed
    while (ido == -1 || ido == 1)
    {
        MapVec vec_in(&workd[ipntr[0] - 1], n);
        MapVec vec_out(&workd[ipntr[1] - 1], n);
        vec_out.noalias() = M.selfadjointView<Lower>() * vec_in;

        saupd(ido, bmat, n, which,
              nev, tol, resid,
              ncv, V, ldv,
              iparam, ipntr, workd,
              workl, lworkl, info);
    }

    // info > 0 means warning, < 0 means error
    if (info > 0)
        std::cout << "warnings occured" << std::endl;
    if (info < 0)
    {
        delete[] workl;
        delete[] workd;
        delete[] ipntr;
        delete[] iparam;
        delete[] resid;

        std::cout << "errors occured" << std::endl;
        end = get_wall_time();
        time_used = (end - start) * 1000;

        return;
    }

    // Retrieve results
    //
    // Whether to calculate eigenvectors or not.
    bool rvec = true;
    // 'A' means to calculate Ritz vectors
    // 'P' to calculate Schur vectors
    char howmny = 'A';
    // Vector of eigenvalues
    double *d = evals.data();
    // Used to store results, will use V instead.
    double *Z = V;
    // Leading dimension of Z, required by FORTRAN
    int ldz = n;
    // Shift
    double sigma = 0;
    // Error information
    int ierr = 0;

    // Number of converged eigenvalues
    int nconv = 0;
    // Number of iterations
    int niter = 0;

    // Use seupd() to retrieve results
    seupd(rvec, howmny, d,
          Z, ldz, sigma, bmat,
          n, which, nev, tol,
          resid, ncv, V, ldv,
          iparam, ipntr, workd, workl,
          lworkl, ierr);

    // Obtain 'nconv' converged eigenvalues
    nconv = iparam[5 - 1];
    // 'niter' number of iterations
    niter = iparam[9 - 1];

    // Free memory of temp arrays
    delete[] workl;
    delete[] workd;
    delete[] ipntr;
    delete[] iparam;
    delete[] resid;

    // ierr < 0 means error
    if (ierr < 0)
    {
        std::cout << "errors occured" << std::endl;
        end = get_wall_time();
        time_used = (end - start) * 1000;

        return;
    }

    /* std::cout << "computed eigenvalues D = \n" << evals.transpose() << std::endl;
    std::cout << "first 5 rows of computed eigenvectors U = \n" <<
    evecs.topLeftCorner(5, nconv) << std::endl;
    std::cout << "nconv = " << nconv << std::endl;
    std::cout << "nops = " << niter << std::endl; */

    end = get_wall_time();
    time_used = (end - start) * 1000;
    MatrixXd err = M * evecs.leftCols(nev) - evecs.leftCols(nev) * evals.asDiagonal();
    prec_err = err.cwiseAbs().maxCoeff();
    nops = niter;
}

void eigs_gen_F77(MatrixXd &M, VectorXd &init_resid, int k, int m,
                  double &time_used, double &prec_err, int &nops)
{
    double start, end;
    prec_err = -1.0;
    start = get_wall_time();

    // Begin ARPACK
    //
    // Initial value of ido
    int ido = 0;
    // 'I' means standard eigen value problem, A * x = lambda * x
    char bmat = 'I';
    // dimension of A (n by n)
    int n = M.rows();
    // Specify selection criteria
    // "LM": largest magnitude
    char which[3] = {'L', 'M', '\0'};
    // Number of eigenvalues requested
    int nev = k;
    // Precision
    double tol = 1e-10;
    // Residual vector
    double *resid = new double[n]();
    std::copy(init_resid.data(), init_resid.data() + n, resid);
    // Number of Ritz values used
    int ncv = m;
    // Vector of eigenvalues
    VectorXd evals_re(nev + 1);
    VectorXd evals_im(nev + 1);
    // Matrix of eigenvectors
    MatrixXd evecs(n, ncv);

    // Store final results of eigenvectors
    // double *V = new double[n * ncv]();
    double *V = evecs.data();
    // Leading dimension of V, required by FORTRAN
    int ldv = n;
    // Control parameters
    int *iparam = new int[11]();
    iparam[1 - 1] = 1;     // ishfts
    iparam[3 - 1] = 1000;  // maxitr
    iparam[7 - 1] = 1;     // mode
    // Some pointers
    int *ipntr = new int[14]();
    /* workd has 3 columns.
     * ipntr[2] - 1 ==> first column to store B * X,
     * ipntr[1] - 1 ==> second to store Y,
     * ipntr[0] - 1 ==> third to store X. */
    double *workd = new double[3 * n]();
    int lworkl = 3 * ncv * ncv + 6 * ncv;
    double *workl = new double[lworkl]();
    // Error flag. 0 means random initialization,
    // otherwise using resid as initial value
    int info = 1;

    naupd(ido, bmat, n, which,
          nev, tol, resid,
          ncv, V, ldv,
          iparam, ipntr, workd,
          workl, lworkl, info);
    // ido == -1 or ido == 1 means more iterations needed
    while (ido == -1 || ido == 1)
    {
        MapVec vec_in(&workd[ipntr[0] - 1], n);
        MapVec vec_out(&workd[ipntr[1] - 1], n);
        vec_out.noalias() = M * vec_in;

        naupd(ido, bmat, n, which,
              nev, tol, resid,
              ncv, V, ldv,
              iparam, ipntr, workd,
              workl, lworkl, info);
    }

    // info > 0 means warning, < 0 means error
    if (info > 0)
        std::cout << "warnings occured" << std::endl;
    if (info < 0)
    {
        delete[] workl;
        delete[] workd;
        delete[] ipntr;
        delete[] iparam;
        delete[] resid;

        std::cout << "errors occured" << std::endl;
        end = get_wall_time();
        time_used = (end - start) * 1000;

        return;
    }

    // Retrieve results
    //
    // Whether to calculate eigenvectors or not.
    bool rvec = true;
    // 'A' means to calculate Ritz vectors
    // 'P' to calculate Schur vectors
    char howmny = 'A';
    // Vector of eigenvalues
    double *dr = evals_re.data();
    double *di = evals_im.data();
    // Used to store results, will use V instead.
    double *Z = V;
    // Leading dimension of Z, required by FORTRAN
    int ldz = n;
    // Shift
    double sigmar = 0;
    double sigmai = 0;
    // Working space
    double *workv = new double[3 * ncv]();
    // Error information
    int ierr = 0;

    // Number of converged eigenvalues
    int nconv = 0;
    // Number of iterations
    int niter = 0;

    // Use seupd() to retrieve results
    neupd(rvec, howmny, dr, di,
          Z, ldz, sigmar, sigmai, workv,
          bmat, n, which, nev, tol,
          resid, ncv, V, ldv, iparam,
          ipntr, workd, workl, lworkl, ierr);

    // Obtain 'nconv' converged eigenvalues
    nconv = iparam[5 - 1];
    // 'niter' number of iterations
    niter = iparam[9 - 1];

    // Free memory of temp arrays
    delete[] workv;
    delete[] workl;
    delete[] workd;
    delete[] ipntr;
    delete[] iparam;
    delete[] resid;

    // ierr < 0 means error
    if (ierr < 0)
    {
        std::cout << "errors occured" << std::endl;
        end = get_wall_time();
        time_used = (end - start) * 1000;

        return;
    }

    /* VectorXcd evals(evals_re.size());
    evals.real() = evals_re;
    evals.imag() = evals_im;
    std::cout << "computed eigenvalues D = \n" << evals << std::endl;
    std::cout << "first 5 rows of computed eigenvectors U = \n" <<
        evecs.topLeftCorner(5, nconv) << std::endl;
    std::cout << "nconv = " << nconv << std::endl;
    std::cout << "nops = " << niter << std::endl; */

    end = get_wall_time();
    time_used = (end - start) * 1000;
    nops = niter;
}


// Copyright (c) 2021 fortiss GmbH
//
// Authors: Klemens Esterle and Tobias Kessler
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.

#include "parameter_preparer.hpp"
#include <math.h>
#include <algorithm>
#include <boost/math/constants/constants.hpp>
#include "common/math/math.hpp"

namespace miqp {
namespace common {
namespace parameter {
using bark::commons::ParamsPtr;
using miqp::common::parameter::RotateVector;

ParameterPreparer::ParameterPreparer(
    const ParamsPtr& params, const int nrRegions,
    const float maxVelocityFitting, const float minVelocityFitting,
    const float accLonMaxLimit, const float accLonMinLimit,
    const float jerkLonMaxLimit, const float accLatMinMaxLimit,
    const float jerkLatMinMaxLimit)
    : bark::commons::BaseType(params),
      vehicle_params_(accLonMaxLimit, accLonMinLimit, jerkLonMaxLimit,
                      accLatMinMaxLimit, jerkLatMinMaxLimit),
      fittingPolynomial_params_(FittingPolynomialParameters(
          nrRegions, maxVelocityFitting, minVelocityFitting)),
      nrRegions_(nrRegions),
      maxVelocityFitting_(maxVelocityFitting) {
  CalculateFractionParameters();
  CalculateMeanAngleVector();
}

void ParameterPreparer::CalculateFractionParameters() {
  const int nrRegions = nrRegions_;
  Eigen::VectorXd alphaVec(nrRegions_ + 1);
  alphaVec.setLinSpaced(0, 2 * M_PI);
  Eigen::MatrixXd fraction_parameters_col(nrRegions_, 2);
  fraction_parameters_col.col(0) =
      maxVelocityFitting_ * alphaVec.head(nrRegions_).array().cos();
  fraction_parameters_col.col(1) =
      maxVelocityFitting_ * alphaVec.head(nrRegions_).array().sin();
  fraction_params_.resize(nrRegions, 4);
  fraction_params_.block(0, 0, nrRegions, 2) = fraction_parameters_col;
  fraction_params_.block(0, 2, nrRegions - 1, 2) =
      fraction_parameters_col.block(1, 0, nrRegions - 1, 2);
  fraction_params_.block(nrRegions - 1, 2, 1, 2) =
      fraction_parameters_col.block(0, 0, 1, 2);
}

LimitPerRegionParameters ParameterPreparer::CalculateAccLimitsPerCar() const {
  int nr_regions = fraction_params_.rows();
  const int NumCars = 1;  // only per-car calculation, see addCar() for multiple

  LimitPerRegionParameters acc_limits_per_regions =
      LimitPerRegionParameters(NumCars, nr_regions);

  for (std::size_t i = 0; i < mean_angle_vector_.size(); ++i) {
    XYPair max_acc_pair, min_acc_pair;
    RotateLimitVectors(vehicle_params_.straight_acc_limits,
                       mean_angle_vector_[i], max_acc_pair, min_acc_pair);

    acc_limits_per_regions.min_x(0, i) = min_acc_pair.x;
    acc_limits_per_regions.min_y(0, i) = min_acc_pair.y;
    acc_limits_per_regions.max_x(0, i) = max_acc_pair.x;
    acc_limits_per_regions.max_y(0, i) = max_acc_pair.y;
  }
  return acc_limits_per_regions;
};

LimitPerRegionParameters ParameterPreparer::CalculateJerkLimitsPerCar() const {
  int nr_regions = fraction_params_.rows();
  int NumCars = 1;  // only per-car calculation, see addCar() for multiple

  LimitPerRegionParameters jerk_limits_per_regions =
      LimitPerRegionParameters(NumCars, nr_regions);

  for (std::size_t i = 0; i < mean_angle_vector_.size(); ++i) {
    XYPair max_jerk_pair, min_jerk_pair;
    RotateLimitVectors(vehicle_params_.straight_jerk_limits,
                       mean_angle_vector_[i], max_jerk_pair, min_jerk_pair);

    jerk_limits_per_regions.min_x(0, i) = min_jerk_pair.x;
    jerk_limits_per_regions.min_y(0, i) = min_jerk_pair.y;
    jerk_limits_per_regions.max_x(0, i) = max_jerk_pair.x;
    jerk_limits_per_regions.max_y(0, i) = max_jerk_pair.y;
  }
  return jerk_limits_per_regions;
};

void ParameterPreparer::CalculateMeanAngleVector() {
  int nr_regions = fraction_params_.rows();
  for (int idxreg = 0; idxreg < nr_regions; ++idxreg) {
    double angle_line1 =
        atan2(fraction_params_(idxreg, 1), fraction_params_(idxreg, 0));
    double angle_line2 =
        atan2(fraction_params_(idxreg, 3), fraction_params_(idxreg, 2));

    miqp::common::math::WrapRadiantTo2Pi(angle_line1);
    miqp::common::math::WrapRadiantTo2Pi(angle_line2);

    if (idxreg + 1 == nr_regions) {
      angle_line2 = angle_line2 + 2 * boost::math::constants::pi<double>();
    }
    mean_angle_vector_.push_back((angle_line1 + angle_line2) / 2);
  }
}

void ParameterPreparer::RotateLimitVectors(const StraightLimits& straight_lim,
                                           const float angle, XYPair& max_pair,
                                           XYPair& min_pair) const {
  XYPair max_max = RotateVector(
      straight_lim.straight_long_max + straight_lim.straight_lat_max, angle);
  XYPair max_min = RotateVector(
      straight_lim.straight_long_max + straight_lim.straight_lat_min, angle);
  XYPair min_max = RotateVector(
      straight_lim.straight_long_min + straight_lim.straight_lat_max, angle);
  XYPair min_min = RotateVector(
      straight_lim.straight_long_min + straight_lim.straight_lat_min, angle);

  float max_x =
      std::max(std::max(max_max.x, max_min.x), std::max(min_max.x, min_min.x));
  float min_x =
      std::min(std::min(max_max.x, max_min.x), std::min(min_max.x, min_min.x));
  float max_y =
      std::max(std::max(max_max.y, min_max.y), std::max(min_min.y, max_min.y));
  float min_y =
      std::min(std::min(max_max.y, min_max.y), std::min(min_min.y, max_min.y));

  miqp::common::math::SwapIfNeeded(max_x, min_x);
  miqp::common::math::SwapIfNeeded(max_y, min_y);
  assert(max_x >= min_x);
  assert(max_y >= min_y);

  max_pair.x = max_x;
  max_pair.y = max_y;

  min_pair.x = min_x;
  min_pair.y = min_y;
};

}  // namespace parameter
}  // namespace common
}  // namespace miqp


#include "MaximumLikelihood.h"
#include "../Ensemble.h"
#include "../Parameters.h"
#include "../Probabilistic.h"
#include "../Obs.h"
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/io.hpp>

EstimatorMaximumLikelihood::EstimatorMaximumLikelihood(const Options& iOptions, const Data& iData, const Probabilistic& iScheme) :
      EstimatorProbabilistic(iOptions, iData, iScheme),
      mForceIdentityMatrix(false) {
   //! Should the covariance matrix for finding the minimum be forced to be an  identity matrix?
   iOptions.getValue("forceIdentityMatrix", mForceIdentityMatrix);
   iOptions.check();
}
void EstimatorMaximumLikelihood::update(const std::vector<Ensemble>& iEnsemble,
      const std::vector<Obs>& iObs,
      Parameters& iParameters) const {

   // Set up coefficients
   // Set up D2S
   int N = getNumCoefficients(iParameters);
   boost::numeric::ublas::matrix<float> D2S(N,N);
   boost::numeric::ublas::matrix<float> accumD2S(N,N);
   int Istart = N;
   for(int k = 0; k < N*N; k++) {
      int i = k % N;
      int j = floor(k/N);
      D2S(i,j) = iParameters[Istart + k];
      accumD2S(i,j) = 0;
   }
   boost::numeric::ublas::matrix<float> Rinv(N,N);
   bool status = getInverse(D2S, Rinv);
   assert(status);
   //std::cout << "D2S[0] = " << D2S(0,0) << " Inverse = " << Rinv(0,0) << std::endl;

   // Initialize new parameters
   std::vector<float> accumCoeff;
   accumCoeff.resize(N);

   for(int i = 0; i < N; i++) {
      accumCoeff[i] = 0;
   }
   int numUpdates = 0;

   for(int t = 0 ; t < (int) iObs.size(); t++) {
      float obs = iObs[t].getValue();
      Ensemble ens = iEnsemble[t];
      if(Global::isValid(obs)) {
         Parameters coeffs;
         getCoefficients(iParameters, coeffs);

         std::vector<float> H;
         bool status = getH(obs, ens, coeffs, H);
         if(status) {
            //std::cout << "H = " << H[0] << std::endl;
            for(int i = 0; i < N; i++) {
               for(int j = 0; j < N; j++) {
                  accumD2S(i,j) += H[i]*H[j];
                  //if(H[j] > 200)
                  //std::cout << "H[j] = " << H[j] << " Rinv(i,j)= " << Rinv(i,j)  << std::endl;
                  accumCoeff[i] += H[j]*Rinv(i,j);
               }
            }
            numUpdates++;
         }
      }
   }

   if(numUpdates > 0) {
      // Update coefficients Pinson (eq 19)
      for(int i = 0; i < N; i++) {
         //std::cout << "Old parameter = " << iParameters[i];
         iParameters[i] = iParameters[i] + 1/mEfold * accumCoeff[i]/numUpdates;
         //std::cout << " new = " << iParameters[i] << std::endl;
      }

      // Update D2S Pinson (eq 20)
      for(int i = 0; i < N; i++) {
         for(int j = 0; j < N; j++) {
            int index = Istart + i + N*j;
            //std::cout << "Old est parameter = " << iParameters[index];
            if(mForceIdentityMatrix)
               iParameters[index] = (i==j) ? 1 : 0;
            else
               iParameters[index] = getLambda() * D2S(i,j) + 1/mEfold * accumD2S(i,j)/numUpdates;
            //std::cout << " new = " << iParameters[index] << std::endl;
         }
      }
   }
   else {
      //std::cout << "Can't update" << std::endl;
   }

}

void EstimatorMaximumLikelihood::getDefaultParameters(const Parameters& iSchemeParameters,
      Parameters& iParameters) const {
   int N = iSchemeParameters.size();
   std::vector<float> param;
   for(int i = 0; i < N; i++) {
      for(int j = 0; j < N; j++) {
         param.push_back(i == j);
      }
   }
   iParameters.setAllParameters(param);
}
int EstimatorMaximumLikelihood::getIndex(int i, int j, int iSize) {
   return i*iSize + j;
}
float EstimatorMaximumLikelihood::getLambda() const {
   return 1 - 1/mEfold;
}

// TODO: Pass only coefficients in here
bool EstimatorMaximumLikelihood::getH(float iObs, const Ensemble& iEnsemble, const Parameters& iCoeffs, std::vector<float>& iH) const {
   int N = iCoeffs.size();

   iH.clear();
   iH.resize(N);

   std::vector<float> gradL;
   bool status = getGradL(iObs, iEnsemble, iCoeffs, gradL);
   if(!status) {
      return false;
   }
   //std::cout << "Grad = " << gradL[0] << std::endl;
   assert((int) gradL.size() == N);

   float L = mScheme.getLikelihood(iObs, iEnsemble, iCoeffs);
   if(L == 0) {
      std::cout << "MaximumLikelihood: Obs = " << iObs << std::endl;
   }
   assert(L > 0);
   if(!Global::isValid(L)) {
      return false;
   }
   for(int i = 0; i < N; i++) {
      float value = gradL[i]/L;
      iH[i] = value;
   }
   return true;
}

bool EstimatorMaximumLikelihood::getGradL(float iObs, const Ensemble& iEnsemble, const Parameters& iCoeffs, std::vector<float>& iGradL) const {
   int N = iCoeffs.size();

   iGradL.clear();
   iGradL.resize(N);

   float dx = 0.001;
   float L0 = mScheme.getLikelihood(iObs, iEnsemble, iCoeffs);
   if(!Global::isValid(L0))
      return false;
   for(int i = 0; i < N; i++) {
      // Perturb current parameter
      Parameters par = iCoeffs;
      par[i] += dx;

      // Compute likelihood derivative
      float currL = mScheme.getLikelihood(iObs, iEnsemble, par);
      if(!Global::isValid(currL))
         return false;
      float dL = currL - L0;
      iGradL[i] = dL/dx;
   }
   return true;
}

bool EstimatorMaximumLikelihood::getInverse(boost::numeric::ublas::matrix<float> iMatrix, boost::numeric::ublas::matrix<float>& iInverse) {
   // Taken from https://gist.github.com/2464434
   using namespace boost::numeric::ublas;
   typedef permutation_matrix<std::size_t> pmatrix;
   // create a working copy of the input
   matrix<float> A(iMatrix);
   // create a permutation matrix for the LU-factorization
   pmatrix pm(A.size1());

   // perform LU-factorization
   int res = lu_factorize(A,pm);
   if( res != 0 ) return false;

   // create identity matrix of "inverse"
   iInverse.assign(identity_matrix<float>(A.size1()));

   // backsubstitute to get the inverse
   lu_substitute(A, pm, iInverse);

   return true;
}

int EstimatorMaximumLikelihood::getNumCoefficients(const Parameters& iParameters) const {
   int N = (-1 + pow(1 + 4*iParameters.size(),0.5))/2;
   assert(N + N*N == iParameters.size());
   return N;
}


///////////////////////////////////////////////////////////////////////////////
// statistics::model::functional::log_posterior_evaluator.hpp                //
//                                                                           //
//  Copyright 2009 Erwann Rogard. 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)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_MODEL_FUNCTIONAL_LOG_POSTERIOR_EVALUATOR_HPP_ER_2009
#define BOOST_STATISTICS_MODEL_FUNCTIONAL_LOG_POSTERIOR_EVALUATOR_HPP_ER_2009
#include <boost/type_traits.hpp>
#include <boost/call_traits.hpp>
#include <boost/operators.hpp>
#include <boost/binary_op/algorithm/for_each.hpp>
#include <boost/joint_dist/unscope/log_unnormalized_pdf.hpp>
#include <boost/scalar_dist/unscope/log_unnormalized_pdf.hpp>
#include <boost/statistics/model/wrap/aggregate/prior_model_dataset.hpp>
#include <boost/statistics/model/functional/log_likelihood_evaluator.hpp>

namespace boost{
namespace statistics{
namespace model{

    // Augments algorithm::log_likehood with a prior of type D
    //
    // Requirements:
    // Let d denote an instance of D and p a parameter, then
    // log_unnormalized_pdf(p_dist,p) must return an object of type T
    template<typename T,typename D,typename M,typename Rx,typename Ry>
    class log_posterior_evaluator : log_likelihood_evaluator<T,M,Rx,Ry>{
        typedef log_likelihood_evaluator<T,M,Rx,Ry> super_;
        public:
        typedef prior_model_dataset_<D,M,Rx,Ry> prior_model_dataset_type;
        typedef typename prior_model_dataset_type::prior_type prior_type;
        typedef typename super_::result_type result_type;

        // Constructor
        log_posterior_evaluator();
        log_posterior_evaluator(const prior_model_dataset_type&);
        log_posterior_evaluator(const log_posterior_evaluator&);
        log_posterior_evaluator& operator=(const log_posterior_evaluator&);

        // Evaluate
        template<typename P>
        result_type operator()(const P& p)const;

        // Access
        const prior_model_dataset_type& prior_model_dataset()const;

        private:
        prior_model_dataset_type pmd_;
    };

    // Implementation //

    // Construction
    template<typename T,typename D,typename M,typename Rx,typename Ry>
    log_posterior_evaluator<T,D,M,Rx,Ry>::log_posterior_evaluator():
    super_(){}

    template<typename T,typename D,typename M,typename Rx,typename Ry>
    log_posterior_evaluator<T,D,M,Rx,Ry>::log_posterior_evaluator(
        const prior_model_dataset_type& pmd
    ):super_(pmd),pmd_(pmd){}

    template<typename T,typename D,typename M,typename Rx,typename Ry>
    log_posterior_evaluator<T,D,M,Rx,Ry>::log_posterior_evaluator(
        const log_posterior_evaluator& that
    ):super_(that),pmd_(that.pmd_){}

    template<typename T,typename D,typename M,typename Rx,typename Ry>
    log_posterior_evaluator<T,D,M,Rx,Ry>&
    log_posterior_evaluator<T,D,M,Rx,Ry>::operator=(
        const log_posterior_evaluator& that
    ){
        if(&that!=this){
            super_::operator=(that);
            pmd_ = (that.pmd_);
        }
        return (*this);
    }

    // Evaluate
    template<typename T,typename D,typename M,typename Rx,typename Ry>
    template<typename P>
    typename log_posterior_evaluator<T,D,M,Rx,Ry>::result_type
    log_posterior_evaluator<T,D,M,Rx,Ry>::operator()(const P& p)const{
        const super_& super = static_cast<const super_&>(*this);
        result_type res =  super(p);
        res += log_unnormalized_pdf(
            prior_model_dataset().prior(),
            p
        );
        return res;
    }

    // Access
    template<typename T,typename D,typename M,typename Rx,typename Ry>
    const
        typename
            log_posterior_evaluator<T,D,M,Rx,Ry>::prior_model_dataset_type&
    log_posterior_evaluator<T,D,M,Rx,Ry>::prior_model_dataset()const{
        return (this->pmd_);
    }

}// model
}// statistics
}// boost

#endif


/**
 * \file CachedSinusGeneratorFilter.cpp
 */

#include "CachedSinusGeneratorFilter.h"

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

#include <cmath>
#include <cstdint>
#include <cstring>

namespace ATK
{
  template<typename DataType_>
  CachedSinusGeneratorFilter<DataType_>::CachedSinusGeneratorFilter(int periods, int seconds)
  :Parent(0, 1), periods(periods), seconds(seconds)
  {
  }

  template<typename DataType_>
  void CachedSinusGeneratorFilter<DataType_>::set_frequency(int periods, int seconds)
  {
    if(periods <= 0)
    {
      throw std::out_of_range("Periods must be strictly positive");
    }
    this->periods = periods;
    this->seconds = seconds;
    setup();
  }

  template<typename DataType_>
  std::pair<int, int> CachedSinusGeneratorFilter<DataType_>::get_frequency() const
  {
    return std::make_pair(periods, seconds);
  }

  template<typename DataType_>
  void CachedSinusGeneratorFilter<DataType_>::set_volume(DataType_ volume)
  {
    this->volume = volume;
  }

  template<typename DataType_>
  DataType_ CachedSinusGeneratorFilter<DataType_>::get_volume() const
  {
    return volume;
  }

  template<typename DataType_>
  void CachedSinusGeneratorFilter<DataType_>::set_offset(DataType_ offset)
  {
    this->offset = offset;
  }

  template<typename DataType_>
  DataType_ CachedSinusGeneratorFilter<DataType_>::get_offset() const
  {
    return offset;
  }

  template<typename DataType_>
  void CachedSinusGeneratorFilter<DataType_>::setup()
  {
    indice = 0;
    cache.resize(output_sampling_rate * seconds);
    for(gsl::index i = 0; i < cache.size(); ++i)
    {
      cache[i] = static_cast<DataType>(std::sin(2 * boost::math::constants::pi<double>() * (i+1) * periods / seconds / output_sampling_rate));
    }
  }

  template<typename DataType_>
  void CachedSinusGeneratorFilter<DataType_>::process_impl(gsl::index size) const
  {
    DataType* ATK_RESTRICT output = outputs[0];
    gsl::index processed = 0;
    while (processed < size)
    {
      auto to_copy = std::min(size - processed, static_cast<gsl::index>(cache.size()) - indice);
      memcpy(reinterpret_cast<void*>(output + processed), reinterpret_cast<const void*>(cache.data() + indice), to_copy * sizeof(DataType_));
      indice += to_copy;
      processed += to_copy;
      if (indice >= cache.size())
      {
        indice = 0;
      }
    }
    for (gsl::index i = 0; i < size; ++i)
    {
      output[i] = static_cast<DataType>(offset + volume * output[i]);
    }
  }

#if ATK_ENABLE_INSTANTIATION
  template class CachedSinusGeneratorFilter<std::int16_t>;
  template class CachedSinusGeneratorFilter<std::int32_t>;
  template class CachedSinusGeneratorFilter<int64_t>;
  template class CachedSinusGeneratorFilter<float>;
#endif
  template class CachedSinusGeneratorFilter<double>;
}


// ---------------------------------------------------------------------
//
// Copyright (c) 2015 - 2022 by the IBAMR developers
// All rights reserved.
//
// This file is part of IBAMR.
//
// IBAMR is free software and is distributed under the 3-clause BSD
// license. The full text of the license can be found in the file
// COPYRIGHT at the top level directory of IBAMR.
//
// ---------------------------------------------------------------------

/////////////////////////////// INCLUDES /////////////////////////////////////

#include "ibamr/CIBStrategy.h"
#include "ibamr/DirectMobilitySolver.h"
#include "ibamr/StokesSpecifications.h"
#include "ibamr/ibamr_enums.h"
#include "ibamr/ibamr_utilities.h"

#include "ibtk/IBTK_MPI.h"
#include "ibtk/PETScSAMRAIVectorReal.h"
#include "ibtk/ibtk_utilities.h"

#include "CartesianGridGeometry.h"
#include "IntVector.h"
#include "PatchHierarchy.h"
#include "PatchLevel.h"
#include "SAMRAIVectorReal.h"
#include "tbox/Database.h"
#include "tbox/MathUtilities.h"
#include "tbox/PIO.h"
#include "tbox/Pointer.h"
#include "tbox/Timer.h"
#include "tbox/TimerManager.h"
#include "tbox/Utilities.h"

#include "petscmat.h"
#include "petscvec.h"
#include "petscviewer.h"
#include "petscviewertypes.h"

#include <Eigen/Cholesky>
#include <Eigen/Eigenvalues>
#include <Eigen/LU>

#include <algorithm>
#include <cmath>
#include <map>
#include <ostream>
#include <string>
#include <utility>
#include <vector>

#include "ibamr/app_namespaces.h" // IWYU pragma: keep

namespace IBAMR
{
/////////////////////////////// STATIC ///////////////////////////////////////

namespace
{
// Timers.
static Timer* t_solve_system;
static Timer* t_solve_body_system;
static Timer* t_initialize_solver_state;
static Timer* t_deallocate_solver_state;
} // namespace

////////////////////////////// PUBLIC ////////////////////////////////////////

DirectMobilitySolver::DirectMobilitySolver(std::string object_name,
                                           Pointer<Database> input_db,
                                           Pointer<CIBStrategy> cib_strategy)
    : d_object_name(std::move(object_name)), d_cib_strategy(cib_strategy)
{
    // Get from input
    if (input_db) getFromInput(input_db);

    IBAMR_DO_ONCE(t_solve_system = TimerManager::getManager()->getTimer("IBAMR::DirectMobilitySolver::solveSystem()");
                  t_solve_body_system =
                      TimerManager::getManager()->getTimer("IBAMR::DirectMobilitySolver::solveBodySystem()");
                  t_initialize_solver_state =
                      TimerManager::getManager()->getTimer("IBAMR::DirectMobilitySolver::initializeSolverState()");
                  t_deallocate_solver_state =
                      TimerManager::getManager()->getTimer("IBAMR::DirectMobilitySolver::deallocateSolverState()"););

    return;
} // DirectMobilitySolver

DirectMobilitySolver::~DirectMobilitySolver()
{
    for (const auto& petsc_mat_pair : d_petsc_mat_map)
    {
        const std::string& mat_name = petsc_mat_pair.first;
        Mat& mobility_mat = d_petsc_mat_map[mat_name].first;
        Mat& body_mobility_mat = d_petsc_mat_map[mat_name].second;
        MatDestroy(&mobility_mat);
        MatDestroy(&body_mobility_mat);
    }

    for (const auto& mat_pair : d_petsc_geometric_mat_map)
    {
        Mat& geometric_mat = d_petsc_geometric_mat_map[mat_pair.first];
        MatDestroy(&geometric_mat);
    }

    d_is_initialized = false;

    return;
} // ~DirectMobilitySolver

void
DirectMobilitySolver::registerMobilityMat(const std::string& mat_name,
                                          const unsigned prototype_struct_id,
                                          MobilityMatrixType mat_type,
                                          std::pair<MobilityMatrixInverseType, MobilityMatrixInverseType> inv_type,
                                          const int managing_proc,
                                          const std::string& filename,
                                          std::pair<double, double> scale)
{
    registerMobilityMat(mat_name,
                        std::vector<unsigned int>(1, prototype_struct_id),
                        mat_type,
                        inv_type,
                        managing_proc,
                        filename,
                        scale);

    return;
} // registerMobilityMat

void
DirectMobilitySolver::registerMobilityMat(const std::string& mat_name,
                                          const std::vector<unsigned>& prototype_struct_ids,
                                          MobilityMatrixType mat_type,
                                          std::pair<MobilityMatrixInverseType, MobilityMatrixInverseType> inv_type,
                                          const int managing_proc,
                                          const std::string& filename,
                                          std::pair<double, double> scale)
{
#if !defined(NDEBUG)
    TBOX_ASSERT(!mat_name.empty());
    for (const auto& prototype_struct_id : prototype_struct_ids)
    {
        TBOX_ASSERT(prototype_struct_id < d_cib_strategy->getNumberOfRigidStructures());
    }
    TBOX_ASSERT(d_mat_map.find(mat_name) == d_mat_map.end());
    TBOX_ASSERT(mat_type != UNKNOWN_MOBILITY_MATRIX_TYPE);
    TBOX_ASSERT(inv_type.first != UNKNOWN_MOBILITY_MATRIX_INVERSE_TYPE);
    TBOX_ASSERT(inv_type.second != UNKNOWN_MOBILITY_MATRIX_INVERSE_TYPE);
#endif

    unsigned int num_nodes = 0;
    for (const auto& prototype_struct_id : prototype_struct_ids)
    {
        num_nodes += d_cib_strategy->getNumberOfNodes(prototype_struct_id);
    }

    // Fill-in various maps.
    d_mat_prototype_id_map[mat_name] = prototype_struct_ids;
    d_mat_proc_map[mat_name] = managing_proc;
    d_mat_nodes_map[mat_name] = num_nodes;
    d_mat_parts_map[mat_name] = static_cast<unsigned>(prototype_struct_ids.size());
    d_mat_type_map[mat_name] = mat_type;
    d_mat_inv_type_map[mat_name] = inv_type;
    d_mat_filename_map[mat_name] = filename;
    d_mat_scale_map[mat_name] = scale;
    d_mat_map[mat_name] = { {}, {} };
    d_geometric_mat_map[mat_name] = {};
    d_ipiv_map[mat_name] = { {}, {} };
    d_petsc_mat_map[mat_name] = { nullptr, nullptr };
    d_petsc_geometric_mat_map[mat_name] = nullptr;

    // Allocate the actual matrices.
    const int mobility_mat_size = num_nodes * NDIM;
    const int body_mobility_mat_size = d_mat_parts_map[mat_name] * s_max_free_dofs;
    const int rank = IBTK_MPI::getRank();

    if (rank == managing_proc)
    {
        d_mat_map[mat_name].first.resize(mobility_mat_size * mobility_mat_size);
        MatCreateSeqDense(PETSC_COMM_SELF,
                          mobility_mat_size,
                          mobility_mat_size,
                          d_mat_map[mat_name].first.data(),
                          &d_petsc_mat_map[mat_name].first);

        d_mat_map[mat_name].second.resize(body_mobility_mat_size * body_mobility_mat_size);
        MatCreateSeqDense(PETSC_COMM_SELF,
                          body_mobility_mat_size,
                          body_mobility_mat_size,
                          d_mat_map[mat_name].second.data(),
                          &d_petsc_mat_map[mat_name].second);

        d_geometric_mat_map[mat_name].resize(mobility_mat_size * body_mobility_mat_size);
        MatCreateSeqDense(PETSC_COMM_SELF,
                          mobility_mat_size,
                          body_mobility_mat_size,
                          d_geometric_mat_map[mat_name].data(),
                          &d_petsc_geometric_mat_map[mat_name]);

        if (d_mat_inv_type_map[mat_name].first == LAPACK_LU)
        {
            d_ipiv_map[mat_name].first.resize(mobility_mat_size);
        }
        if (d_mat_inv_type_map[mat_name].second == LAPACK_LU)
        {
            d_ipiv_map[mat_name].second.resize(body_mobility_mat_size);
        }
    }

    return;
} // registerMobilityMat

void
DirectMobilitySolver::registerStructIDsWithMobilityMat(const std::string& mat_name,
                                                       const std::vector<std::vector<unsigned> >& struct_ids)
{
#if !defined(NDEBUG)
    TBOX_ASSERT(d_mat_map.find(mat_name) != d_mat_map.end());
    for (const auto& struct_id : struct_ids)
    {
        TBOX_ASSERT(struct_id.size() == d_mat_prototype_id_map[mat_name].size());
        unsigned num_nodes = 0;
        for (const auto& id : struct_id)
        {
            TBOX_ASSERT(id < d_cib_strategy->getNumberOfRigidStructures());

            num_nodes += d_cib_strategy->getNumberOfNodes(id);
        }
        TBOX_ASSERT(num_nodes == d_mat_nodes_map[mat_name]);
    }
#endif

    d_mat_actual_id_map[mat_name] = struct_ids;

    return;
} // registerStructIDsWithMobilityMat

void
DirectMobilitySolver::setStokesSpecifications(const StokesSpecifications& stokes_spec)
{
    d_rho = stokes_spec.getRho();
    d_mu = stokes_spec.getMu();

    return;
} // setStokesSpecifications

void
DirectMobilitySolver::setSolutionTime(const double solution_time)
{
    d_solution_time = solution_time;

    return;
} // setSolutionTime

void
DirectMobilitySolver::setTimeInterval(double current_time, double new_time)
{
    d_current_time = current_time;
    d_new_time = new_time;

    return;
} // setTimeInterval

bool
DirectMobilitySolver::solveSystem(Vec x, Vec b)
{
    IBAMR_TIMER_START(t_solve_system);

    // Initialize the solver, when necessary.
    const bool deallocate_after_solve = !d_is_initialized;
    if (deallocate_after_solve) initializeSolverState(x, b);

    const int rank = IBTK_MPI::getRank();
    static const int data_depth = NDIM;

    for (const auto& petsc_mat_pair : d_petsc_mat_map)
    {
        const std::string& mat_name = petsc_mat_pair.first;
        Mat& mat = d_petsc_mat_map[mat_name].first;
        const MobilityMatrixInverseType& inv_type = d_mat_inv_type_map[mat_name].first;
        const std::vector<std::vector<unsigned> >& struct_ids = d_mat_actual_id_map[mat_name];
        const int managing_proc = d_mat_proc_map[mat_name];
        const int mat_size = d_mat_nodes_map[mat_name] * data_depth;
        const int num_structs = static_cast<int>(struct_ids.size());

        for (int k = 0; k < num_structs; ++k)
        {
            std::vector<double> rhs;
            if (rank == managing_proc) rhs.resize(mat_size);
            d_cib_strategy->copyVecToArray(b, rhs.data(), struct_ids[k], data_depth, managing_proc);
            if (!d_recompute_mob_mat)
            {
                d_cib_strategy->rotateArray(rhs.data(),
                                            struct_ids[k],
                                            /*use_transpose*/ true,
                                            managing_proc,
                                            data_depth);
            }
            if (rank == managing_proc) computeSolution(mat, inv_type, d_ipiv_map[mat_name].first.data(), rhs.data());
            if (!d_recompute_mob_mat)
            {
                d_cib_strategy->rotateArray(rhs.data(),
                                            struct_ids[k],
                                            /*use_transpose*/ false,
                                            managing_proc,
                                            data_depth);
            }
            d_cib_strategy->copyArrayToVec(x, rhs.data(), struct_ids[k], data_depth, managing_proc);
        }
    }

    IBAMR_TIMER_STOP(t_solve_system);

    return true;
} // solveSystem

bool
DirectMobilitySolver::solveBodySystem(Vec x, Vec b)
{
    IBAMR_TIMER_START(t_solve_body_system);

    // Initialize the solver, when necessary.
    const bool deallocate_after_solve = !d_is_initialized;
    if (deallocate_after_solve) initializeSolverState(x, b);

    const int rank = IBTK_MPI::getRank();
    static const int data_depth = s_max_free_dofs;

    for (const auto& petsc_mat_pair : d_petsc_mat_map)
    {
        const std::string& mat_name = petsc_mat_pair.first;
        Mat& mat = d_petsc_mat_map[mat_name].second;
        const MobilityMatrixInverseType& inv_type = d_mat_inv_type_map[mat_name].second;
        const std::vector<std::vector<unsigned> >& struct_ids = d_mat_actual_id_map[mat_name];
        const int mat_size = d_mat_parts_map[mat_name] * data_depth;
        const int managing_proc = d_mat_proc_map[mat_name];
        const int num_structs = static_cast<int>(struct_ids.size());

        for (int k = 0; k < num_structs; ++k)
        {
            std::vector<double> rhs;
            if (rank == managing_proc) rhs.resize(mat_size);
            d_cib_strategy->copyFreeDOFsVecToArray(b, rhs.data(), struct_ids[k], managing_proc);
            if (!d_recompute_mob_mat)
            {
                d_cib_strategy->rotateArray(rhs.data(),
                                            struct_ids[k],
                                            /*use_transpose*/ true,
                                            managing_proc,
                                            data_depth);
            }
            if (rank == managing_proc) computeSolution(mat, inv_type, d_ipiv_map[mat_name].second.data(), rhs.data());
            if (!d_recompute_mob_mat)
            {
                d_cib_strategy->rotateArray(rhs.data(),
                                            struct_ids[k],
                                            /*use_transpose*/ false,
                                            managing_proc,
                                            data_depth);
            }
            d_cib_strategy->copyFreeDOFsArrayToVec(x, rhs.data(), struct_ids[k], managing_proc);
        }
    }

    IBAMR_TIMER_STOP(t_solve_body_system);

    return true;
} // solveBodySystem

void
DirectMobilitySolver::initializeSolverState(Vec x, Vec /*b*/)
{
    if (d_is_initialized) return;

    IBAMR_TIMER_START(t_initialize_solver_state);

    int rank = IBTK_MPI::getRank();
    auto managed_mats = static_cast<unsigned>(d_mat_map.size());

    static bool recreate_mobility_matrices = true;
    static std::vector<bool> read_files(managed_mats, false);
    bool initial_time = !d_recompute_mob_mat;

    if (recreate_mobility_matrices)
    {
        // Get grid-info
        Vec* vx;
        VecNestGetSubVecs(x, nullptr, &vx);
        Pointer<SAMRAIVectorReal<NDIM, double> > vx0;
        IBTK::PETScSAMRAIVectorReal::getSAMRAIVectorRead(vx[0], &vx0);
        Pointer<PatchHierarchy<NDIM> > patch_hierarchy = vx0->getPatchHierarchy();
        const int finest_ln = patch_hierarchy->getFinestLevelNumber();
        IBTK::PETScSAMRAIVectorReal::restoreSAMRAIVectorRead(vx[0], &vx0);
        Pointer<PatchLevel<NDIM> > struct_patch_level = patch_hierarchy->getPatchLevel(finest_ln);
        const IntVector<NDIM>& ratio = struct_patch_level->getRatio();
        Pointer<CartesianGridGeometry<NDIM> > grid_geom = patch_hierarchy->getGridGeometry();
        const double* dx0 = grid_geom->getDx();
        const double* X_upper = grid_geom->getXUpper();
        const double* X_lower = grid_geom->getXLower();
        double domain_extents[NDIM], dx[NDIM];
        for (int d = 0; d < NDIM; ++d)
        {
            dx[d] = dx0[d] / ratio(d);
            domain_extents[d] = X_upper[d] - X_lower[d];
        }

        int file_counter = 0;
        for (auto it = d_petsc_mat_map.begin(); it != d_petsc_mat_map.end(); ++it, ++file_counter)
        {
            const std::string& mat_name = it->first;
            Mat& mobility_mat = d_petsc_mat_map[mat_name].first;
            Mat& geometric_mat = d_petsc_geometric_mat_map[mat_name];
            const MobilityMatrixType& mat_type = d_mat_type_map[mat_name];
            const std::vector<unsigned>& struct_ids = d_mat_prototype_id_map[mat_name];
            const std::pair<double, double>& scale = d_mat_scale_map[mat_name];
            const int managing_proc = d_mat_proc_map[mat_name];

            if (mat_type == READ_FROM_FILE && !read_files[file_counter])
            {
                // Get the matrix from file.
                const std::string& filename = d_mat_filename_map[mat_name];
                if (rank == managing_proc)
                {
                    PetscViewer binary_viewer;
                    PetscViewerBinaryOpen(PETSC_COMM_SELF, filename.c_str(), FILE_MODE_READ, &binary_viewer);
                    MatLoad(mobility_mat, binary_viewer);
                    PetscViewerDestroy(&binary_viewer);
                }

                read_files[file_counter] = true;
            }
            else
            {
                d_cib_strategy->constructMobilityMatrix(mat_name,
                                                        mat_type,
                                                        mobility_mat,
                                                        struct_ids,
                                                        dx,
                                                        domain_extents,
                                                        initial_time,
                                                        d_rho,
                                                        d_mu,
                                                        scale,
                                                        d_f_periodic_corr,
                                                        managing_proc);
            }

            // Construct the geometric matrix that maps rigid body velocity to
            // nodal velocity.
            d_cib_strategy->constructGeometricMatrix(mat_name, geometric_mat, struct_ids, initial_time, managing_proc);
        }
        factorizeMobilityMatrix();
        constructBodyMobilityMatrix();
        factorizeBodyMobilityMatrix();
    }

    d_is_initialized = true;
    recreate_mobility_matrices = d_recompute_mob_mat;

    IBAMR_TIMER_STOP(t_initialize_solver_state);

    return;
} // initializeSolverState

void
DirectMobilitySolver::deallocateSolverState()
{
    d_is_initialized = false;

    return;
} // deallocateSolverState

const std::vector<unsigned>&
DirectMobilitySolver::getPrototypeStructIDs(const std::string& mat_name)
{
#if !defined(NDEBUG)
    TBOX_ASSERT(d_mat_prototype_id_map.find(mat_name) != d_mat_prototype_id_map.end());
#endif
    return d_mat_prototype_id_map[mat_name];

} // getPrototypeStructIDs

const std::vector<std::vector<unsigned> >&
DirectMobilitySolver::getStructIDs(const std::string& mat_name)
{
#if !defined(NDEBUG)
    TBOX_ASSERT(d_mat_actual_id_map.find(mat_name) != d_mat_actual_id_map.end());
#endif
    return d_mat_actual_id_map[mat_name];

} // getStructIDs

///////////////////////////// PRIVATE ////////////////////////////////////////

void
DirectMobilitySolver::getFromInput(Pointer<Database> input_db)
{
    Pointer<Database> comp_db;
    comp_db = input_db->isDatabase("LAPACK_SVD") ? input_db->getDatabase("LAPACK_SVD") : Pointer<Database>(nullptr);
    if (comp_db)
    {
        d_svd_replace_value = comp_db->getDouble("eigenvalue_replace_value");
        d_svd_eps = comp_db->getDouble("min_eigenvalue_threshold");
    }

    // Other parameters
    d_f_periodic_corr = input_db->getDoubleWithDefault("f_periodic_correction", d_f_periodic_corr);
    d_recompute_mob_mat = input_db->getBoolWithDefault("recompute_mob_mat_perstep", d_recompute_mob_mat);

    return;
} // getFromInput

void
DirectMobilitySolver::factorizeMobilityMatrix()
{
    int rank = IBTK_MPI::getRank();
    for (const auto& petsc_mat_pair : d_petsc_mat_map)
    {
        const std::string& mat_name = petsc_mat_pair.first;
        if (rank != d_mat_proc_map[mat_name]) continue;

        Mat& mat = d_petsc_mat_map[mat_name].first;
        const MobilityMatrixInverseType& inv_type = d_mat_inv_type_map[mat_name].first;
        const int mat_size = d_mat_nodes_map[mat_name] * NDIM;
        double* mat_data = nullptr;
        MatDenseGetArray(mat, &mat_data);
        factorizeDenseMatrix(mat_data, mat_size, inv_type, d_ipiv_map[mat_name].first.data(), mat_name, "Mobility");
        MatDenseRestoreArray(mat, &mat_data);
    }
    return;

} // factorizeMobilityMatrix

void
DirectMobilitySolver::constructBodyMobilityMatrix()
{
    int rank = IBTK_MPI::getRank();
    for (const auto& petsc_mat_pair : d_petsc_mat_map)
    {
        const std::string& mat_name = petsc_mat_pair.first;
        if (rank != d_mat_proc_map[mat_name]) continue;

        const int row_size = d_mat_nodes_map[mat_name] * NDIM;
        const int col_size = d_mat_parts_map[mat_name] * s_max_free_dofs;
        const MobilityMatrixInverseType& mobility_inv_type = d_mat_inv_type_map[mat_name].first;

        Mat& mobility_mat = d_petsc_mat_map[mat_name].first;
        Mat& body_mob_mat = d_petsc_mat_map[mat_name].second;
        Mat& geometric_mat = d_petsc_geometric_mat_map[mat_name];

        // Allocate a temporary matrix that holds the Matrix-Matrix product.
        // Here we are multiplying inverse of mobility matrix with geometric matrix.
        std::vector<double> product_mat_data(row_size * col_size);
        Mat product_mat;
        MatCreateSeqDense(PETSC_COMM_SELF, row_size, col_size, product_mat_data.data(), &product_mat);
        MatCopy(geometric_mat, product_mat, SAME_NONZERO_PATTERN);

        for (int col = 0; col < col_size; ++col)
        {
            double* col_data;
            MatDenseGetArray(product_mat, &col_data);
            computeSolution(
                mobility_mat, mobility_inv_type, d_ipiv_map[mat_name].first.data(), &col_data[col * row_size]);
            MatDenseRestoreArray(product_mat, &col_data);
        }
        MatTransposeMatMult(geometric_mat, product_mat, MAT_REUSE_MATRIX, PETSC_DEFAULT, &body_mob_mat);

        MatDestroy(&product_mat);
    }

    return;
} // generateBodyFrictionMatrix

void
DirectMobilitySolver::factorizeBodyMobilityMatrix()
{
    int rank = IBTK_MPI::getRank();
    for (const auto& petsc_mat_pair : d_petsc_mat_map)
    {
        const std::string& mat_name = petsc_mat_pair.first;
        if (rank != d_mat_proc_map[mat_name]) continue;

        Mat& mat = d_petsc_mat_map[mat_name].second;
        const MobilityMatrixInverseType& inv_type = d_mat_inv_type_map[mat_name].second;
        const int mat_size = d_mat_parts_map[mat_name] * s_max_free_dofs;

        double* mat_data = nullptr;
        MatDenseGetArray(mat, &mat_data);
        factorizeDenseMatrix(
            mat_data, mat_size, inv_type, d_ipiv_map[mat_name].second.data(), mat_name, "Body Mobility");
        MatDenseRestoreArray(mat, &mat_data);
    }
    return;

} // factorizeBodyMobilityMatrix

void
DirectMobilitySolver::factorizeDenseMatrix(double* mat_data,
                                           const int mat_size,
                                           const MobilityMatrixInverseType& inv_type,
                                           int* ipiv,
                                           const std::string& mat_name,
                                           const std::string& err_msg)
{
    using MatrixType = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>;
    // Older versions of Eigen don't make Eigen::Index publicly available
#if EIGEN_VERSION_AT_LEAST(3, 3, 0)
    using IndexType = Eigen::Index;
#else
    using IndexType = typename MatrixType::Index;
#endif
    Eigen::Map<MatrixType> mat_view(mat_data, IndexType(mat_size), IndexType(mat_size));

    if (inv_type != LAPACK_LU)
    {
        // For compatibility with the old code, we copy the lower triangle into
        // the upper triangle, even if they aren't actually equal
        for (int i = 0; i < mat_size; ++i)
        {
            for (int j = i + 1; j < mat_size; ++j)
            {
                mat_view(i, j) = mat_view(j, i);
            }
        }
    }

    if (inv_type == LAPACK_CHOLESKY)
    {
        Eigen::LLT<MatrixType> cholesky_factorization(mat_view);
        mat_view = cholesky_factorization.matrixL();
    }
    else if (inv_type == LAPACK_LU)
    {
        Eigen::PartialPivLU<MatrixType> plu_factorization(mat_view);
        mat_view = plu_factorization.matrixLU();
        const auto& indices = plu_factorization.permutationP().indices();
        TBOX_ASSERT(indices.rows() * indices.cols() == mat_size);
        std::copy_n(indices.data(), mat_size, ipiv);
    }
    else if (inv_type == LAPACK_SVD)
    {
        // Use the symmetric eigenvalue decomposition as a stand-in for the SVD.
        // In particular, since A = V D V^T, where V is the matrix of eigenvectors
        // and D is the matrix of eigenvalues, we can factorize A as
        //
        //   A = V sqrt(D) sqrt(D) V^T
        //     = V sqrt(D) (V sqrt(D))^T
        //
        // and instead store A <- V sqrt(D)
        Eigen::SelfAdjointEigenSolver<MatrixType> eigensolver(mat_view);
        Eigen::Matrix<double, Eigen::Dynamic, 1> eigenvalues = eigensolver.eigenvalues();
        const MatrixType eigenvectors = eigensolver.eigenvectors();
        // Make negative eigenvalues to be equal to min eigen value from
        // input option
        int counter = 0, counter_zero = 0;
        for (int i = 0; i < mat_size; ++i)
        {
            if (eigenvalues[i] < d_svd_eps)
            {
                eigenvalues[i] = d_svd_replace_value;
                counter++;
            }
        }
        for (int i = 0; i < mat_size; ++i)
        {
            if (IBTK::abs_equal_eps(eigenvalues[i], 0.0))
            {
                counter_zero++;
            }

            for (int j = 0; j < mat_size; ++j)
            {
                if (IBTK::abs_equal_eps(eigenvalues[j], 0.0))
                {
                    mat_view(i, j) = 0.0;
                }
                else
                {
                    mat_view(i, j) = eigenvectors(i, j) / std::sqrt(eigenvalues[j]);
                }
            }
        }

        plog << "DirectMobilityMatrix::factorizeDenseMatrix(): For " << err_msg << " matrix: " << counter
             << " eigenvalues for dense matrix with handle " << mat_name
             << " have been changed. Number of zero eigenvalues placed are " << counter_zero << std::endl;
    }
    else
    {
        TBOX_ERROR("DirectMobilityMatrix::factorizeDenseMatrix(): Unsupported dense "
                   << "matrix inversion method called for " << err_msg << std::endl);
    }

    return;
} // factorizeDenseMatrix

void
DirectMobilitySolver::computeSolution(Mat& mat, const MobilityMatrixInverseType& inv_type, int* ipiv, double* rhs)
{
    // Get pointer to matrix.
    int mat_size = 0;
    double* mat_data = nullptr;
    MatGetSize(mat, &mat_size, nullptr);
    MatDenseGetArray(mat, &mat_data);
    using MatrixType = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>;
    // Older versions of Eigen don't make Eigen::Index publicly available
#if EIGEN_VERSION_AT_LEAST(3, 3, 0)
    using IndexType = Eigen::Index;
#else
    using IndexType = typename MatrixType::Index;
#endif
    Eigen::Map<MatrixType> mat_view(mat_data, IndexType(mat_size), IndexType(mat_size));
    using VectorType = Eigen::Matrix<double, Eigen::Dynamic, 1>;
    Eigen::Map<VectorType> rhs_view(rhs, IndexType(mat_size), IndexType(1));

    if (inv_type == LAPACK_CHOLESKY)
    {
        rhs_view = mat_view.triangularView<Eigen::Lower>().solve(rhs_view);
        rhs_view = mat_view.triangularView<Eigen::Lower>().transpose().solve(rhs_view);
    }
    else if (inv_type == LAPACK_LU)
    {
        // A    = P L U
        // A^-1 = U^-1 L^-1 P^-1
        using IndicesType = Eigen::Matrix<int, Eigen::Dynamic, 1>;
        Eigen::Map<IndicesType> indices(ipiv, mat_size);
        PermutationWrapper<decltype(indices)> permutation(indices);
        rhs_view = permutation.transpose() * rhs_view;
        rhs_view = mat_view.triangularView<Eigen::UnitLower>().solve(rhs_view);
        rhs_view = mat_view.triangularView<Eigen::Upper>().solve(rhs_view);
    }
    else if (inv_type == LAPACK_SVD)
    {
        rhs_view = mat_view * mat_view.transpose() * rhs_view;
    }
    else
    {
        TBOX_ERROR("DirectMobilitySolver::computeSolution(). Inverse method not supported." << std::endl);
    }

    MatDenseRestoreArray(mat, &mat_data);

    return;
} // computeSolution

//////////////////////////////////////////////////////////////////////////////

} // namespace IBAMR


// Copyright (c) 2018 by University Paris-Est Marne-la-Vallee
// GeometricExplicit.hpp
// This file is part of the Garamon for e3ga.
// Authors: Stephane Breuils and Vincent Nozick
// Conctat: vincent.nozick@u-pem.fr
//
// Licence MIT
// A a copy of the MIT License is given along with this program

/// \file GeometricExplicit.hpp
/// \author Stephane Breuils, Vincent Nozick
/// \brief Explicit precomputed per grades geometric products of e3ga.

#ifndef E3GA_GEOMETRIC_PRODUCT_EXPLICIT_HPP__
#define E3GA_GEOMETRIC_PRODUCT_EXPLICIT_HPP__
#pragma once

#include <Eigen/Core>

#include "e3ga/Mvec.hpp"
#include "e3ga/Constants.hpp"

/*!
 * @namespace e3ga
 */
namespace e3ga {
    template<typename T> class Mvec;

    /// \brief Compute the geometric product between two homogeneous multivectors mv1 (grade 2) and mv2 (grade 2).
	/// \tparam the type of value that we manipulate, either float or double or something else.
	/// \param mv1 - the first homogeneous multivector of grade 2 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 2 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1 mv2 whose grade is 2
	template<typename T>
	void geometric_2_2_2(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3.coeffRef(0) += -mv1.coeff(1)*mv2.coeff(2) + mv1.coeff(2)*mv2.coeff(1);
		mv3.coeffRef(1) +=  mv1.coeff(0)*mv2.coeff(2) - mv1.coeff(2)*mv2.coeff(0);
		mv3.coeffRef(2) += -mv1.coeff(0)*mv2.coeff(1) + mv1.coeff(1)*mv2.coeff(0);
	}

    template<typename T>
	std::array<std::array<std::array<std::function<void(const Eigen::Matrix<T, Eigen::Dynamic, 1> & , const Eigen::Matrix<T, Eigen::Dynamic, 1> & , Eigen::Matrix<T, Eigen::Dynamic, 1>&)>, 4>, 4>, 4> geometricFunctionsContainer =
	{{
		{{
			{{{},{},{},{}}},
			{{{},{},{},{}}},
			{{{},{},{},{}}},
			{{{},{},{},{}}}
		}},
		{{
			{{{},{},{},{}}},
			{{{},{},{},{}}},
			{{{},{},{},{}}},
			{{{},{},{},{}}}
		}},
		{{
			{{{},{},{},{}}},
			{{{},{},{},{}}},
			{{{},{},geometric_2_2_2<T>,{}}},
			{{{},{},{},{}}}
		}},
		{{
			{{{},{},{},{}}},
			{{{},{},{},{}}},
			{{{},{},{},{}}},
			{{{},{},{},{}}}
		}}
	}};

}/// End of Namespace

#endif // E3GA_GEOMETRIC_PRODUCT_EXPLICIT_HPP__


///////////////////////////////////////////////////////////////////////////////
//  Copyright 2014 Anton Bikineev
//  Copyright 2014 Christopher Kormanyos
//  Copyright 2014 John Maddock
//  Copyright 2014 Paul Bristow
//  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)
//
#ifndef BOOST_MATH_HYPERGEOMETRIC_0F1_BESSEL_HPP
#define BOOST_MATH_HYPERGEOMETRIC_0F1_BESSEL_HPP

#include <boost/math/special_functions/bessel.hpp>
#include <boost/math/special_functions/gamma.hpp>

  namespace boost { namespace math { namespace detail {

  template <class T, class Policy>
  inline T hypergeometric_0F1_bessel(const T& b, const T& z, const Policy& pol)
  {
    BOOST_MATH_STD_USING

    const bool is_z_nonpositive = z <= 0;

    const T sqrt_z = is_z_nonpositive ? T(sqrt(-z)) : T(sqrt(z));
    const T bessel_mult = is_z_nonpositive ?
      boost::math::cyl_bessel_j(b - 1, 2 * sqrt_z, pol) :
      boost::math::cyl_bessel_i(b - 1, 2 * sqrt_z, pol) ;

    if (b > boost::math::max_factorial<T>::value)
    {
       const T lsqrt_z = log(sqrt_z);
       const T lsqrt_z_pow_b = (b - 1) * lsqrt_z;
       T lg = (boost::math::lgamma(b, pol) - lsqrt_z_pow_b);
       lg = exp(lg);
       return lg * bessel_mult;
    }
    else
    {
       const T sqrt_z_pow_b = pow(sqrt_z, b - 1);
       return (boost::math::tgamma(b, pol) / sqrt_z_pow_b) * bessel_mult;
    }
  }

  } } } // namespaces

#endif // BOOST_MATH_HYPERGEOMETRIC_0F1_BESSEL_HPP


#include <iostream>
#include <stdio.h>

#include <Eigen/Dense>

#include <boost/array.hpp>
#include <boost/numeric/odeint.hpp>

using namespace boost::numeric;

using std::cout;
using std::endl;

#include "MotionModels.h"
#include "EKF.h"

namespace syllo {

     EKF::EKF()
     {
     }

     EKF::EKF(const Eigen::MatrixXf &F,
	      const Eigen::MatrixXf &B,
	      const Eigen::MatrixXf &H,
	      const Eigen::MatrixXf &Q,
	      const Eigen::MatrixXf &R,
	      double dt)
     {
	  setModel(F, B, H, Q, R, dt);
     }

     int EKF::setModel(const Eigen::MatrixXf &F,
		       const Eigen::MatrixXf &B,
		       const Eigen::MatrixXf &H,
		       const Eigen::MatrixXf &Q,
		       const Eigen::MatrixXf &R,
		       double dt)
     {
	  F_ = F;
	  B_ = B;
	  H_ = H;
          Q_ = Q;
	  R_ = R;
	  eye_ = Eigen::MatrixXf::Identity(F.rows(), F.cols());
	  dt_ = dt;

	  return 0;
     }

     int EKF::init(const Eigen::MatrixXf &x0,
		   const Eigen::MatrixXf &P0)
     {
	  x_ = x0;
	  P_ = P0;
	  return 0;
     }

     int EKF::predict(const Eigen::MatrixXf &u)
     {
          x_ = F_*x_ + B_*u;
          P_ = F_*P_*F_.transpose() + Q_;

	  //runge_kutta4< syllo::state_3d_type > stepper;
	  //syllo::state_3d_type x = {x_(0,0), x_(1,0), x_(2,0)};
	  //stepper.do_step(cart_model, x , 0 , dt_ );
          //
	  //x_ << x[0], x[1], x[2];
          //
	  //////x_ = F_*x_ + B_*u;
	  ////Eigen::MatrixXf xdot;
	  ////xdot.resize(3,1);
	  ////
	  ////xdot << u(0,0)   * cos(x_(2,0)),
	  ////        u(0,0)   * sin(x_(2,0)),
   	  ////        u(0,0)/3 * tan(u(1,0));
	  ////
	  ////x_ =  x_ + xdot;
	  //
	  //// compute jacobian...
	  //Eigen::MatrixXf J;
	  //J.resize(3,3);
	  //J << 0, 0, -u(0,0)*sin(x_(2,0)),
	  //     0, 0, u(0,0)*cos(x_(2,0)),
	  //     0, 0, 0;
          //
	  ////J << 0, 0, 0,
	  ////     0, 0, 0,
	  ////     0, 0, 0;
	  //
	  ////P_ = F_*P_*F_.transpose() + Q_;
	  //P_ = J*P_*J.transpose() + Q_;
	  return 0;
     }

     int EKF::update(const Eigen::MatrixXf &z)
     {
	  K_ = P_*H_.transpose()*(H_*P_*H_.transpose() + R_).inverse();
	  x_ = x_ + K_*(z - H_*x_);
	  P_ = (eye_ - K_*H_)*P_;
	  return 0;
     }

     Eigen::MatrixXf EKF::state()
     {
	  return x_;
     }

     Eigen::MatrixXf EKF::covariance()
     {
	  return P_;
     }
}


#include <iostream>
#include <fstream>
#include <complex>
#include <string>
#include <boost/multi_array.hpp>

using std::string;
using std::complex;
using std::cout;
using std::endl;

typedef boost::multi_array<double,2> rarray;
typedef boost::multi_array<complex<double>,3> harray;
//using rarray = boost::multi_array<double,2>; // for C++11 or later
//using harray = boost::multi_array<complex<double>,3>; //for C++11 or later

int input_hamiltonian(string &fname, rarray &rvec, harray &hop, const int &flag){
  const int no=hop.size(), nr=rvec.size();
  string fname_tmp,str;
  switch(flag){
  case 1:{
    fname_tmp=fname;
    break;
  }
  case 2:{
    fname_tmp=fname+"/irvec.txt";
    break;
  }
  case 3:{
    fname_tmp=fname+"_hr.dat";
    break;
  }
  case 4:{
    fname_tmp="Hopping.dat";
  }
  }
  std::ifstream rfile(fname_tmp.c_str(),std::ios::in);
  if(rfile.fail()){
    cout<<"fail input file"<<endl;
    return -1;
  }

  switch(flag){
  case 1:{
    for(int i=0; i<no; i++){
      for(int j=0; j<no;j++){
	for(int k=0;k<nr;k++){
	  getline(rfile,str);
	  sscanf(str.c_str(),"%lf %lf %lf %lf %lf",
		      &rvec[k][0], &rvec[k][1], &rvec[k][2],
		      &hop[j][i][k].real(), &hop[j][i][k].imag());
	}
      }
    }
    break;
  }
  case 2:{
    std::ifstream hfile((fname+"/ham_r.txt").c_str(),std::ios::in);
    if(hfile.fail()){
      cout<<"fail input file"<<endl;
      return -1;
    }
    for(int i=0; i<nr; i++){
      getline(rfile,str);
      sscanf(str.c_str(),"%lf %lf %lf",&rvec[i][0], &rvec[i][1], &rvec[i][2]);
      for(int j=0; j<no;j++){
	for(int k=0;k<no;k++){
          getline(hfile,str);
          sscanf(str.c_str(),"(%lf, %lf)",&hop[k][j][i].real(), &hop[k][j][i].imag());
	}
      }
    }
    break;
  }
  case 3:{
    break;
  }
  case 4:{
    int l,m;
    double tmp[3];
    for(int i=0;i<7+no;i++){
      getline(rfile,str);
    }
    for(int i=0;i<nr;i++){
      for(int j=0;j<no;j++){
	for(int k=0;k<no;k++){
	  getline(rfile,str);
	  sscanf(str.c_str(),"%lf %lf %lf %lf %lf %lf %ld %ld %lf %lf",
		 &rvec[i][0], &rvec[i][1], &rvec[i][2],
		 &tmp[0], &tmp[1], &tmp[2], &l, &m,
		 &hop[k][j][i].real(), &hop[k][j][i].imag());
	}
      }
    }
    break;
  }
  }
  return 0;
}

int main(){
  static const int no=1,nr=4,flag=1;

  int err;
  string fname="ham.dat";
  rarray rvec(boost::extents[nr][3]);
  harray hop(boost::extents[no][no][nr]);

  err=input_hamiltonian(fname,rvec,hop,flag);
  for(int i=0; i<nr; i++){
    for(int j=0;j<3; j++){
      cout<<rvec[i][j]<<" ";
    }
    for(int j=0; j<no;j++){
      for(int k=0;k<no;k++){
	cout<<hop[k][j][i];
      }
    }
    cout<<endl;
  }
  return 0;
}


// Copyright (c) 2019 Franka Emika GmbH
// Use of this source code is governed by the Apache-2.0 license, see LICENSE

#include <Eigen/Dense>
#include <Eigen/Geometry>

#include <franka/lowpass_filter.h>

namespace franka {

double lowpassFilter(double sample_time, double y, double y_last, double cutoff_frequency) {
  if (sample_time < 0 || !std::isfinite(sample_time)) {
    throw std::invalid_argument("lowpass-filter: sample_time is negative, infinite or NaN.");
  }
  if (cutoff_frequency <= 0 || !std::isfinite(cutoff_frequency)) {
    throw std::invalid_argument(
        "lowpass-filter: cutoff_frequency is zero, negative, infinite or NaN.");
  }
  if (!std::isfinite(y) || !std::isfinite(y_last)) {
    throw std::invalid_argument(
        "lowpass-filter: current or past input value of the signal to be filtered is infinite or "
        "NaN.");
  }
  double gain = sample_time / (sample_time + (1.0 / (2.0 * M_PI * cutoff_frequency)));
  return gain * y + (1 - gain) * y_last;
}

std::array<double, 16> cartesianLowpassFilter(double sample_time,
                                              std::array<double, 16> y,
                                              std::array<double, 16> y_last,
                                              double cutoff_frequency) {
  if (sample_time < 0 || !std::isfinite(sample_time)) {
    throw std::invalid_argument(
        "Cartesian lowpass-filter: sample_time is negative, infinite or NaN.");
  }
  if (cutoff_frequency <= 0 || !std::isfinite(cutoff_frequency)) {
    throw std::invalid_argument(
        "Cartesian lowpass-filter: cutoff_frequency is zero, negative, infinite or NaN.");
  }
  for (size_t i = 0; i < y.size(); i++) {
    if (!std::isfinite(y[i]) || !std::isfinite(y_last[i])) {
      throw std::invalid_argument(
          "Cartesian lowpass-filter: current or past input value of the signal to be filtered is "
          "infinite or NaN.");
    }
  }
  Eigen::Affine3d transform(Eigen::Matrix4d::Map(y.data()));
  Eigen::Affine3d transform_last(Eigen::Matrix4d::Map(y_last.data()));
  Eigen::Quaterniond orientation(transform.linear());
  Eigen::Quaterniond orientation_last(transform_last.linear());

  double gain = sample_time / (sample_time + (1.0 / (2.0 * M_PI * cutoff_frequency)));
  transform.translation() =
      gain * transform.translation() + (1.0 - gain) * transform_last.translation();
  orientation = orientation_last.slerp(gain, orientation);

  transform.linear() << orientation.normalized().toRotationMatrix();
  std::array<double, 16> filtered_values{};
  Eigen::Map<Eigen::Matrix4d>(&filtered_values[0], 4, 4) = transform.matrix();

  return filtered_values;
}
}  // namespace franka


#include <iostream>
#include <fstream>
#include <iomanip>
#include <cmath>
#include <memory>
#include <boost/math/tools/roots.hpp>

#include "Config.hpp"
#include "MirrorPlasma.hpp"
#include "BatchRunner.hpp"

extern "C" {
#include <fenv.h>
}

int main( int argc, char** argv )
{
	std::string fname( "Mirror.conf" );
	if ( argc == 2 )
		fname = argv[ 1 ];
	if ( argc > 2 )
	{
		std::cerr << "Usage: MCTrans++ ConfigFile.conf" << std::endl;
		return 1;
	}

#if defined(DEBUG) && defined(FPEXCEPT)
	std::cerr << "Floating Point Exceptions Enabled" << std::endl;
	::feenableexcept( FE_INVALID | FE_DIVBYZERO | FE_OVERFLOW );
#endif

	BatchRunner runner(fname);
	runner.runBatchSolve();
	return 0;

}

std::shared_ptr<MirrorPlasma> MCTransConfig::Solve()
{
	switch ( Type ) {
		case SolveType::SteadyStateMachSolve:
			doMachSolve( *ReferencePlasmaState );
			break;
		case SolveType::SteadyStateTempSolve:
			doTempSolve( *ReferencePlasmaState );
			break;
		default:
			throw std::invalid_argument( "Unknown Solve Type" );
	}

	return std::move( ReferencePlasmaState );
}

void MCTransConfig::doMachSolve( MirrorPlasma& plasma ) const
{
	// NB This uses power densities in W/m^3
	auto PowerBalance = [ &plasma ]( double M ) {
		plasma.MachNumber = M;
		plasma.ComputeSteadyStateNeutrals();

		double HeatLoss = plasma.IonHeatLosses() + plasma.ElectronHeatLosses();
		double Heating = plasma.IonHeating() + plasma.ElectronHeating();

		return Heating - HeatLoss;
	};

	boost::uintmax_t iters = 1000;
	boost::math::tools::eps_tolerance<double> tol( 11 ); // only bother getting part in 1024 accuracy
	double InitialMach = plasma.initialMach(); // Usually M > 4 for these solutions
	double Factor = 1.25;
	bool rising = true; // Confinement gets uniformly better for increasing M, and Viscous heating increases with M
	auto M_bounds = boost::math::tools::bracket_and_solve_root( PowerBalance, InitialMach, Factor, rising, tol, iters );
	auto M_lower = M_bounds.first, M_upper = M_bounds.second;
	plasma.MachNumber = ( M_lower + M_upper )/2.0;
	plasma.ComputeSteadyStateNeutrals();
}