fwd_bem_model.cpp

Go to the documentation of this file.

//=============================================================================================================
 
//=============================================================================================================
// INCLUDES
//=============================================================================================================
 
#include "fwd_bem_model.h"
#include "fwd_bem_solution.h"
#include "fwd_eeg_sphere_model.h"
#include <mne/mne_surface.h>
#include <mne/mne_triangle.h>
#include <mne/mne_source_space.h>
 
#include "fwd_comp_data.h"
 
#include <memory>
 
#include "fwd_thread_arg.h"
 
#include <fiff/fiff_stream.h>
#include <fiff/fiff_named_matrix.h>
 
#include <QFile>
#include <QList>
#include <QThread>
#include <QtConcurrent>
#include <QDebug>
 
// MSVC builds already define _USE_MATH_DEFINES globally (see src/CMakeLists.txt),
// so define it here only for the toolchains that do not.
#ifndef _USE_MATH_DEFINES
#define _USE_MATH_DEFINES
#endif
#include <math.h>
 
#include <Eigen/Dense>
 
static const Eigen::Vector3f Qx(1.0f, 0.0f, 0.0f);
static const Eigen::Vector3f Qy(0.0f, 1.0f, 0.0f);
static const Eigen::Vector3f Qz(0.0f, 0.0f, 1.0f);
 
//=============================================================================================================
// Local constants
//=============================================================================================================
 
namespace
{
constexpr int X = 0;
constexpr int Z = 2;
constexpr int FAIL = -1;
constexpr int OK = 0;
constexpr int LOADED = 1;    // fwd_bem_load_solution: successfully loaded
constexpr int NOT_FOUND = 0; // fwd_bem_load_solution: solution not available
 
// BEM file suffixes (kept for documentation).
[[maybe_unused]] constexpr auto BEM_SUFFIX = "-bem.fif";
constexpr auto BEM_SOL_SUFFIX = "-bem-sol.fif";
constexpr float EPS = 1e-5f; // Points closer to origin than this are considered at the origin
constexpr float CEPS = 1e-5f;
}
 
namespace FWDLIB
{
 
struct SurfExpl
{
    int kind;
    const QString name;
};
 
struct MethodExpl
{
    int method;
    const QString name;
};
 
} // namespace FWDLIB
 
static FWDLIB::SurfExpl surf_expl[] = {{FIFFV_BEM_SURF_ID_BRAIN, "inner skull"},
                                       {FIFFV_BEM_SURF_ID_SKULL, "outer skull"},
                                       {FIFFV_BEM_SURF_ID_HEAD, "scalp"},
                                       {-1, "unknown"}};
 
static FWDLIB::MethodExpl method_expl[] = {{FWDLIB::FWD_BEM_CONSTANT_COLL, "constant collocation"},
                                           {FWDLIB::FWD_BEM_LINEAR_COLL, "linear collocation"},
                                           {-1, "unknown"}};
 
static QString strip_from(const QString& s, const QString& suffix)
{
    QString res;
 
    if (s.endsWith(suffix)) {
        res = s;
        res.chop(suffix.size());
    } else
        res = s;
 
    return res;
}
 
 
namespace FWDLIB
{

struct FrameNameRec
{
    int frame;
    const QString name;
};
 
}
 
const QString mne_coord_frame_name_40(int frame)
 
{
    static FWDLIB::FrameNameRec frames[] = {
        {FIFFV_COORD_UNKNOWN, "unknown"},
        {FIFFV_COORD_DEVICE, "MEG device"},
        {FIFFV_COORD_ISOTRAK, "isotrak"},
        {FIFFV_COORD_HPI, "hpi"},
        {FIFFV_COORD_HEAD, "head"},
        {FIFFV_COORD_MRI, "MRI (surface RAS)"},
        {FIFFV_MNE_COORD_MRI_VOXEL, "MRI voxel"},
        {FIFFV_COORD_MRI_SLICE, "MRI slice"},
        {FIFFV_COORD_MRI_DISPLAY, "MRI display"},
        {FIFFV_MNE_COORD_CTF_DEVICE, "CTF MEG device"},
        {FIFFV_MNE_COORD_CTF_HEAD, "CTF/4D/KIT head"},
        {FIFFV_MNE_COORD_RAS, "RAS (non-zero origin)"},
        {FIFFV_MNE_COORD_MNI_TAL, "MNI Talairach"},
        {FIFFV_MNE_COORD_FS_TAL_GTZ, "Talairach (MNI z > 0)"},
        {FIFFV_MNE_COORD_FS_TAL_LTZ, "Talairach (MNI z < 0)"},
        {-1, "unknown"}};
    int k;
    for (k = 0; frames[k].frame != -1; k++) {
        if (frame == frames[k].frame)
            return frames[k].name;
    }
    return frames[k].name;
}
 
//=============================================================================================================
// USED NAMESPACES
//=============================================================================================================
 
using namespace Eigen;
using namespace FIFFLIB;
using namespace MNELIB;
using namespace FWDLIB;
 
//=============================================================================================================
// DEFINE MEMBER METHODS
//=============================================================================================================
 
FwdBemModel::FwdBemModel()
: nsurf(0)
, bem_method(FWD_BEM_UNKNOWN)
, nsol(0)
, head_mri_t()
, ip_approach_limit(FWD_BEM_IP_APPROACH_LIMIT)
, use_ip_approach(false)
{
}
 
//=============================================================================================================
 
FwdBemModel::~FwdBemModel()
{
    // surfs cleaned up automatically via shared_ptr
    this->fwd_bem_free_solution();
}
 
//=============================================================================================================
 
void FwdBemModel::fwd_bem_free_solution()
{
    this->solution.resize(0, 0);
    this->sol_name.clear();
    this->v0.resize(0);
    this->bem_method = FWD_BEM_UNKNOWN;
    this->nsol = 0;
}
 
//=============================================================================================================
 
QString FwdBemModel::fwd_bem_make_bem_sol_name(const QString& name)
/*
     * Make a standard BEM solution file name
     */
{
    QString s1, s2;
 
    s1 = strip_from(name, ".fif");
    s2 = strip_from(s1, "-sol");
    s1 = strip_from(s2, "-bem");
    s2 = QString("%1%2").arg(s1).arg(BEM_SOL_SUFFIX);
    return s2;
}
 
//=============================================================================================================
 
const QString& FwdBemModel::fwd_bem_explain_surface(int kind)
{
    int k;
 
    for (k = 0; surf_expl[k].kind >= 0; k++)
        if (surf_expl[k].kind == kind)
            return surf_expl[k].name;
 
    return surf_expl[k].name;
}
 
//=============================================================================================================
 
const QString& FwdBemModel::fwd_bem_explain_method(int method)
 
{
    int k;
 
    for (k = 0; method_expl[k].method >= 0; k++)
        if (method_expl[k].method == method)
            return method_expl[k].name;
 
    return method_expl[k].name;
}
 
//=============================================================================================================
 
int FwdBemModel::get_int(FiffStream::SPtr& stream, const FiffDirNode::SPtr& node, int what, int* res)
/*
     * Wrapper to get int's
     */
{
    FiffTag::UPtr t_pTag;
    if (node->find_tag(stream, what, t_pTag)) {
        if (t_pTag->getType() != FIFFT_INT) {
            qWarning("Expected an integer tag : %d (found data type %d instead)", what, t_pTag->getType());
            return FAIL;
        }
        *res = *t_pTag->toInt();
        return OK;
    }
    return FAIL;
}
 
//=============================================================================================================
 
MNESurface* FwdBemModel::fwd_bem_find_surface(int kind)
{
    for (int k = 0; k < this->nsurf; k++)
        if (this->surfs[k]->id == kind)
            return this->surfs[k].get();
    qWarning("Desired surface (%d = %s) not found.", kind, fwd_bem_explain_surface(kind).toUtf8().constData());
    return nullptr;
}
 
//=============================================================================================================
 
FwdBemModel::UPtr FwdBemModel::fwd_bem_load_surfaces(const QString& name, const std::vector<int>& kinds)
/*
 * Load a set of surfaces
 */
{
    std::vector<std::shared_ptr<MNESurface>> surfs;
    const int nkind = static_cast<int>(kinds.size());
    Eigen::VectorXf sigma_tmp(nkind);
    int j, k;
 
    if (nkind <= 0) {
        qCritical("No surfaces specified to fwd_bem_load_surfaces");
        return nullptr;
    }
 
    for (k = 0; k < nkind; k++) {
        float cond = -1.0f;
        auto s = MNESurface::read_bem_surface(name, kinds[k], true, cond);
        if (!s)
            return nullptr;
        if (cond < 0.0) {
            qCritical("No conductivity available for surface %s", fwd_bem_explain_surface(kinds[k]).toUtf8().constData());
            return nullptr;
        }
        if (s->coord_frame != FIFFV_COORD_MRI) {
            qCritical("FsSurface %s not specified in MRI coordinates.", fwd_bem_explain_surface(kinds[k]).toUtf8().constData());
            return nullptr;
        }
        sigma_tmp[k] = cond;
        surfs.push_back(std::move(s));
    }
    auto m = std::make_unique<FwdBemModel>();
 
    m->surf_name = name;
    m->nsurf = nkind;
    m->surfs = std::move(surfs);
    m->sigma = sigma_tmp;
    m->ntri.resize(nkind);
    m->np.resize(nkind);
    m->gamma.resize(nkind, nkind);
    m->source_mult.resize(nkind);
    m->field_mult.resize(nkind);
    /*
     * Build a shifted conductivity array with sigma[-1] = 0 (outside)
     */
    Eigen::VectorXf sigma1(nkind + 1);
    sigma1[0] = 0.0f;
    sigma1.tail(nkind) = m->sigma;
    // sigma[j] below refers to sigma1[j+1], sigma[j-1] to sigma1[j]
    /*
     * Gamma factors and multipliers
     */
    for (j = 0; j < m->nsurf; j++) {
        m->ntri[j] = m->surfs[j]->ntri;
        m->np[j] = m->surfs[j]->np;
        m->source_mult[j] = 2.0f / (sigma1[j + 1] + sigma1[j]);
        m->field_mult[j] = sigma1[j + 1] - sigma1[j];
        for (k = 0; k < m->nsurf; k++)
            m->gamma(j, k) = (sigma1[k + 1] - sigma1[k]) / (sigma1[j + 1] + sigma1[j]);
    }
 
    return m;
}
 
//=============================================================================================================
 
FwdBemModel::UPtr FwdBemModel::fwd_bem_load_homog_surface(const QString& name)
/*
 * Load surfaces for the homogeneous model
 */
{
    return fwd_bem_load_surfaces(name, {FIFFV_BEM_SURF_ID_BRAIN});
}
 
//=============================================================================================================
 
FwdBemModel::UPtr FwdBemModel::fwd_bem_load_three_layer_surfaces(const QString& name)
/*
 * Load surfaces for three-layer model
 */
{
    return fwd_bem_load_surfaces(name, {FIFFV_BEM_SURF_ID_HEAD, FIFFV_BEM_SURF_ID_SKULL, FIFFV_BEM_SURF_ID_BRAIN});
}
 
//=============================================================================================================
 
int FwdBemModel::fwd_bem_load_solution(const QString& name, int bemMethod)
/*
     * Load the potential solution matrix and attach it to the model
     */
{
    QFile file(name);
    FiffStream::SPtr stream(new FiffStream(&file));
 
    FiffDirNode::SPtr bem_node;
    int method;
    FiffTag::UPtr t_pTag;
    int nSolutions;
 
    if (!stream->open()) {
        stream->close();
        return NOT_FOUND;
    }
 
    /*
       * Find the BEM data
       */
    {
        QList<FiffDirNode::SPtr> nodes = stream->dirtree()->dir_tree_find(FIFFB_BEM);
 
        if (nodes.size() == 0) {
            qWarning("No BEM data in %s", name.toUtf8().constData());
            stream->close();
            return NOT_FOUND;
        }
        bem_node = nodes[0];
    }
    /*
        * Approximation method
        */
    if (get_int(stream, bem_node, FIFF_BEM_APPROX, &method) != OK) {
        stream->close();
        return NOT_FOUND;
    }
    if (method == FIFFV_BEM_APPROX_CONST)
        method = FWD_BEM_CONSTANT_COLL;
    else if (method == FIFFV_BEM_APPROX_LINEAR)
        method = FWD_BEM_LINEAR_COLL;
    else {
        qWarning("Cannot handle BEM approximation method : %d", method);
        stream->close();
        return FAIL;
    }
    if (bemMethod != FWD_BEM_UNKNOWN && method != bemMethod) {
        qWarning("Approximation method in file : %d desired : %d", method, bemMethod);
        stream->close();
        return NOT_FOUND;
    }
    {
        int dim, k;
 
        if (!bem_node->find_tag(stream, FIFF_BEM_POT_SOLUTION, t_pTag)) {
            stream->close();
            return FAIL;
        }
        qint32 ndim;
        QVector<qint32> dims;
        t_pTag->getMatrixDimensions(ndim, dims);
 
        if (ndim != 2) {
            qWarning("Expected a two-dimensional solution matrix instead of a %d dimensional one", ndim);
            stream->close();
            return FAIL;
        }
        for (k = 0, dim = 0; k < nsurf; k++)
            dim = dim + ((method == FWD_BEM_LINEAR_COLL) ? surfs[k]->np : surfs[k]->ntri);
        if (dims[0] != dim || dims[1] != dim) {
            qWarning("Expected a %d x %d solution matrix instead of a %d x %d  one", dim, dim, dims[0], dims[1]);
            stream->close();
            return NOT_FOUND;
        }
 
        MatrixXf tmp_sol = t_pTag->toFloatMatrix().transpose();
        nSolutions = dims[1];
    }
    fwd_bem_free_solution();
    sol_name = name;
    solution = t_pTag->toFloatMatrix().transpose();
    this->nsol = nSolutions;
    this->bem_method = method;
    stream->close();
 
    return LOADED;
}
 
//=============================================================================================================
 
int FwdBemModel::fwd_bem_set_head_mri_t(const FiffCoordTrans& t)
/*
     * Set the coordinate transformation
     */
{
    if (t.from == FIFFV_COORD_HEAD && t.to == FIFFV_COORD_MRI) {
        head_mri_t = t;
        return OK;
    } else if (t.from == FIFFV_COORD_MRI && t.to == FIFFV_COORD_HEAD) {
        head_mri_t = t.inverted();
        return OK;
    } else {
        qWarning("Improper coordinate transform delivered to fwd_bem_set_head_mri_t");
        return FAIL;
    }
}
 
//=============================================================================================================
 
std::unique_ptr<MNESourceSpace> FwdBemModel::make_guesses(MNESurface* guess_surf, float guessrad, const Eigen::Vector3f& guess_r0, float grid, float exclude, float mindist)
{
    QString bemname;
    MNESurface::UPtr sphere_owner;
    std::unique_ptr<MNESourceSpace> res;
    int k;
    float dist;
 
    if (!guess_surf) {
        qInfo("Making a spherical guess space with radius %7.1f mm...", 1000 * guessrad);
 
        QFile bemFile(QString(QCoreApplication::applicationDirPath() + "/../resources/general/surf2bem/icos.fif"));
        if (!QCoreApplication::startingUp())
            bemFile.setFileName(QCoreApplication::applicationDirPath() + QString("/../resources/general/surf2bem/icos.fif"));
        else if (!bemFile.exists())
            bemFile.setFileName("../resources/general/surf2bem/icos.fif");
 
        if (!bemFile.exists()) {
            qDebug() << bemFile.fileName() << "does not exists.";
            return res;
        }
 
        bemname = bemFile.fileName();
 
        sphere_owner = MNESurface::read_bem_surface(bemname, 9003, false);
        if (!sphere_owner)
            return res;
 
        for (k = 0; k < sphere_owner->np; k++) {
            dist = sphere_owner->point(k).norm();
            sphere_owner->rr.row(k) = (guessrad * sphere_owner->rr.row(k) / dist) + guess_r0.transpose();
        }
        if (sphere_owner->add_geometry_info(true) == FAIL)
            return res;
        guess_surf = sphere_owner.get();
    } else {
        qInfo("Guess surface (%d = %s) is in %s coordinates",
              guess_surf->id, FwdBemModel::fwd_bem_explain_surface(guess_surf->id).toUtf8().constData(),
              mne_coord_frame_name_40(guess_surf->coord_frame).toUtf8().constData());
    }
    qInfo("Filtering (grid = %6.f mm)...", 1000 * grid);
    res = MNESourceSpace::make_volume_source_space(*guess_surf, grid, exclude, mindist);
 
    return res;
}
 
//=============================================================================================================
 
double FwdBemModel::calc_beta(const Eigen::Vector3d& rk, const Eigen::Vector3d& rk1)
 
{
    Eigen::Vector3d rkk1 = rk1 - rk;
    double size = rkk1.norm();
 
    return log((rk.norm() * size + rk.dot(rkk1)) /
               (rk1.norm() * size + rk1.dot(rkk1))) /
        size;
}
 
//=============================================================================================================
 
void FwdBemModel::lin_pot_coeff(const Eigen::Vector3f& from, MNETriangle& to, Eigen::Vector3d& omega) /* The final result */
/*
          * The linear potential matrix element computations
          */
{
    Eigen::Vector3d y1, y2, y3; /* Corners with origin at from */
    double l1, l2, l3;          /* Lengths of y1, y2, and y3 */
    double solid;               /* The standard solid angle */
    Eigen::Vector3d vec_omega;  /* The cross-product integral */
    double triple;              /* (y1 x y2) . y3 */
    double ss;
    double beta[3], bbeta[3];
    int j, k;
    double n2, area2;
    static const double solid_eps = 4.0 * M_PI / 1.0E6;
    /*
       * Corners with origin at from (float → double)
       */
    y1 = (to.r1 - from).cast<double>();
    y2 = (to.r2 - from).cast<double>();
    y3 = (to.r3 - from).cast<double>();
    /*
       * Circular indexing for vertex access
       */
    const Eigen::Vector3d* y_arr[5] = {&y3, &y1, &y2, &y3, &y1};
    const Eigen::Vector3d** yy = y_arr + 1; /* yy can have index -1! */
    /*
       * The standard solid angle computation
       */
    Eigen::Vector3d cross = y1.cross(y2);
    triple = cross.dot(y3);
 
    l1 = y1.norm();
    l2 = y2.norm();
    l3 = y3.norm();
    ss = (l1 * l2 * l3 + y1.dot(y2) * l3 + y1.dot(y3) * l2 + y2.dot(y3) * l1);
    solid = 2.0 * atan2(triple, ss);
    if (std::fabs(solid) < solid_eps) {
        omega.setZero();
    } else {
        /*
         * Calculate the magic vector vec_omega
         */
        for (j = 0; j < 3; j++)
            beta[j] = calc_beta(*yy[j], *yy[j + 1]);
        bbeta[0] = beta[2] - beta[0];
        bbeta[1] = beta[0] - beta[1];
        bbeta[2] = beta[1] - beta[2];
 
        vec_omega.setZero();
        for (j = 0; j < 3; j++)
            vec_omega += bbeta[j] * (*yy[j]);
        /*
         * Put it all together...
         */
        area2 = 2.0 * to.area;
        n2 = 1.0 / (area2 * area2);
        Eigen::Vector3d nn_d = to.nn.cast<double>();
        for (k = 0; k < 3; k++) {
            Eigen::Vector3d z = yy[k + 1]->cross(*yy[k - 1]);
            Eigen::Vector3d diff = *yy[k - 1] - *yy[k + 1];
            omega[k] = n2 * (-area2 * z.dot(nn_d) * solid + triple * diff.dot(vec_omega));
        }
    }
#ifdef CHECK
    /*
       * Check it out!
       *
       * omega1 + omega2 + omega3 = solid
       */
    double rel1 = (solid + omega[0] + omega[1] + omega[2]) / solid;
    /*
       * The other way of evaluating...
       */
    Eigen::Vector3d check = Eigen::Vector3d::Zero();
    Eigen::Vector3d nn_check = to.nn.cast<double>();
    for (k = 0; k < 3; k++) {
        Eigen::Vector3d z = nn_check.cross(*yy[k]);
        check += omega[k] * z;
    }
    check *= -area2 / triple;
    qInfo("(%g,%g,%g) =? (%g,%g,%g)",
          check[0], check[1], check[2],
          vec_omega[0], vec_omega[1], vec_omega[2]);
    check -= vec_omega;
    double rel2 = sqrt(check.dot(check) / vec_omega.dot(vec_omega));
    qInfo("err1 = %g, err2 = %g", 100 * rel1, 100 * rel2);
#endif
    return;
}
 
//=============================================================================================================
 
void FwdBemModel::correct_auto_elements(MNESurface& surf, Eigen::MatrixXf& mat)
/*
          * Improve auto-element approximation...
          */
{
    float sum, miss;
    int nnode = surf.np;
    int ntri = surf.ntri;
    int nmemb;
    int j, k;
    float pi2 = static_cast<float>(2.0 * M_PI);
    MNETriangle* tri;
 
#ifdef SIMPLE
    for (j = 0; j < nnode; j++) {
        sum = 0.0;
        for (k = 0; k < nnode; k++)
            sum = sum + mat(j, k);
        qInfo("row %d sum = %g missing = %g", j + 1, sum / pi2,
              1.0 - sum / pi2);
        mat(j, j) = pi2 - sum;
    }
#else
    for (j = 0; j < nnode; j++) {
        /*
         * How much is missing?
         */
        sum = 0.0;
        for (k = 0; k < nnode; k++)
            sum = sum + mat(j, k);
        miss = pi2 - sum;
        nmemb = surf.nneighbor_tri[j];
        /*
         * The node itself receives one half
         */
        mat(j, j) = miss / 2.0;
        /*
         * The rest is divided evenly among the member nodes...
         */
        miss = miss / (4.0 * nmemb);
        for (k = 0, tri = surf.tris.data(); k < ntri; k++, tri++) {
            if (tri->vert[0] == j) {
                mat(j, tri->vert[1]) = mat(j, tri->vert[1]) + miss;
                mat(j, tri->vert[2]) = mat(j, tri->vert[2]) + miss;
            } else if (tri->vert[1] == j) {
                mat(j, tri->vert[0]) = mat(j, tri->vert[0]) + miss;
                mat(j, tri->vert[2]) = mat(j, tri->vert[2]) + miss;
            } else if (tri->vert[2] == j) {
                mat(j, tri->vert[0]) = mat(j, tri->vert[0]) + miss;
                mat(j, tri->vert[1]) = mat(j, tri->vert[1]) + miss;
            }
        }
        /*
         * Just check it it out...
         *
        for (k = 0, sum = 0; k < nnode; k++)
          sum = sum + mat(j,k);
        qInfo("row %d sum = %g",j+1,sum/pi2);
        */
    }
#endif
    return;
}
 
//=============================================================================================================
 
Eigen::MatrixXf FwdBemModel::fwd_bem_lin_pot_coeff(const std::vector<MNESurface*>& surfs)
/*
 * Calculate the coefficients for linear collocation approach
 */
{
    int np1, np2, ntri, np_tot, np_max;
    MNETriangle* tri;
    Eigen::Vector3d omega;
    int j, k, p, q, c;
    int joff, koff;
    MNESurface* surf1;
    MNESurface* surf2;
 
    for (p = 0, np_tot = np_max = 0; p < static_cast<int>(surfs.size()); p++) {
        np_tot += surfs[p]->np;
        if (surfs[p]->np > np_max)
            np_max = surfs[p]->np;
    }
 
    Eigen::MatrixXf mat = Eigen::MatrixXf::Zero(np_tot, np_tot);
    Eigen::VectorXd row(np_max);
    for (p = 0, joff = 0; p < static_cast<int>(surfs.size()); p++, joff = joff + np1) {
        surf1 = surfs[p];
        np1 = surf1->np;
        for (q = 0, koff = 0; q < static_cast<int>(surfs.size()); q++, koff = koff + np2) {
            surf2 = surfs[q];
            np2 = surf2->np;
            ntri = surf2->ntri;
 
            qInfo("\t\t%s (%d) -> %s (%d) ... ",
                  fwd_bem_explain_surface(surf1->id).toUtf8().constData(), np1,
                  fwd_bem_explain_surface(surf2->id).toUtf8().constData(), np2);
 
            for (j = 0; j < np1; j++) {
                for (k = 0; k < np2; k++)
                    row[k] = 0.0;
                for (k = 0, tri = surf2->tris.data(); k < ntri; k++, tri++) {
                    /*
               * No contribution from a triangle that
               * this vertex belongs to
               */
                    if (p == q && (tri->vert[0] == j || tri->vert[1] == j || tri->vert[2] == j))
                        continue;
                    /*
               * Otherwise do the hard job
               */
                    lin_pot_coeff(surf1->point(j), *tri, omega);
                    for (c = 0; c < 3; c++)
                        row[tri->vert[c]] = row[tri->vert[c]] - omega[c];
                }
                for (k = 0; k < np2; k++)
                    mat(j + joff, k + koff) = row[k];
            }
            if (p == q) {
                Eigen::MatrixXf sub_mat = mat.block(joff, koff, np1, np1);
                correct_auto_elements(*surf1, sub_mat);
                mat.block(joff, koff, np1, np1) = sub_mat;
            }
            qInfo("[done]");
        }
    }
    return mat;
}
 
//=============================================================================================================
 
int FwdBemModel::fwd_bem_linear_collocation_solution()
/*
     * Compute the linear collocation potential solution
     */
{
    Eigen::MatrixXf coeff;
    float ip_mult;
    int k;
 
    fwd_bem_free_solution();
 
    // Extract raw surface pointers for coefficient functions
    std::vector<MNESurface*> rawSurfs;
    rawSurfs.reserve(nsurf);
    for (auto& s : surfs)
        rawSurfs.push_back(s.get());
 
    qInfo("\nComputing the linear collocation solution...");
    qInfo("\tMatrix coefficients...");
    coeff = fwd_bem_lin_pot_coeff(rawSurfs);
    if (coeff.size() == 0) {
        fwd_bem_free_solution();
        return FAIL;
    }
 
    for (k = 0, nsol = 0; k < nsurf; k++)
        nsol += surfs[k]->np;
 
    qInfo("\tInverting the coefficient matrix...");
    solution = fwd_bem_multi_solution(coeff, &gamma, nsurf, np);
    if (solution.size() == 0) {
        fwd_bem_free_solution();
        return FAIL;
    }
 
    /*
       * IP approach?
       */
    if ((nsurf == 3) &&
        (ip_mult = sigma[nsurf - 2] / sigma[nsurf - 1]) <= ip_approach_limit) {
        Eigen::MatrixXf ip_solution;
 
        qInfo("IP approach required...");
 
        qInfo("\tMatrix coefficients (homog)...");
        std::vector<MNESurface*> last_surfs = {surfs.back().get()};
        coeff = fwd_bem_lin_pot_coeff(last_surfs);
        if (coeff.size() == 0) {
            fwd_bem_free_solution();
            return FAIL;
        }
 
        qInfo("\tInverting the coefficient matrix (homog)...");
        ip_solution = fwd_bem_homog_solution(coeff, surfs[nsurf - 1]->np);
        if (ip_solution.size() == 0) {
            fwd_bem_free_solution();
            return FAIL;
        }
 
        qInfo("\tModify the original solution to incorporate IP approach...");
 
        fwd_bem_ip_modify_solution(solution, ip_solution, ip_mult, nsurf, this->np);
    }
    bem_method = FWD_BEM_LINEAR_COLL;
    qInfo("Solution ready.");
    return OK;
}
 
//=============================================================================================================
 
Eigen::MatrixXf FwdBemModel::fwd_bem_multi_solution(Eigen::MatrixXf& solids, const Eigen::MatrixXf* gamma, int nsurf, const Eigen::VectorXi& ntri)
/*
          * Invert I - solids/(2*M_PI)
          * Take deflation into account
          * The matrix is destroyed after inversion
          * This is the general multilayer case
          */
{
    int j, k, p, q;
    float defl;
    float pi2 = static_cast<float>(1.0 / (2 * M_PI));
    float mult;
    int joff, koff, jup, kup, ntot;
 
    for (j = 0, ntot = 0; j < nsurf; j++)
        ntot += ntri[j];
    defl = 1.0 / ntot;
    /*
       * Modify the matrix
       */
    for (p = 0, joff = 0; p < nsurf; p++) {
        jup = ntri[p] + joff;
        for (q = 0, koff = 0; q < nsurf; q++) {
            kup = ntri[q] + koff;
            mult = (gamma == nullptr) ? pi2 : pi2 * (*gamma)(p, q);
            for (j = joff; j < jup; j++)
                for (k = koff; k < kup; k++)
                    solids(j, k) = defl - solids(j, k) * mult;
            koff = kup;
        }
        joff = jup;
    }
    for (k = 0; k < ntot; k++)
        solids(k, k) = solids(k, k) + 1.0;
 
    Eigen::MatrixXf result = solids.inverse();
    return result;
}
 
//=============================================================================================================
 
Eigen::MatrixXf FwdBemModel::fwd_bem_homog_solution(Eigen::MatrixXf& solids, int ntri)
/*
          * Invert I - solids/(2*M_PI)
          * Take deflation into account
          * The matrix is destroyed after inversion
          * This is the homogeneous model case
          */
{
    return fwd_bem_multi_solution(solids, nullptr, 1, Eigen::VectorXi::Constant(1, ntri));
}
 
//=============================================================================================================
 
void FwdBemModel::fwd_bem_ip_modify_solution(Eigen::MatrixXf& solution, Eigen::MatrixXf& ip_solution, float ip_mult, int nsurf, const Eigen::VectorXi& ntri)
/*
          * Modify the solution according to the IP approach
          */
{
    int s;
    int j, k, joff, koff, nlast;
    float mult;
 
    for (s = 0, koff = 0; s < nsurf - 1; s++)
        koff = koff + ntri[s];
    nlast = ntri[nsurf - 1];
 
    Eigen::VectorXf row(nlast);
    mult = (1.0 + ip_mult) / ip_mult;
 
    qInfo("\t\tCombining...");
    qInfo("t ");
    ip_solution.transposeInPlace();
 
    for (s = 0, joff = 0; s < nsurf; s++) {
        qInfo("%d3 ", s + 1);
        /*
         * For each row in this surface block, compute dot products
         * with the transposed ip_solution and subtract 2x the result
         */
        for (j = 0; j < ntri[s]; j++) {
            for (k = 0; k < nlast; k++) {
                row[k] = solution.row(j + joff).segment(koff, nlast).dot(ip_solution.row(k).head(nlast));
            }
            solution.row(j + joff).segment(koff, nlast) -= 2.0f * row.transpose();
        }
        joff = joff + ntri[s];
    }
 
    qInfo("t ");
    ip_solution.transposeInPlace();
 
    qInfo("33 ");
    /*
     * The lower right corner is a special case
     */
    for (j = 0; j < nlast; j++)
        for (k = 0; k < nlast; k++)
            solution(j + koff, k + koff) += mult * ip_solution(j, k);
    /*
     * Final scaling
     */
    qInfo("done.\n\t\tScaling...");
    solution *= ip_mult;
    qInfo("done.");
    return;
}
 
//=============================================================================================================
 
int FwdBemModel::fwd_bem_check_solids(const Eigen::MatrixXf& angles, int ntri1, int ntri2, float desired)
/*
 * Check the angle computations
 */
{
    float sum;
    int j, k;
    int res = 0;
 
    Eigen::VectorXf sums(ntri1);
    for (j = 0; j < ntri1; j++) {
        sum = 0;
        for (k = 0; k < ntri2; k++)
            sum = sum + angles(j, k);
        sums[j] = sum / (2 * M_PI);
    }
    for (j = 0; j < ntri1; j++)
        /*
        * Three cases:
        * same surface: sum = 2*pi
        * to outer:     sum = 4*pi
        * to inner:     sum = 0*pi;
        */
        if (std::fabs(sums[j] - desired) > 1e-4) {
            qWarning("solid angle matrix: rowsum[%d] = 2PI*%g",
                     j + 1, sums[j]);
            res = -1;
            break;
        }
    return res;
}
 
//=============================================================================================================
 
Eigen::MatrixXf FwdBemModel::fwd_bem_solid_angles(const std::vector<MNESurface*>& surfs)
/*
          * Compute the solid angle matrix
          */
{
    MNESurface* surf1;
    MNESurface* surf2;
    MNETriangle* tri;
    int ntri1, ntri2, ntri_tot;
    int j, k, p, q;
    int joff, koff;
    float result;
    float desired;
 
    for (p = 0, ntri_tot = 0; p < static_cast<int>(surfs.size()); p++)
        ntri_tot += surfs[p]->ntri;
 
    Eigen::MatrixXf solids = Eigen::MatrixXf::Zero(ntri_tot, ntri_tot);
    for (p = 0, joff = 0; p < static_cast<int>(surfs.size()); p++, joff = joff + ntri1) {
        surf1 = surfs[p];
        ntri1 = surf1->ntri;
        for (q = 0, koff = 0; q < static_cast<int>(surfs.size()); q++, koff = koff + ntri2) {
            surf2 = surfs[q];
            ntri2 = surf2->ntri;
            qInfo("\t\t%s (%d) -> %s (%d) ... ", fwd_bem_explain_surface(surf1->id).toUtf8().constData(), ntri1, fwd_bem_explain_surface(surf2->id).toUtf8().constData(), ntri2);
            for (j = 0; j < ntri1; j++)
                for (k = 0, tri = surf2->tris.data(); k < ntri2; k++, tri++) {
                    if (p == q && j == k)
                        result = 0.0;
                    else
                        result = MNESurfaceOrVolume::solid_angle(surf1->tris[j].cent, *tri);
                    solids(j + joff, k + koff) = result;
                }
            qInfo("[done]");
            if (p == q)
                desired = 1;
            else if (p < q)
                desired = 0;
            else
                desired = 2;
            Eigen::MatrixXf sub_block = solids.block(joff, koff, ntri1, ntri2);
            if (fwd_bem_check_solids(sub_block, ntri1, ntri2, desired) == FAIL) {
                return Eigen::MatrixXf();
            }
        }
    }
    return solids;
}
 
//=============================================================================================================
 
int FwdBemModel::fwd_bem_constant_collocation_solution()
/*
     * Compute the solution for the constant collocation approach
     */
{
    Eigen::MatrixXf solids;
    int k;
    float ip_mult;
 
    fwd_bem_free_solution();
 
    // Extract raw surface pointers for coefficient functions
    std::vector<MNESurface*> rawSurfs;
    rawSurfs.reserve(nsurf);
    for (auto& s : surfs)
        rawSurfs.push_back(s.get());
 
    qInfo("\nComputing the constant collocation solution...");
    qInfo("\tSolid angles...");
    solids = fwd_bem_solid_angles(rawSurfs);
    if (solids.size() == 0) {
        fwd_bem_free_solution();
        return FAIL;
    }
 
    for (k = 0, nsol = 0; k < nsurf; k++)
        nsol += surfs[k]->ntri;
 
    qInfo("\tInverting the coefficient matrix...");
    solution = fwd_bem_multi_solution(solids, &gamma, nsurf, ntri);
    if (solution.size() == 0) {
        fwd_bem_free_solution();
        return FAIL;
    }
    /*
       * IP approach?
       */
    if ((nsurf == 3) &&
        (ip_mult = sigma[nsurf - 2] / sigma[nsurf - 1]) <= ip_approach_limit) {
        Eigen::MatrixXf ip_solution;
 
        qInfo("IP approach required...");
 
        qInfo("\tSolid angles (homog)...");
        std::vector<MNESurface*> last_surfs = {surfs.back().get()};
        solids = fwd_bem_solid_angles(last_surfs);
        if (solids.size() == 0) {
            fwd_bem_free_solution();
            return FAIL;
        }
 
        qInfo("\tInverting the coefficient matrix (homog)...");
        ip_solution = fwd_bem_homog_solution(solids, surfs[nsurf - 1]->ntri);
        if (ip_solution.size() == 0) {
            fwd_bem_free_solution();
            return FAIL;
        }
 
        qInfo("\tModify the original solution to incorporate IP approach...");
        fwd_bem_ip_modify_solution(solution, ip_solution, ip_mult, nsurf, this->ntri);
    }
    bem_method = FWD_BEM_CONSTANT_COLL;
    qInfo("Solution ready.");
 
    return OK;
}
 
//=============================================================================================================
 
int FwdBemModel::fwd_bem_compute_solution(int bemMethod)
/*
 * Compute the solution
 */
{
    /*
        * Compute the solution
        */
    if (bemMethod == FWD_BEM_LINEAR_COLL)
        return fwd_bem_linear_collocation_solution();
    else if (bemMethod == FWD_BEM_CONSTANT_COLL)
        return fwd_bem_constant_collocation_solution();
 
    fwd_bem_free_solution();
    qWarning("Unknown BEM method: %d", bemMethod);
    return FAIL;
}
 
//=============================================================================================================
 
int FwdBemModel::fwd_bem_load_recompute_solution(const QString& name, int bemMethod, int force_recompute)
/*
 * Load or recompute the potential solution matrix
 */
{
    int solres;
 
    if (!force_recompute) {
        fwd_bem_free_solution();
        solres = fwd_bem_load_solution(name, bemMethod);
        if (solres == LOADED) {
            qInfo("\nLoaded %s BEM solution from %s", fwd_bem_explain_method(this->bem_method).toUtf8().constData(), name.toUtf8().constData());
            return OK;
        } else if (solres == FAIL)
            return FAIL;
#ifdef DEBUG
        else
            qWarning("Desired BEM  solution not available in %s (%s)", name, err_get_error());
#endif
    }
    if (bemMethod == FWD_BEM_UNKNOWN)
        bemMethod = FWD_BEM_LINEAR_COLL;
    return fwd_bem_compute_solution(bemMethod);
}
 
//=============================================================================================================
 
int FwdBemModel::fwd_bem_save_model(const QString& name) const
{
    if (nsurf == 0) {
        qWarning("[FwdBemModel::fwd_bem_save_model] No model to save");
        return FAIL;
    }
    if (solution.size() > 0 && bem_method != FWD_BEM_CONSTANT_COLL && bem_method != FWD_BEM_LINEAR_COLL) {
        qWarning("[FwdBemModel::fwd_bem_save_model] Unknown BEM method : %d", bem_method);
        return FAIL;
    }
    QFile file(name);
    FiffStream::SPtr stream = FiffStream::start_file(file);
    if (!stream) {
        return FAIL;
    }
    stream->start_block(FIFFB_BEM);
    int coordFrame = surfs[0]->coord_frame;
    stream->write_int(FIFF_BEM_COORD_FRAME, &coordFrame);
    for (int k = 0; k < nsurf; ++k) {
        surfs[k]->sigma = sigma[k];
        stream->start_block(FIFFB_BEM_SURF);
        surfs[k]->writeToStream(stream.data());
        stream->end_block(FIFFB_BEM_SURF);
    }
    if (solution.size() > 0) {
        int approx = (bem_method == FWD_BEM_CONSTANT_COLL) ? FIFFV_BEM_APPROX_CONST : FIFFV_BEM_APPROX_LINEAR;
        stream->write_int(FIFF_BEM_APPROX, &approx);
        stream->write_float_matrix(FIFF_BEM_POT_SOLUTION, solution);
    }
    stream->end_block(FIFFB_BEM);
    stream->end_file();
    return OK;
}
 
//=============================================================================================================
 
float FwdBemModel::fwd_bem_inf_field(const Eigen::Vector3f& rd, const Eigen::Vector3f& Q, const Eigen::Vector3f& rp, const Eigen::Vector3f& dir)
/*
     * Infinite-medium magnetic field
     * (without \mu_0/4\pi)
     */
{
    Eigen::Vector3f diff = rp - rd;
    float diff2 = diff.squaredNorm();
    Eigen::Vector3f cr = Q.cross(diff);
 
    return cr.dot(dir) / (diff2 * std::sqrt(diff2));
}
 
//=============================================================================================================
 
float FwdBemModel::fwd_bem_inf_pot(const Eigen::Vector3f& rd, const Eigen::Vector3f& Q, const Eigen::Vector3f& rp)
/*
     * The infinite medium potential
     */
{
    Eigen::Vector3f diff = rp - rd;
    float diff2 = diff.squaredNorm();
    return Q.dot(diff) / (4.0 * M_PI * diff2 * std::sqrt(diff2));
}
 
//=============================================================================================================
 
int FwdBemModel::fwd_bem_specify_els(FwdCoilSet* els)
/*
     * Set up for computing the solution at a set of electrodes
     */
{
    FwdCoil* el;
    MNESurface* scalp;
    int k, p, q, v;
    float r[3], w[3], dist;
    int best;
    MNETriangle* tri;
    float x, y, z;
    FwdBemSolution* sol = nullptr;
 
    if (solution.size() == 0) {
        qWarning("Solution not computed in fwd_bem_specify_els");
        return FAIL;
    }
    if (!els || els->ncoil() == 0)
        return OK;
    els->user_data.reset();
    /*
       * Hard work follows
       */
    els->user_data = std::make_unique<FwdBemSolution>();
    sol = els->user_data.get();
 
    sol->ncoil = els->ncoil();
    sol->np = nsol;
    sol->solution = Eigen::MatrixXf::Zero(sol->ncoil, sol->np);
    /*
       * Go through all coils
       */
    for (k = 0; k < els->ncoil(); k++) {
        el = els->coils[k].get();
        scalp = surfs[0].get();
        /*
         * Go through all 'integration points'
         */
        for (p = 0; p < el->np; p++) {
            r[0] = el->rmag(p, 0);
            r[1] = el->rmag(p, 1);
            r[2] = el->rmag(p, 2);
            if (!head_mri_t.isEmpty())
                FiffCoordTrans::apply_trans(r, head_mri_t, FIFFV_MOVE);
            best = scalp->project_to_surface(nullptr, Eigen::Map<const Eigen::Vector3f>(r), dist);
            if (best < 0) {
                qWarning("One of the electrodes could not be projected onto the scalp surface. How come?");
                els->user_data.reset();
                return FAIL;
            }
            if (bem_method == FWD_BEM_CONSTANT_COLL) {
                /*
                 * Simply pick the value at the triangle
                 */
                for (q = 0; q < nsol; q++)
                    sol->solution(k, q) += el->w[p] * solution(best, q);
            } else if (bem_method == FWD_BEM_LINEAR_COLL) {
                /*
                 * Calculate a linear interpolation between the vertex values
                 */
                tri = &scalp->tris[best];
                scalp->triangle_coords(Eigen::Map<const Eigen::Vector3f>(r), best, x, y, z);
 
                w[0] = el->w[p] * (1.0 - x - y);
                w[1] = el->w[p] * x;
                w[2] = el->w[p] * y;
                for (v = 0; v < 3; v++) {
                    for (q = 0; q < nsol; q++)
                        sol->solution(k, q) += w[v] * solution(tri->vert[v], q);
                }
            } else {
                qWarning("Unknown BEM approximation method : %d", bem_method);
                els->user_data.reset();
                return FAIL;
            }
        }
    }
    return OK;
}
 
//=============================================================================================================
 
void FwdBemModel::fwd_bem_pot_grad_calc(const Eigen::Vector3f& rd, const Eigen::Vector3f& Q, FwdCoilSet* els, int all_surfs, Eigen::Ref<Eigen::VectorXf> xgrad, Eigen::Ref<Eigen::VectorXf> ygrad, Eigen::Ref<Eigen::VectorXf> zgrad)
/*
 * Compute the potentials due to a current dipole
 */
{
    MNETriangle* tri;
    int nTriangles;
    int s, k, p, nSolutions, pp;
    float mult;
    Eigen::Vector3f ee;
    Eigen::Vector3f mri_rd = rd;
    Eigen::Vector3f mri_Q = Q;
 
    Eigen::Ref<Eigen::VectorXf>* grads[] = {&xgrad, &ygrad, &zgrad};
 
    if (v0.size() == 0)
        v0.resize(this->nsol);
    float* v0p = v0.data();
 
    if (!head_mri_t.isEmpty()) {
        FiffCoordTrans::apply_trans(mri_rd.data(), head_mri_t, FIFFV_MOVE);
        FiffCoordTrans::apply_trans(mri_Q.data(), head_mri_t, FIFFV_NO_MOVE);
    }
    for (pp = X; pp <= Z; pp++) {
        Eigen::Ref<Eigen::VectorXf>& grad = *grads[pp];
 
        ee = Eigen::Vector3f::Unit(pp);
        if (!head_mri_t.isEmpty())
            FiffCoordTrans::apply_trans(ee.data(), head_mri_t, FIFFV_NO_MOVE);
 
        for (s = 0, p = 0; s < nsurf; s++) {
            nTriangles = surfs[s]->ntri;
            tri = surfs[s]->tris.data();
            mult = source_mult[s];
            for (k = 0; k < nTriangles; k++, tri++)
                v0p[p++] = mult * fwd_bem_inf_pot_der(mri_rd, mri_Q, tri->cent, ee);
        }
        if (els) {
            FwdBemSolution* sol = els->user_data.get();
            nSolutions = sol->ncoil;
            for (k = 0; k < nSolutions; k++)
                grad[k] = sol->solution.row(k).dot(v0);
        } else {
            nSolutions = all_surfs ? this->nsol : surfs[0]->ntri;
            for (k = 0; k < nSolutions; k++)
                grad[k] = solution.row(k).dot(v0);
        }
    }
    return;
}
 
//=============================================================================================================
 
void FwdBemModel::fwd_bem_lin_pot_calc(const Eigen::Vector3f& rd, const Eigen::Vector3f& Q, FwdCoilSet* els, int all_surfs, Eigen::Ref<Eigen::VectorXf> pot)
/*
 * Compute the potentials due to a current dipole
 * using the linear potential approximation
 */
{
    int nPoints;
    int s, k, p, nSolutions;
    float mult;
    Eigen::Vector3f mri_rd = rd;
    Eigen::Vector3f mri_Q = Q;
 
    if (v0.size() == 0)
        v0.resize(this->nsol);
    float* v0p = v0.data();
 
    if (!head_mri_t.isEmpty()) {
        FiffCoordTrans::apply_trans(mri_rd.data(), head_mri_t, FIFFV_MOVE);
        FiffCoordTrans::apply_trans(mri_Q.data(), head_mri_t, FIFFV_NO_MOVE);
    }
    for (s = 0, p = 0; s < nsurf; s++) {
        nPoints = surfs[s]->np;
        mult = source_mult[s];
        for (k = 0; k < nPoints; k++)
            v0p[p++] = mult * fwd_bem_inf_pot(mri_rd, mri_Q, surfs[s]->point(k));
    }
    if (els) {
        FwdBemSolution* sol = els->user_data.get();
        nSolutions = sol->ncoil;
        for (k = 0; k < nSolutions; k++)
            pot[k] = sol->solution.row(k).dot(v0);
    } else {
        nSolutions = all_surfs ? this->nsol : surfs[0]->np;
        for (k = 0; k < nSolutions; k++)
            pot[k] = solution.row(k).dot(v0);
    }
    return;
}
 
//=============================================================================================================
 
void FwdBemModel::fwd_bem_lin_pot_grad_calc(const Eigen::Vector3f& rd, const Eigen::Vector3f& Q, FwdCoilSet* els, int all_surfs, Eigen::Ref<Eigen::VectorXf> xgrad, Eigen::Ref<Eigen::VectorXf> ygrad, Eigen::Ref<Eigen::VectorXf> zgrad)
/*
 * Compute the derivaties of potentials due to a current dipole with respect to the dipole position
 * using the linear potential approximation
 */
{
    int nPoints;
    int s, k, p, nSolutions, pp;
    float mult;
    Eigen::Vector3f mri_rd = rd;
    Eigen::Vector3f mri_Q = Q;
    Eigen::Vector3f ee;
 
    Eigen::Ref<Eigen::VectorXf>* grads[] = {&xgrad, &ygrad, &zgrad};
 
    if (v0.size() == 0)
        v0.resize(this->nsol);
    float* v0p = v0.data();
 
    if (!head_mri_t.isEmpty()) {
        FiffCoordTrans::apply_trans(mri_rd.data(), head_mri_t, FIFFV_MOVE);
        FiffCoordTrans::apply_trans(mri_Q.data(), head_mri_t, FIFFV_NO_MOVE);
    }
    for (pp = X; pp <= Z; pp++) {
        Eigen::Ref<Eigen::VectorXf>& grad = *grads[pp];
 
        ee = Eigen::Vector3f::Unit(pp);
        if (!head_mri_t.isEmpty())
            FiffCoordTrans::apply_trans(ee.data(), head_mri_t, FIFFV_NO_MOVE);
 
        for (s = 0, p = 0; s < nsurf; s++) {
            nPoints = surfs[s]->np;
            mult = source_mult[s];
            for (k = 0; k < nPoints; k++)
                v0p[p++] = mult * fwd_bem_inf_pot_der(mri_rd, mri_Q, surfs[s]->point(k), ee);
        }
        if (els) {
            FwdBemSolution* sol = els->user_data.get();
            nSolutions = sol->ncoil;
            for (k = 0; k < nSolutions; k++)
                grad[k] = sol->solution.row(k).dot(v0);
        } else {
            nSolutions = all_surfs ? this->nsol : surfs[0]->np;
            for (k = 0; k < nSolutions; k++)
                grad[k] = solution.row(k).dot(v0);
        }
    }
    return;
}
 
//=============================================================================================================
 
void FwdBemModel::fwd_bem_pot_calc(const Eigen::Vector3f& rd, const Eigen::Vector3f& Q, FwdCoilSet* els, int all_surfs, Eigen::Ref<Eigen::VectorXf> pot)
/*
          * Compute the potentials due to a current dipole
          */
{
    MNETriangle* tri;
    int nTriangles;
    int s, k, p, nSolutions;
    float mult;
    Eigen::Vector3f mri_rd = rd;
    Eigen::Vector3f mri_Q = Q;
 
    if (v0.size() == 0)
        v0.resize(this->nsol);
    float* v0p = v0.data();
 
    if (!head_mri_t.isEmpty()) {
        FiffCoordTrans::apply_trans(mri_rd.data(), head_mri_t, FIFFV_MOVE);
        FiffCoordTrans::apply_trans(mri_Q.data(), head_mri_t, FIFFV_NO_MOVE);
    }
    for (s = 0, p = 0; s < nsurf; s++) {
        nTriangles = surfs[s]->ntri;
        tri = surfs[s]->tris.data();
        mult = source_mult[s];
        for (k = 0; k < nTriangles; k++, tri++)
            v0p[p++] = mult * fwd_bem_inf_pot(mri_rd, mri_Q, tri->cent);
    }
    if (els) {
        FwdBemSolution* sol = els->user_data.get();
        nSolutions = sol->ncoil;
        for (k = 0; k < nSolutions; k++)
            pot[k] = sol->solution.row(k).dot(v0);
    } else {
        nSolutions = all_surfs ? this->nsol : surfs[0]->ntri;
        for (k = 0; k < nSolutions; k++)
            pot[k] = solution.row(k).dot(v0);
    }
    return;
}
 
//=============================================================================================================
 
int FwdBemModel::fwd_bem_pot_els(const Eigen::Vector3f& rd, const Eigen::Vector3f& Q, FwdCoilSet& els, Eigen::Ref<Eigen::VectorXf> pot, void* client) /* The model */
/*
     * This version calculates the potential on all surfaces
     */
{
    auto* m = static_cast<FwdBemModel*>(client);
    FwdBemSolution* sol = els.user_data.get();
 
    if (!m) {
        qWarning("No BEM model specified to fwd_bem_pot_els");
        return FAIL;
    }
    if (m->solution.size() == 0) {
        qWarning("No solution available for fwd_bem_pot_els");
        return FAIL;
    }
    if (!sol || sol->ncoil != els.ncoil()) {
        qWarning("No appropriate electrode-specific data available in fwd_bem_pot_coils");
        return FAIL;
    }
    if (m->bem_method == FWD_BEM_CONSTANT_COLL) {
        m->fwd_bem_pot_calc(rd, Q, &els, false, pot);
    } else if (m->bem_method == FWD_BEM_LINEAR_COLL) {
        m->fwd_bem_lin_pot_calc(rd, Q, &els, false, pot);
    } else {
        qWarning("Unknown BEM method : %d", m->bem_method);
        return FAIL;
    }
    return OK;
}
 
//=============================================================================================================
 
int FwdBemModel::fwd_bem_pot_grad_els(const Eigen::Vector3f& rd, const Eigen::Vector3f& Q, FwdCoilSet& els, Eigen::Ref<Eigen::VectorXf> pot, Eigen::Ref<Eigen::VectorXf> xgrad, Eigen::Ref<Eigen::VectorXf> ygrad, Eigen::Ref<Eigen::VectorXf> zgrad, void* client) /* The model */
/*
     * This version calculates the potential on all surfaces
     */
{
    auto* m = static_cast<FwdBemModel*>(client);
    FwdBemSolution* sol = els.user_data.get();
 
    if (!m) {
        qCritical("No BEM model specified to fwd_bem_pot_els");
        return FAIL;
    }
    if (m->solution.size() == 0) {
        qCritical("No solution available for fwd_bem_pot_els");
        return FAIL;
    }
    if (!sol || sol->ncoil != els.ncoil()) {
        qCritical("No appropriate electrode-specific data available in fwd_bem_pot_coils");
        return FAIL;
    }
    if (m->bem_method == FWD_BEM_CONSTANT_COLL) {
        m->fwd_bem_pot_calc(rd, Q, &els, false, pot);
        m->fwd_bem_pot_grad_calc(rd, Q, &els, false, xgrad, ygrad, zgrad);
    } else if (m->bem_method == FWD_BEM_LINEAR_COLL) {
        m->fwd_bem_lin_pot_calc(rd, Q, &els, false, pot);
        m->fwd_bem_lin_pot_grad_calc(rd, Q, &els, false, xgrad, ygrad, zgrad);
    } else {
        qCritical("Unknown BEM method : %d", m->bem_method);
        return FAIL;
    }
    return OK;
}
 
//=============================================================================================================
 
inline double arsinh(double x)
{
    return std::asinh(x);
}
 
void FwdBemModel::calc_f(const Eigen::Vector3d& xx, const Eigen::Vector3d& yy, Eigen::Vector3d& f0, Eigen::Vector3d& fx, Eigen::Vector3d& fy)
{
    double det = -xx[1] * yy[0] + xx[2] * yy[0] +
        xx[0] * yy[1] - xx[2] * yy[1] - xx[0] * yy[2] + xx[1] * yy[2];
 
    f0[0] = -xx[2] * yy[1] + xx[1] * yy[2];
    f0[1] = xx[2] * yy[0] - xx[0] * yy[2];
    f0[2] = -xx[1] * yy[0] + xx[0] * yy[1];
 
    fx[0] = yy[1] - yy[2];
    fx[1] = -yy[0] + yy[2];
    fx[2] = yy[0] - yy[1];
 
    fy[0] = -xx[1] + xx[2];
    fy[1] = xx[0] - xx[2];
    fy[2] = -xx[0] + xx[1];
 
    f0 /= det;
    fx /= det;
    fy /= det;
}
 
//=============================================================================================================
 
void FwdBemModel::calc_magic(double u, double z, double A, double B, Eigen::Vector3d& beta, double& D)
{
    double B2 = 1.0 + B * B;
    double ABu = A + B * u;
    D = sqrt(u * u + z * z + ABu * ABu);
    beta[0] = ABu / sqrt(u * u + z * z);
    beta[1] = (A * B + B2 * u) / sqrt(A * A + B2 * z * z);
    beta[2] = (B * z * z - A * u) / (z * D);
}
 
//=============================================================================================================
 
void FwdBemModel::field_integrals(const Eigen::Vector3f& from, MNETriangle& to, double& I1p, Eigen::Vector2d& T, Eigen::Vector2d& S1, Eigen::Vector2d& S2, Eigen::Vector3d& f0, Eigen::Vector3d& fx, Eigen::Vector3d& fy)
{
    double xx[4], yy[4];
    double A, B, z, dx;
    Eigen::Vector3d beta;
    double I1, Tx, Ty, Txx, Tyy, Sxx, mult;
    double S1x, S1y, S2x;
    double D1, B2;
    int k;
    /*
       * Preliminaries...
       *
       * 1. Move origin to viewpoint...
       *
       */
    Eigen::Vector3d y1 = (to.r1 - from).cast<double>();
    Eigen::Vector3d y2 = (to.r2 - from).cast<double>();
    Eigen::Vector3d y3 = (to.r3 - from).cast<double>();
    /*
       * 2. Calculate local xy coordinates...
       */
    Eigen::Vector3d ex_d = to.ex.cast<double>();
    Eigen::Vector3d ey_d = to.ey.cast<double>();
    xx[0] = y1.dot(ex_d);
    xx[1] = y2.dot(ex_d);
    xx[2] = y3.dot(ex_d);
    xx[3] = xx[0];
 
    yy[0] = y1.dot(ey_d);
    yy[1] = y2.dot(ey_d);
    yy[2] = y3.dot(ey_d);
    yy[3] = yy[0];
 
    calc_f(Eigen::Map<const Eigen::Vector3d>(xx), Eigen::Map<const Eigen::Vector3d>(yy), f0, fx, fy);
    /*
       * 3. Distance of the plane from origin...
       */
    z = y1.dot(to.nn.cast<double>());
    /*
       * Put together the line integral...
       * We use the convention where the local y-axis
       * is parallel to the last side and, therefore, dx = 0
       * on that side. We can thus omit the last side from this
       * computation in some cases.
       */
    I1 = 0.0;
    Tx = 0.0;
    Ty = 0.0;
    S1x = 0.0;
    S1y = 0.0;
    S2x = 0.0;
    for (k = 0; k < 2; k++) {
        dx = xx[k + 1] - xx[k];
        A = (yy[k] * xx[k + 1] - yy[k + 1] * xx[k]) / dx;
        B = (yy[k + 1] - yy[k]) / dx;
        B2 = (1.0 + B * B);
        /*
         * Upper limit
         */
        calc_magic(xx[k + 1], z, A, B, beta, D1);
        I1 = I1 - xx[k + 1] * arsinh(beta[0]) - (A / sqrt(1.0 + B * B)) * arsinh(beta[1]) - z * atan(beta[2]);
        Txx = arsinh(beta[1]) / sqrt(B2);
        Tx = Tx + Txx;
        Ty = Ty + B * Txx;
        Sxx = (D1 - A * B * Txx) / B2;
        S1x = S1x + Sxx;
        S1y = S1y + B * Sxx;
        Sxx = (B * D1 + A * Txx) / B2;
        S2x = S2x + Sxx;
        /*
         * Lower limit
         */
        calc_magic(xx[k], z, A, B, beta, D1);
        I1 = I1 + xx[k] * arsinh(beta[0]) + (A / sqrt(1.0 + B * B)) * arsinh(beta[1]) + z * atan(beta[2]);
        Txx = arsinh(beta[1]) / sqrt(B2);
        Tx = Tx - Txx;
        Ty = Ty - B * Txx;
        Sxx = (D1 - A * B * Txx) / B2;
        S1x = S1x - Sxx;
        S1y = S1y - B * Sxx;
        Sxx = (B * D1 + A * Txx) / B2;
        S2x = S2x - Sxx;
    }
    /*
       * Handle last side (dx = 0) in a special way;
       */
    mult = 1.0 / sqrt(xx[k] * xx[k] + z * z);
    /*
       * Upper...
       */
    Tyy = arsinh(mult * yy[k + 1]);
    Ty = Ty + Tyy;
    S1y = S1y + xx[k] * Tyy;
    /*
       * Lower...
       */
    Tyy = arsinh(mult * yy[k]);
    Ty = Ty - Tyy;
    S1y = S1y - xx[k] * Tyy;
    /*
       * Set return values
       */
    I1p = I1;
    T[0] = Tx;
    T[1] = Ty;
    S1[0] = S1x;
    S1[1] = S1y;
    S2[0] = S2x;
    S2[1] = -S1x;
}
 
//=============================================================================================================
 
double FwdBemModel::one_field_coeff(const Eigen::Vector3f& dest, const Eigen::Vector3f& normal, MNETriangle& tri)
{
    double beta[3];
    double bbeta[3];
    int j;
 
    Eigen::Vector3d y1 = (tri.r1 - dest).cast<double>();
    Eigen::Vector3d y2 = (tri.r2 - dest).cast<double>();
    Eigen::Vector3d y3 = (tri.r3 - dest).cast<double>();
 
    const Eigen::Vector3d* yy[4] = {&y1, &y2, &y3, &y1};
    for (j = 0; j < 3; j++)
        beta[j] = calc_beta(*yy[j], *yy[j + 1]);
    bbeta[0] = beta[2] - beta[0];
    bbeta[1] = beta[0] - beta[1];
    bbeta[2] = beta[1] - beta[2];
 
    Eigen::Vector3d coeff = Eigen::Vector3d::Zero();
    for (j = 0; j < 3; j++)
        coeff += bbeta[j] * (*yy[j]);
    return coeff.dot(normal.cast<double>());
}
 
//=============================================================================================================
 
Eigen::MatrixXf FwdBemModel::fwd_bem_field_coeff(FwdCoilSet* coils) /* Gradiometer coil positions */
/*
     * Compute the weighting factors to obtain the magnetic field
     */
{
    MNESurface* surf;
    MNETriangle* tri;
    FwdCoil* coil;
    FwdCoilSet::UPtr tcoils;
    int nTriangles;
    int j, k, p, s, off;
    double res;
    double mult;
 
    if (solution.size() == 0) {
        qWarning("Solution matrix missing in fwd_bem_field_coeff");
        return Eigen::MatrixXf();
    }
    if (bem_method != FWD_BEM_CONSTANT_COLL) {
        qWarning("BEM method should be constant collocation for fwd_bem_field_coeff");
        return Eigen::MatrixXf();
    }
    if (coils->coord_frame != FIFFV_COORD_MRI) {
        if (coils->coord_frame == FIFFV_COORD_HEAD) {
            if (head_mri_t.isEmpty()) {
                qWarning("head -> mri coordinate transform missing in fwd_bem_field_coeff");
                return Eigen::MatrixXf();
            } else {
                /*
                    * Make a transformed duplicate
                    */
                if ((tcoils = coils->dup_coil_set(head_mri_t)) == nullptr)
                    return Eigen::MatrixXf();
                coils = tcoils.get();
            }
        } else {
            qWarning("Incompatible coil coordinate frame %d for fwd_bem_field_coeff", coils->coord_frame);
            return Eigen::MatrixXf();
        }
    }
    nTriangles = nsol;
    Eigen::MatrixXf coeff = Eigen::MatrixXf::Zero(coils->ncoil(), nTriangles);
 
    for (s = 0, off = 0; s < nsurf; s++) {
        surf = surfs[s].get();
        nTriangles = surf->ntri;
        tri = surf->tris.data();
        mult = field_mult[s];
 
        for (k = 0; k < nTriangles; k++, tri++) {
            for (j = 0; j < coils->ncoil(); j++) {
                coil = coils->coils[j].get();
                res = 0.0;
                for (p = 0; p < coil->np; p++)
                    res = res + coil->w[p] * one_field_coeff(coil->pos(p), coil->dir(p), *tri);
                coeff(j, k + off) = mult * res;
            }
        }
        off = off + nTriangles;
    }
    return coeff;
}
 
//=============================================================================================================
 
double FwdBemModel::calc_gamma(const Eigen::Vector3d& rk, const Eigen::Vector3d& rk1)
{
    Eigen::Vector3d rkk1 = rk1 - rk;
    double size = rkk1.norm();
 
    return log((rk1.norm() * size + rk1.dot(rkk1)) /
               (rk.norm() * size + rk.dot(rkk1))) /
        size;
}
 
//=============================================================================================================
 
void FwdBemModel::fwd_bem_one_lin_field_coeff_ferg(const Eigen::Vector3f& dest, const Eigen::Vector3f& dir, MNETriangle& tri, Eigen::Vector3d& res)
{
    double triple, l1, l2, l3, solid, clen;
    double common, sum, beta, gamma;
    int k;
 
    Eigen::Vector3d rjk[3];
    rjk[0] = (tri.r3 - tri.r2).cast<double>();
    rjk[1] = (tri.r1 - tri.r3).cast<double>();
    rjk[2] = (tri.r2 - tri.r1).cast<double>();
 
    Eigen::Vector3d y1 = (tri.r1 - dest).cast<double>();
    Eigen::Vector3d y2 = (tri.r2 - dest).cast<double>();
    Eigen::Vector3d y3 = (tri.r3 - dest).cast<double>();
 
    const Eigen::Vector3d* yy[4] = {&y1, &y2, &y3, &y1};
 
    Eigen::Vector3d nn_d = tri.nn.cast<double>();
    clen = y1.dot(nn_d);
    Eigen::Vector3d c_vec = clen * nn_d;
    Eigen::Vector3d A_vec = dest.cast<double>() + c_vec;
 
    Eigen::Vector3d c1 = tri.r1.cast<double>() - A_vec;
    Eigen::Vector3d c2 = tri.r2.cast<double>() - A_vec;
    Eigen::Vector3d c3 = tri.r3.cast<double>() - A_vec;
 
    const Eigen::Vector3d* cc[4] = {&c1, &c2, &c3, &c1};
    /*
       * beta and gamma...
       */
    for (sum = 0.0, k = 0; k < 3; k++) {
        Eigen::Vector3d cross = cc[k]->cross(*cc[k + 1]);
        beta = cross.dot(nn_d);
        gamma = calc_gamma(*yy[k], *yy[k + 1]);
        sum = sum + beta * gamma;
    }
    /*
       * Solid angle...
       */
    Eigen::Vector3d cross = y1.cross(y2);
    triple = cross.dot(y3);
 
    l1 = y1.norm();
    l2 = y2.norm();
    l3 = y3.norm();
    solid = 2.0 * atan2(triple, (l1 * l2 * l3 + y1.dot(y2) * l3 + y1.dot(y3) * l2 + y2.dot(y3) * l1));
    /*
       * Now we are ready to assemble it all together
       */
    Eigen::Vector3d dir_d = dir.cast<double>();
    common = (sum - clen * solid) / (2.0 * tri.area);
    for (k = 0; k < 3; k++)
        res[k] = -rjk[k].dot(dir_d) * common;
    return;
}
 
//=============================================================================================================
 
void FwdBemModel::fwd_bem_one_lin_field_coeff_uran(const Eigen::Vector3f& dest, const Eigen::Vector3f& dir_in, MNETriangle& tri, Eigen::Vector3d& res)
{
    double I1;
    Eigen::Vector2d T, S1, S2;
    Eigen::Vector3d f0, fx, fy;
    double res_x, res_y;
    double x_fac, y_fac;
    int k;
    /*
       * Compute the component integrals
       */
    field_integrals(dest, tri, I1, T, S1, S2, f0, fx, fy);
    /*
       * Compute the coefficient for each node...
       */
    Eigen::Vector3f dir = dir_in.normalized();
 
    x_fac = -dir.dot(tri.ex);
    y_fac = -dir.dot(tri.ey);
    for (k = 0; k < 3; k++) {
        res_x = f0[k] * T[0] + fx[k] * S1[0] + fy[k] * S2[0] + fy[k] * I1;
        res_y = f0[k] * T[1] + fx[k] * S1[1] + fy[k] * S2[1] - fx[k] * I1;
        res[k] = x_fac * res_x + y_fac * res_y;
    }
}
 
//=============================================================================================================
 
void FwdBemModel::fwd_bem_one_lin_field_coeff_simple(const Eigen::Vector3f& dest, const Eigen::Vector3f& normal, MNETriangle& source, Eigen::Vector3d& res)
{
    int k;
    const Eigen::Vector3f* rr[3] = {&source.r1, &source.r2, &source.r3};
 
    for (k = 0; k < 3; k++) {
        Eigen::Vector3f diff = dest - *rr[k];
        float dl = diff.squaredNorm();
        Eigen::Vector3f vec_result = diff.cross(source.nn);
        res[k] = source.area * vec_result.dot(normal) / (3.0 * dl * sqrt(dl));
    }
    return;
}
 
//=============================================================================================================
 
Eigen::MatrixXf FwdBemModel::fwd_bem_lin_field_coeff(FwdCoilSet* coils, int method) /* Which integration formula to use */
/*
          * Compute the weighting factors to obtain the magnetic field
          * in the linear potential approximation
          */
{
    MNESurface* surf;
    MNETriangle* tri;
    FwdCoil* coil;
    FwdCoilSet::UPtr tcoils;
    int nTriangles;
    int j, k, p, pp, off, s;
    Eigen::Vector3d res, one;
    float mult;
    linFieldIntFunc func;
 
    if (solution.size() == 0) {
        qWarning("Solution matrix missing in fwd_bem_lin_field_coeff");
        return Eigen::MatrixXf();
    }
    if (bem_method != FWD_BEM_LINEAR_COLL) {
        qWarning("BEM method should be linear collocation for fwd_bem_lin_field_coeff");
        return Eigen::MatrixXf();
    }
    if (coils->coord_frame != FIFFV_COORD_MRI) {
        if (coils->coord_frame == FIFFV_COORD_HEAD) {
            if (head_mri_t.isEmpty()) {
                qWarning("head -> mri coordinate transform missing in fwd_bem_lin_field_coeff");
                return Eigen::MatrixXf();
            } else {
                /*
                    * Make a transformed duplicate
                    */
                if ((tcoils = coils->dup_coil_set(head_mri_t)) == nullptr)
                    return Eigen::MatrixXf();
                coils = tcoils.get();
            }
        } else {
            qWarning("Incompatible coil coordinate frame %d for fwd_bem_field_coeff", coils->coord_frame);
            return Eigen::MatrixXf();
        }
    }
    if (method == FWD_BEM_LIN_FIELD_FERGUSON)
        func = fwd_bem_one_lin_field_coeff_ferg;
    else if (method == FWD_BEM_LIN_FIELD_URANKAR)
        func = fwd_bem_one_lin_field_coeff_uran;
    else
        func = fwd_bem_one_lin_field_coeff_simple;
 
    Eigen::MatrixXf coeff = Eigen::MatrixXf::Zero(coils->ncoil(), nsol);
    /*
       * Process each of the surfaces
       */
    for (s = 0, off = 0; s < nsurf; s++) {
        surf = surfs[s].get();
        nTriangles = surf->ntri;
        tri = surf->tris.data();
        mult = field_mult[s];
 
        for (k = 0; k < nTriangles; k++, tri++) {
            for (j = 0; j < coils->ncoil(); j++) {
                coil = coils->coils[j].get();
                res.setZero();
                /*
             * Accumulate the coefficients for each triangle node...
             */
                for (p = 0; p < coil->np; p++) {
                    func(coil->pos(p), coil->dir(p), *tri, one);
                    res += coil->w[p] * one;
                }
                /*
             * Add these to the corresponding coefficient matrix
             * elements...
             */
                for (pp = 0; pp < 3; pp++)
                    coeff(j, tri->vert[pp] + off) = coeff(j, tri->vert[pp] + off) + mult * res[pp];
            }
        }
        off = off + surf->np;
    }
    /*
       * Discard the duplicate
       */
    return coeff;
}
 
//=============================================================================================================
 
int FwdBemModel::fwd_bem_specify_coils(FwdCoilSet* coils)
/*
     * Set up for computing the solution at a set of coils
      */
{
    Eigen::MatrixXf sol;
    FwdBemSolution* csol = nullptr;
 
    if (solution.size() == 0) {
        qWarning("Solution not computed in fwd_bem_specify_coils");
        return FAIL;
    }
    if (coils)
        coils->user_data.reset();
    if (!coils || coils->ncoil() == 0)
        return OK;
    if (bem_method == FWD_BEM_CONSTANT_COLL)
        sol = fwd_bem_field_coeff(coils);
    else if (bem_method == FWD_BEM_LINEAR_COLL)
        sol = fwd_bem_lin_field_coeff(coils, FWD_BEM_LIN_FIELD_SIMPLE);
    else {
        qWarning("Unknown BEM method in fwd_bem_specify_coils : %d", bem_method);
        return FAIL;
    }
    if (sol.size() == 0)
        return FAIL;
    coils->user_data = std::make_unique<FwdBemSolution>();
    csol = coils->user_data.get();
 
    csol->ncoil = coils->ncoil();
    csol->np = nsol;
    csol->solution = sol * solution;
 
    return OK;
}
 
//=============================================================================================================
 
void FwdBemModel::fwd_bem_lin_field_calc(const Eigen::Vector3f& rd, const Eigen::Vector3f& Q, FwdCoilSet& coils, Eigen::Ref<Eigen::VectorXf> B)
/*
     * Calculate the magnetic field in a set of coils
     */
{
    int s, k, p, nPoints;
    FwdCoil* coil;
    float mult;
    Eigen::Vector3f my_rd = rd;
    Eigen::Vector3f my_Q = Q;
    FwdBemSolution* sol = coils.user_data.get();
    /*
       * Infinite-medium potentials
       */
    if (v0.size() == 0)
        v0.resize(nsol);
    float* v0p = v0.data();
    /*
       * The dipole location and orientation must be transformed
       */
    if (!head_mri_t.isEmpty()) {
        FiffCoordTrans::apply_trans(my_rd.data(), head_mri_t, FIFFV_MOVE);
        FiffCoordTrans::apply_trans(my_Q.data(), head_mri_t, FIFFV_NO_MOVE);
    }
    /*
       * Compute the inifinite-medium potentials at the vertices
       */
    for (s = 0, p = 0; s < nsurf; s++) {
        nPoints = surfs[s]->np;
        mult = source_mult[s];
        for (k = 0; k < nPoints; k++)
            v0p[p++] = mult * fwd_bem_inf_pot(my_rd, my_Q, surfs[s]->point(k));
    }
    /*
       * Primary current contribution
       * (can be calculated in the coil/dipole coordinates)
       */
    for (k = 0; k < coils.ncoil(); k++) {
        coil = coils.coils[k].get();
        B[k] = 0.0;
        for (p = 0; p < coil->np; p++)
            B[k] = B[k] + coil->w[p] * fwd_bem_inf_field(rd, Q, coil->pos(p), coil->dir(p));
    }
    /*
       * Volume current contribution
       */
    for (k = 0; k < coils.ncoil(); k++)
        B[k] = B[k] + sol->solution.row(k).dot(v0);
    /*
       * Scale correctly
       */
    for (k = 0; k < coils.ncoil(); k++)
        B[k] = MAG_FACTOR * B[k];
    return;
}
 
//=============================================================================================================
 
void FwdBemModel::fwd_bem_field_calc(const Eigen::Vector3f& rd, const Eigen::Vector3f& Q, FwdCoilSet& coils, Eigen::Ref<Eigen::VectorXf> B)
/*
     * Calculate the magnetic field in a set of coils
     */
{
    int s, k, p, nTriangles;
    FwdCoil* coil;
    MNETriangle* tri;
    float mult;
    Eigen::Vector3f my_rd = rd;
    Eigen::Vector3f my_Q = Q;
    FwdBemSolution* sol = coils.user_data.get();
    /*
       * Infinite-medium potentials
       */
    if (v0.size() == 0)
        v0.resize(nsol);
    float* v0p = v0.data();
    /*
       * The dipole location and orientation must be transformed
       */
    if (!head_mri_t.isEmpty()) {
        FiffCoordTrans::apply_trans(my_rd.data(), head_mri_t, FIFFV_MOVE);
        FiffCoordTrans::apply_trans(my_Q.data(), head_mri_t, FIFFV_NO_MOVE);
    }
    /*
       * Compute the inifinite-medium potentials at the centers of the triangles
       */
    for (s = 0, p = 0; s < nsurf; s++) {
        nTriangles = surfs[s]->ntri;
        tri = surfs[s]->tris.data();
        mult = source_mult[s];
        for (k = 0; k < nTriangles; k++, tri++)
            v0p[p++] = mult * fwd_bem_inf_pot(my_rd, my_Q, tri->cent);
    }
    /*
       * Primary current contribution
       * (can be calculated in the coil/dipole coordinates)
       */
    for (k = 0; k < coils.ncoil(); k++) {
        coil = coils.coils[k].get();
        B[k] = 0.0;
        for (p = 0; p < coil->np; p++)
            B[k] = B[k] + coil->w[p] * fwd_bem_inf_field(rd, Q, coil->pos(p), coil->dir(p));
    }
    /*
       * Volume current contribution
       */
    for (k = 0; k < coils.ncoil(); k++)
        B[k] = B[k] + sol->solution.row(k).dot(v0);
    /*
       * Scale correctly
       */
    for (k = 0; k < coils.ncoil(); k++)
        B[k] = MAG_FACTOR * B[k];
    return;
}
 
//=============================================================================================================
 
void FwdBemModel::fwd_bem_field_grad_calc(const Eigen::Vector3f& rd, const Eigen::Vector3f& Q, FwdCoilSet& coils, Eigen::Ref<Eigen::VectorXf> xgrad, Eigen::Ref<Eigen::VectorXf> ygrad, Eigen::Ref<Eigen::VectorXf> zgrad)
/*
 * Calculate the magnetic field in a set of coils
 */
{
    FwdBemSolution* sol = coils.user_data.get();
    int s, k, p, nTriangles, pp;
    FwdCoil* coil;
    MNETriangle* tri;
    float mult;
    Eigen::Ref<Eigen::VectorXf>* grads[] = {&xgrad, &ygrad, &zgrad};
    Eigen::Vector3f ee, mri_ee;
    Eigen::Vector3f mri_rd = rd;
    Eigen::Vector3f mri_Q = Q;
    /*
       * Infinite-medium potentials
       */
    if (v0.size() == 0)
        v0.resize(nsol);
    float* v0p = v0.data();
    /*
       * The dipole location and orientation must be transformed
       */
    if (!head_mri_t.isEmpty()) {
        FiffCoordTrans::apply_trans(mri_rd.data(), head_mri_t, FIFFV_MOVE);
        FiffCoordTrans::apply_trans(mri_Q.data(), head_mri_t, FIFFV_NO_MOVE);
    }
    for (pp = X; pp <= Z; pp++) {
        Eigen::Ref<Eigen::VectorXf>& grad = *grads[pp];
        /*
         * Select the correct gradient component
         */
        ee = Eigen::Vector3f::Unit(pp);
        mri_ee = ee;
        if (!head_mri_t.isEmpty())
            FiffCoordTrans::apply_trans(mri_ee.data(), head_mri_t, FIFFV_NO_MOVE);
        /*
         * Compute the inifinite-medium potential derivatives at the centers of the triangles
         */
        for (s = 0, p = 0; s < nsurf; s++) {
            nTriangles = surfs[s]->ntri;
            tri = surfs[s]->tris.data();
            mult = source_mult[s];
            for (k = 0; k < nTriangles; k++, tri++) {
                v0p[p++] = mult * fwd_bem_inf_pot_der(mri_rd, mri_Q, tri->cent, mri_ee);
            }
        }
        /*
         * Primary current contribution
         * (can be calculated in the coil/dipole coordinates)
         */
        for (k = 0; k < coils.ncoil(); k++) {
            coil = coils.coils[k].get();
            grad[k] = 0.0;
            for (p = 0; p < coil->np; p++)
                grad[k] = grad[k] + coil->w[p] * fwd_bem_inf_field_der(rd, Q, coil->pos(p), coil->dir(p), ee);
        }
        /*
         * Volume current contribution
         */
        for (k = 0; k < coils.ncoil(); k++)
            grad[k] = grad[k] + sol->solution.row(k).dot(v0);
        /*
         * Scale correctly
         */
        for (k = 0; k < coils.ncoil(); k++)
            grad[k] = MAG_FACTOR * grad[k];
    }
    return;
}
 
//=============================================================================================================
 
float FwdBemModel::fwd_bem_inf_field_der(const Eigen::Vector3f& rd, const Eigen::Vector3f& Q, const Eigen::Vector3f& rp, const Eigen::Vector3f& dir, const Eigen::Vector3f& comp)
/*
 * Derivative of the infinite-medium magnetic field with respect to
 * one of the dipole position coordinates (without \mu_0/4\pi)
 */
{
    Eigen::Vector3f diff = rp - rd;
    float diff2 = diff.squaredNorm();
    float diff3 = std::sqrt(diff2) * diff2;
    float diff5 = diff3 * diff2;
    Eigen::Vector3f cr = Q.cross(diff);
    Eigen::Vector3f crn = dir.cross(Q);
 
    return 3 * cr.dot(dir) * comp.dot(diff) / diff5 - comp.dot(crn) / diff3;
}
 
//=============================================================================================================
 
float FwdBemModel::fwd_bem_inf_pot_der(const Eigen::Vector3f& rd, const Eigen::Vector3f& Q, const Eigen::Vector3f& rp, const Eigen::Vector3f& comp)
/*
 * Derivative of the infinite-medium potential with respect to one of
 * the dipole position coordinates
 */
{
    Eigen::Vector3f diff = rp - rd;
    float diff2 = diff.squaredNorm();
    float diff3 = std::sqrt(diff2) * diff2;
    float diff5 = diff3 * diff2;
 
    float res = 3 * Q.dot(diff) * comp.dot(diff) / diff5 - comp.dot(Q) / diff3;
    return res / (4.0 * M_PI);
}
 
//=============================================================================================================
 
void FwdBemModel::fwd_bem_lin_field_grad_calc(const Eigen::Vector3f& rd, const Eigen::Vector3f& Q, FwdCoilSet& coils, Eigen::Ref<Eigen::VectorXf> xgrad, Eigen::Ref<Eigen::VectorXf> ygrad, Eigen::Ref<Eigen::VectorXf> zgrad)
/*
     * Calculate the gradient with respect to dipole position coordinates in a set of coils
     */
{
    FwdBemSolution* sol = coils.user_data.get();
 
    int s, k, p, nPoints, pp;
    FwdCoil* coil;
    float mult;
    Eigen::Vector3f ee, mri_ee;
    Eigen::Vector3f mri_rd = rd;
    Eigen::Vector3f mri_Q = Q;
    Eigen::Ref<Eigen::VectorXf>* grads[] = {&xgrad, &ygrad, &zgrad};
    /*
       * Space for infinite-medium potentials
       */
    if (v0.size() == 0)
        v0.resize(nsol);
    float* v0p = v0.data();
    /*
       * The dipole location and orientation must be transformed
       */
    if (!head_mri_t.isEmpty()) {
        FiffCoordTrans::apply_trans(mri_rd.data(), head_mri_t, FIFFV_MOVE);
        FiffCoordTrans::apply_trans(mri_Q.data(), head_mri_t, FIFFV_NO_MOVE);
    }
    for (pp = X; pp <= Z; pp++) {
        Eigen::Ref<Eigen::VectorXf>& grad = *grads[pp];
        /*
         * Select the correct gradient component
         */
        ee = Eigen::Vector3f::Unit(pp);
        mri_ee = ee;
        if (!head_mri_t.isEmpty())
            FiffCoordTrans::apply_trans(mri_ee.data(), head_mri_t, FIFFV_NO_MOVE);
        /*
         * Compute the inifinite-medium potentials at the vertices
         */
        for (s = 0, p = 0; s < nsurf; s++) {
            nPoints = surfs[s]->np;
            mult = source_mult[s];
 
            for (k = 0; k < nPoints; k++)
                v0p[p++] = mult * fwd_bem_inf_pot_der(mri_rd, mri_Q, surfs[s]->point(k), mri_ee);
        }
        /*
         * Primary current contribution
         * (can be calculated in the coil/dipole coordinates)
         */
        for (k = 0; k < coils.ncoil(); k++) {
            coil = coils.coils[k].get();
            grad[k] = 0.0;
            for (p = 0; p < coil->np; p++)
                grad[k] = grad[k] + coil->w[p] * fwd_bem_inf_field_der(rd, Q, coil->pos(p), coil->dir(p), ee);
        }
        /*
         * Volume current contribution
         */
        for (k = 0; k < coils.ncoil(); k++)
            grad[k] = grad[k] + sol->solution.row(k).dot(v0);
        /*
         * Scale correctly
         */
        for (k = 0; k < coils.ncoil(); k++)
            grad[k] = MAG_FACTOR * grad[k];
    }
    return;
}
 
//=============================================================================================================
 
int FwdBemModel::fwd_bem_field(const Eigen::Vector3f& rd, const Eigen::Vector3f& Q, FwdCoilSet& coils, Eigen::Ref<Eigen::VectorXf> B, void* client) /* The model */
/*
     * This version calculates the magnetic field in a set of coils
     * Call fwd_bem_specify_coils first to establish the coil-specific
     * solution matrix
     */
{
    auto* m = static_cast<FwdBemModel*>(client);
    FwdBemSolution* sol = coils.user_data.get();
 
    if (!m) {
        qWarning("No BEM model specified to fwd_bem_field");
        return FAIL;
    }
    if (!sol || sol->solution.size() == 0 || sol->ncoil != coils.ncoil()) {
        qWarning("No appropriate coil-specific data available in fwd_bem_field");
        return FAIL;
    }
    if (m->bem_method == FWD_BEM_CONSTANT_COLL) {
        m->fwd_bem_field_calc(rd, Q, coils, B);
    } else if (m->bem_method == FWD_BEM_LINEAR_COLL) {
        m->fwd_bem_lin_field_calc(rd, Q, coils, B);
    } else {
        qWarning("Unknown BEM method : %d", m->bem_method);
        return FAIL;
    }
    return OK;
}
 
//=============================================================================================================
 
int FwdBemModel::fwd_bem_field_grad(const Eigen::Vector3f& rd,
                                    const Eigen::Vector3f& Q,
                                    FwdCoilSet& coils,
                                    Eigen::Ref<Eigen::VectorXf> Bval,
                                    Eigen::Ref<Eigen::VectorXf> xgrad,
                                    Eigen::Ref<Eigen::VectorXf> ygrad,
                                    Eigen::Ref<Eigen::VectorXf> zgrad,
                                    void* client) /* Client data to be passed to some foward modelling routines */
{
    auto* m = static_cast<FwdBemModel*>(client);
    FwdBemSolution* sol = coils.user_data.get();
 
    if (!m) {
        qCritical("No BEM model specified to fwd_bem_field");
        return FAIL;
    }
 
    if (!sol || sol->solution.size() == 0 || sol->ncoil != coils.ncoil()) {
        qCritical("No appropriate coil-specific data available in fwd_bem_field");
        return FAIL;
    }
 
    if (m->bem_method == FWD_BEM_CONSTANT_COLL) {
        m->fwd_bem_field_calc(rd, Q, coils, Bval);
 
        m->fwd_bem_field_grad_calc(rd, Q, coils, xgrad, ygrad, zgrad);
    } else if (m->bem_method == FWD_BEM_LINEAR_COLL) {
        m->fwd_bem_lin_field_calc(rd, Q, coils, Bval);
 
        m->fwd_bem_lin_field_grad_calc(rd, Q, coils, xgrad, ygrad, zgrad);
    } else {
        qCritical("Unknown BEM method : %d", m->bem_method);
        return FAIL;
    }
 
    return OK;
}
 
//=============================================================================================================
 
void FwdBemModel::meg_eeg_fwd_one_source_space(FwdThreadArg* a)
/*
 * Compute the MEG or EEG forward solution for one source space
 * and possibly for only one source component
 */
{
    MNESourceSpace* s = a->s;
    int j, p, q;
 
    int ncoil = a->coils_els->ncoil();
    Eigen::MatrixXf tmp_vec_res(3, ncoil); /* Only needed for vec_field_pot (3 rows → 3 columns) */
 
    auto fail = [&]() {
        a->stat = FAIL;
    };
 
    p = a->off;
    q = 3 * a->off;
    if (a->fixed_ori) {                         /* The normal source component only */
        if (a->field_pot_grad && a->res_grad) { /* Gradient requested? */
            for (j = 0; j < s->np; j++) {
                if (s->inuse[j]) {
                    if (a->field_pot_grad(s->point(j),
                                          s->normal(j),
                                          *a->coils_els,
                                          a->res->col(p),
                                          a->res_grad->col(q),
                                          a->res_grad->col(q + 1),
                                          a->res_grad->col(q + 2),
                                          a->client) != OK) {
                        fail();
                        return;
                    }
                    q = q + 3;
                    p++;
                }
            }
        } else {
            for (j = 0; j < s->np; j++)
                if (s->inuse[j]) {
                    if (a->field_pot(s->point(j),
                                     s->normal(j),
                                     *a->coils_els,
                                     a->res->col(p),
                                     a->client) != OK) {
                        fail();
                        return;
                    }
                    p++;
                }
        }
    } else {                                    /* All source components */
        if (a->field_pot_grad && a->res_grad) { /* Gradient requested? */
            for (j = 0; j < s->np; j++) {
                if (s->inuse[j]) {
                    if (a->comp < 0) { /* Compute all components */
                        if (a->field_pot_grad(s->point(j),
                                              Qx, *a->coils_els, a->res->col(p), a->res_grad->col(q), a->res_grad->col(q + 1), a->res_grad->col(q + 2),
                                              a->client) != OK) {
                            fail();
                            return;
                        }
                        q = q + 3;
                        p++;
                        if (a->field_pot_grad(s->point(j),
                                              Qy, *a->coils_els, a->res->col(p), a->res_grad->col(q), a->res_grad->col(q + 1), a->res_grad->col(q + 2),
                                              a->client) != OK) {
                            fail();
                            return;
                        }
                        q = q + 3;
                        p++;
                        if (a->field_pot_grad(s->point(j),
                                              Qz, *a->coils_els, a->res->col(p), a->res_grad->col(q), a->res_grad->col(q + 1), a->res_grad->col(q + 2),
                                              a->client) != OK) {
                            fail();
                            return;
                        }
                        q = q + 3;
                        p++;
                    } else if (a->comp == 0) { /* Compute x component */
                        if (a->field_pot_grad(s->point(j),
                                              Qx, *a->coils_els, a->res->col(p), a->res_grad->col(q), a->res_grad->col(q + 1), a->res_grad->col(q + 2),
                                              a->client) != OK) {
                            fail();
                            return;
                        }
                        q = q + 3;
                        p++;
                        q = q + 3;
                        p++;
                        q = q + 3;
                        p++;
                    } else if (a->comp == 1) { /* Compute y component */
                        q = q + 3;
                        p++;
                        if (a->field_pot_grad(s->point(j),
                                              Qy, *a->coils_els, a->res->col(p), a->res_grad->col(q), a->res_grad->col(q + 1), a->res_grad->col(q + 2),
                                              a->client) != OK) {
                            fail();
                            return;
                        }
                        q = q + 3;
                        p++;
                        q = q + 3;
                        p++;
                    } else if (a->comp == 2) { /* Compute z component */
                        q = q + 3;
                        p++;
                        q = q + 3;
                        p++;
                        if (a->field_pot_grad(s->point(j),
                                              Qz, *a->coils_els, a->res->col(p), a->res_grad->col(q), a->res_grad->col(q + 1), a->res_grad->col(q + 2),
                                              a->client) != OK) {
                            fail();
                            return;
                        }
                        q = q + 3;
                        p++;
                    }
                }
            }
        } else {
            for (j = 0; j < s->np; j++) {
                if (s->inuse[j]) {
                    if (a->vec_field_pot) {
                        if (a->vec_field_pot(s->point(j), *a->coils_els, tmp_vec_res, a->client) != OK) {
                            fail();
                            return;
                        }
                        a->res->col(p++) = tmp_vec_res.row(0).transpose();
                        a->res->col(p++) = tmp_vec_res.row(1).transpose();
                        a->res->col(p++) = tmp_vec_res.row(2).transpose();
                    } else {
                        if (a->comp < 0) { /* Compute all components here */
                            if (a->field_pot(s->point(j), Qx, *a->coils_els, a->res->col(p++), a->client) != OK) {
                                fail();
                                return;
                            }
                            if (a->field_pot(s->point(j), Qy, *a->coils_els, a->res->col(p++), a->client) != OK) {
                                fail();
                                return;
                            }
                            if (a->field_pot(s->point(j), Qz, *a->coils_els, a->res->col(p++), a->client) != OK) {
                                fail();
                                return;
                            }
                        } else if (a->comp == 0) { /* Compute x component */
                            if (a->field_pot(s->point(j), Qx, *a->coils_els, a->res->col(p++), a->client) != OK) {
                                fail();
                                return;
                            }
                            p++;
                            p++;
                        } else if (a->comp == 1) { /* Compute y component */
                            p++;
                            if (a->field_pot(s->point(j), Qy, *a->coils_els, a->res->col(p++), a->client) != OK) {
                                fail();
                                return;
                            }
                            p++;
                        } else if (a->comp == 2) { /* Compute z component */
                            p++;
                            p++;
                            if (a->field_pot(s->point(j), Qz, *a->coils_els, a->res->col(p++), a->client) != OK) {
                                fail();
                                return;
                            }
                        }
                    }
                }
            }
        }
    }
    a->stat = OK;
    return;
}
 
//=============================================================================================================
 
int FwdBemModel::compute_forward_meg(std::vector<std::unique_ptr<MNESourceSpace>>& spaces,
                                     FwdCoilSet* coils,
                                     FwdCoilSet* comp_coils,
                                     MNECTFCompDataSet* comp_data,
                                     bool fixed_ori,
                                     const Vector3f& r0,
                                     bool use_threads,
                                     FiffNamedMatrix& resp,
                                     FiffNamedMatrix& resp_grad,
                                     bool bDoGrad)
/*
 * Compute the MEG forward solution
 * Use either the sphere model or BEM in the calculations
 */
{
    // A model without surfaces stands for the sphere model (MNE-C passed a null bem_model).
    const bool sphere = nsurf == 0;
    Eigen::Vector3f sphere_r0 = r0;
 
    Eigen::MatrixXf res_mat;      /* The forward solution matrix (ncoil x nsources) */
    Eigen::MatrixXf res_grad_mat; /* The gradient (ncoil x 3*nsources) */
    MatrixXd matRes;
    MatrixXd matResGrad;
    int nrow = 0;
    std::unique_ptr<FwdCompData> comp;
    fwdFieldFunc field;          /* Computes the field for one dipole orientation */
    fwdVecFieldFunc vec_field;   /* Computes the field for all dipole orientations */
    fwdFieldGradFunc field_grad; /* Computes the field and gradient with respect to dipole position
                                             * for one dipole orientation */
    int nmeg = coils->ncoil();   /* Number of channels */
    int nsource;                 /* Total number of sources */
    int nspace = static_cast<int>(spaces.size());
    int k, p, q, off;
    QStringList names; /* Channel names */
    void* client;
    FwdThreadArg::UPtr one_arg;
    int nproc = QThread::idealThreadCount();
    QStringList emptyList;
 
    auto cleanup_fail = [&]() {
        one_arg.reset();
        comp.reset();
        return FAIL;
    };
 
    /*
     * Use the new compensated field computation
     * It works the same way independent of whether or not the compensation is in effect
     */
    if (sphere) {
        comp = FwdCompData::fwd_make_comp_data(comp_data,
                                               coils,
                                               comp_coils,
                                               FwdBemModel::fwd_sphere_field,
                                               FwdBemModel::fwd_sphere_field_vec,
                                               FwdBemModel::fwd_sphere_field_grad,
                                               sphere_r0.data());
        if (!comp)
            return cleanup_fail();
        vec_field = FwdCompData::fwd_comp_field_vec;
    } else {
#ifdef TEST
        qInfo("Using differences.");
        comp = FwdCompData::fwd_make_comp_data(comp_data,
                                               coils, comp_coils,
                                               FwdBemModel::fwd_bem_field,
                                               nullptr,
                                               my_bem_field_grad,
                                               this);
#else
        comp = FwdCompData::fwd_make_comp_data(comp_data,
                                               coils,
                                               comp_coils,
                                               FwdBemModel::fwd_bem_field,
                                               nullptr,
                                               FwdBemModel::fwd_bem_field_grad,
                                               this);
#endif
        if (!comp)
            return cleanup_fail();
        /*
        * Field computation matrices...
        */
        qInfo("Composing the field computation matrix...");
        if (fwd_bem_specify_coils(coils) == FAIL)
            return cleanup_fail();
        qInfo("[done]");
 
        if (comp->set && comp->set->current) { /* Test just to specify confusing output */
            qInfo("Composing the field computation matrix (compensation coils)...");
            if (fwd_bem_specify_coils(comp->comp_coils.get()) == FAIL)
                return cleanup_fail();
            qInfo("[done]");
        }
        vec_field = nullptr;
    }
    field = FwdCompData::fwd_comp_field;
    field_grad = FwdCompData::fwd_comp_field_grad;
    client = comp.get();
    /*
       * Count the sources
       */
    for (k = 0, nsource = 0; k < nspace; k++)
        nsource += spaces[k]->nuse;
    /*
       * Allocate space for the solution
       */
    {
        int ncols = fixed_ori ? nsource : 3 * nsource;
        res_mat = Eigen::MatrixXf::Zero(nmeg, ncols);
    }
    if (bDoGrad) {
        int ncols = fixed_ori ? 3 * nsource : 3 * 3 * nsource;
        res_grad_mat = Eigen::MatrixXf::Zero(nmeg, ncols);
    }
    /*
     * Set up the argument for the field computation
     */
    one_arg = std::make_unique<FwdThreadArg>();
    one_arg->res = &res_mat;
    one_arg->res_grad = bDoGrad ? &res_grad_mat : nullptr;
    one_arg->off = 0;
    one_arg->coils_els = coils;
    one_arg->client = client;
    one_arg->s = nullptr;
    one_arg->fixed_ori = fixed_ori;
    one_arg->field_pot = field;
    one_arg->vec_field_pot = vec_field;
    one_arg->field_pot_grad = field_grad;
 
    if (nproc < 2)
        use_threads = false;
 
    if (use_threads) {
        int nthread = (fixed_ori || vec_field) ? nspace : 3 * nspace;
        std::vector<FwdThreadArg::UPtr> args;
        int stat;
        /*
        * We need copies to allocate separate workspace for each thread
        */
        if (fixed_ori || vec_field) {
            for (k = 0, off = 0; k < nthread; k++) {
                auto t_arg = FwdThreadArg::create_meg_multi_thread_duplicate(*one_arg, !sphere);
                t_arg->s = spaces[k].get();
                t_arg->off = off;
                off = fixed_ori ? off + spaces[k]->nuse : off + 3 * spaces[k]->nuse;
                args.push_back(std::move(t_arg));
            }
            qInfo("%d processors. I will use one thread for each of the %d source spaces.",
                  nproc, nspace);
        } else {
            for (k = 0, off = 0, q = 0; k < nspace; k++) {
                for (p = 0; p < 3; p++, q++) {
                    auto t_arg = FwdThreadArg::create_meg_multi_thread_duplicate(*one_arg, !sphere);
                    t_arg->s = spaces[k].get();
                    t_arg->off = off;
                    t_arg->comp = p;
                    args.push_back(std::move(t_arg));
                }
                off = fixed_ori ? off + spaces[k]->nuse : off + 3 * spaces[k]->nuse;
            }
            qInfo("%d processors. I will use %d threads : %d source spaces x 3 source components.",
                  nproc, nthread, nspace);
        }
        qInfo("Computing MEG at %d source locations (%s orientations)...",
              nsource, fixed_ori ? "fixed" : "free");
        /*
        * Ready to start the threads & Wait for them to complete
        */
        QtConcurrent::blockingMap(args, [](FwdThreadArg::UPtr& a) {
            meg_eeg_fwd_one_source_space(a.get());
        });
        /*
        * Check the results
        */
        for (k = 0, stat = OK; k < nthread; k++)
            if (args[k]->stat != OK) {
                stat = FAIL;
                break;
            }
        if (stat != OK)
            return cleanup_fail();
    } else {
        qInfo("Computing MEG at %d source locations (%s orientations, no threads)...",
              nsource, fixed_ori ? "fixed" : "free");
        for (k = 0, off = 0; k < nspace; k++) {
            one_arg->s = spaces[k].get();
            one_arg->off = off;
            meg_eeg_fwd_one_source_space(one_arg.get());
            if (one_arg->stat != OK)
                return cleanup_fail();
            off = fixed_ori ? off + one_arg->s->nuse : off + 3 * one_arg->s->nuse;
        }
    }
    qInfo("done.");
    {
        QStringList orig_names;
        for (k = 0; k < nmeg; k++)
            orig_names.append(coils->coils[k]->chname);
        names = orig_names;
    }
    one_arg.reset();
    comp.reset();
 
    // Store solution: res_mat is (nmeg x nsources), transpose to (nsources x nmeg)
    nrow = fixed_ori ? nsource : 3 * nsource;
    resp.nrow = nrow;
    resp.ncol = nmeg;
    resp.row_names = emptyList;
    resp.col_names = names;
    resp.data = res_mat.transpose().cast<double>();
    resp.transpose_named_matrix();
 
    if (bDoGrad && res_grad_mat.size() > 0) {
        nrow = fixed_ori ? 3 * nsource : 3 * 3 * nsource;
        resp_grad.nrow = nrow;
        resp_grad.ncol = nmeg;
        resp_grad.row_names = emptyList;
        resp_grad.col_names = names;
        resp_grad.data = res_grad_mat.transpose().cast<double>();
        resp_grad.transpose_named_matrix();
    }
    return OK;
}
 
//=============================================================================================================
 
int FwdBemModel::compute_forward_eeg(std::vector<std::unique_ptr<MNESourceSpace>>& spaces,
                                     FwdCoilSet* els,
                                     bool fixed_ori,
                                     FwdEegSphereModel* eeg_model,
                                     bool use_threads,
                                     FiffNamedMatrix& resp,
                                     FiffNamedMatrix& resp_grad,
                                     bool bDoGrad)
/*
     * Compute the EEG forward solution
     * Use either the sphere model or BEM in the calculations
     */
{
    const bool sphere = nsurf == 0;
 
    Eigen::MatrixXf res_mat;      /* The forward solution matrix (neeg x nsources) */
    Eigen::MatrixXf res_grad_mat; /* The gradient (neeg x 3*nsources) */
    MatrixXd matRes;
    MatrixXd matResGrad;
    int nrow = 0;
    fwdFieldFunc pot;          /* Computes the potentials for one dipole orientation */
    fwdVecFieldFunc vec_pot;   /* Computes the potentials for all dipole orientations */
    fwdFieldGradFunc pot_grad; /* Computes the potential and gradient with respect to dipole position
                                             * for one dipole orientation */
    int nsource;               /* Total number of sources */
    int nspace = static_cast<int>(spaces.size());
    int neeg = els->ncoil(); /* Number of channels */
    int k, p, q, off;
    QStringList names; /* Channel names */
    void* client;
    FwdThreadArg::UPtr one_arg;
    int nproc = QThread::idealThreadCount();
    QStringList emptyList;
    /*
       * Count the sources
       */
    for (k = 0, nsource = 0; k < nspace; k++)
        nsource += spaces[k]->nuse;
 
    if (sphere) {
        if (!eeg_model) {
            qCritical("EEG sphere model not defined.");
            return FAIL;
        }
        if (eeg_model->nfit == 0) {
            qInfo("Using the standard series expansion for a multilayer sphere model for EEG");
            pot = FwdEegSphereModel::fwd_eeg_multi_spherepot_coil1;
            vec_pot = nullptr;
            pot_grad = nullptr;
        } else {
            qInfo("Using the equivalent source approach in the homogeneous sphere for EEG");
            pot = FwdEegSphereModel::fwd_eeg_spherepot_coil;
            vec_pot = FwdEegSphereModel::fwd_eeg_spherepot_coil_vec;
            pot_grad = FwdEegSphereModel::fwd_eeg_spherepot_grad_coil;
        }
        client = eeg_model;
    } else {
        if (fwd_bem_specify_els(els) == FAIL)
            return FAIL;
        client = this;
        pot = fwd_bem_pot_els;
        vec_pot = nullptr;
#ifdef TEST
        qInfo("Using differences.");
        pot_grad = my_bem_pot_grad;
#else
        pot_grad = fwd_bem_pot_grad_els;
#endif
    }
    /*
     * Allocate space for the solution
     */
    {
        int ncols = fixed_ori ? nsource : 3 * nsource;
        res_mat = Eigen::MatrixXf::Zero(neeg, ncols);
    }
    if (bDoGrad) {
        if (!pot_grad) {
            qCritical("EEG gradient calculation function not available");
            return FAIL;
        }
        int ncols = fixed_ori ? 3 * nsource : 3 * 3 * nsource;
        res_grad_mat = Eigen::MatrixXf::Zero(neeg, ncols);
    }
    /*
       * Set up the argument for the field computation
       */
    one_arg = std::make_unique<FwdThreadArg>();
    one_arg->res = &res_mat;
    one_arg->res_grad = bDoGrad ? &res_grad_mat : nullptr;
    one_arg->off = 0;
    one_arg->coils_els = els;
    one_arg->client = client;
    one_arg->s = nullptr;
    one_arg->fixed_ori = fixed_ori;
    one_arg->field_pot = pot;
    one_arg->vec_field_pot = vec_pot;
    one_arg->field_pot_grad = pot_grad;
 
    if (nproc < 2)
        use_threads = false;
 
    if (use_threads) {
        int nthread = (fixed_ori || vec_pot) ? nspace : 3 * nspace;
        std::vector<FwdThreadArg::UPtr> args;
        int stat;
        /*
        * We need copies to allocate separate workspace for each thread
        */
        if (fixed_ori || vec_pot) {
            for (k = 0, off = 0; k < nthread; k++) {
                auto t_arg = FwdThreadArg::create_eeg_multi_thread_duplicate(*one_arg, !sphere);
                t_arg->s = spaces[k].get();
                t_arg->off = off;
                off = fixed_ori ? off + spaces[k]->nuse : off + 3 * spaces[k]->nuse;
                args.push_back(std::move(t_arg));
            }
            qInfo("%d processors. I will use one thread for each of the %d source spaces.", nproc, nspace);
        } else {
            for (k = 0, off = 0, q = 0; k < nspace; k++) {
                for (p = 0; p < 3; p++, q++) {
                    auto t_arg = FwdThreadArg::create_eeg_multi_thread_duplicate(*one_arg, !sphere);
                    t_arg->s = spaces[k].get();
                    t_arg->off = off;
                    t_arg->comp = p;
                    args.push_back(std::move(t_arg));
                }
                off = fixed_ori ? off + spaces[k]->nuse : off + 3 * spaces[k]->nuse;
            }
            qInfo("%d processors. I will use %d threads : %d source spaces x 3 source components.", nproc, nthread, nspace);
        }
        qInfo("Computing EEG at %d source locations (%s orientations)...",
              nsource, fixed_ori ? "fixed" : "free");
        /*
        * Ready to start the threads & Wait for them to complete
        */
        QtConcurrent::blockingMap(args, [](FwdThreadArg::UPtr& a) {
            meg_eeg_fwd_one_source_space(a.get());
        });
        /*
        * Check the results
        */
        for (k = 0, stat = OK; k < nthread; k++)
            if (args[k]->stat != OK) {
                stat = FAIL;
                break;
            }
        if (stat != OK)
            return FAIL;
    } else {
        qInfo("Computing EEG at %d source locations (%s orientations, no threads)...",
              nsource, fixed_ori ? "fixed" : "free");
        for (k = 0, off = 0; k < nspace; k++) {
            one_arg->s = spaces[k].get();
            one_arg->off = off;
            meg_eeg_fwd_one_source_space(one_arg.get());
            if (one_arg->stat != OK)
                return FAIL;
            off = fixed_ori ? off + one_arg->s->nuse : off + 3 * one_arg->s->nuse;
        }
    }
    qInfo("done.");
    {
        QStringList orig_names;
        for (k = 0; k < neeg; k++)
            orig_names.append(els->coils[k]->chname);
        names = orig_names;
    }
    one_arg.reset();
 
    // Store solution: res_mat is (neeg x nsources), transpose to (nsources x neeg)
    nrow = fixed_ori ? nsource : 3 * nsource;
    resp.nrow = nrow;
    resp.ncol = neeg;
    resp.row_names = emptyList;
    resp.col_names = names;
    resp.data = res_mat.transpose().cast<double>();
    resp.transpose_named_matrix();
 
    if (bDoGrad && res_grad_mat.size() > 0) {
        nrow = fixed_ori ? 3 * nsource : 3 * 3 * nsource;
        resp_grad.nrow = nrow;
        resp_grad.ncol = neeg;
        resp_grad.row_names = emptyList;
        resp_grad.col_names = names;
        resp_grad.data = res_grad_mat.transpose().cast<double>();
        resp_grad.transpose_named_matrix();
    }
    return OK;
}
 
//=============================================================================================================
 
int FwdBemModel::fwd_sphere_field(const Eigen::Vector3f& rd, const Eigen::Vector3f& Q, FwdCoilSet& coils, Eigen::Ref<Eigen::VectorXf> Bval, void* client) /* Client data will be the sphere model origin */
{
    /* This version uses Jukka Sarvas' field computation
         for details, see
 
         Jukka Sarvas:
 
         Basic mathematical and electromagnetic concepts
         of the biomagnetic inverse problem,
 
         Phys. Med. Biol. 1987, Vol. 32, 1, 11-22
 
         The formulas have been manipulated for efficient computation
         by Matti Hamalainen, February 1990
 
      */
    auto* r0 = static_cast<float*>(client);
    float a, a2, r, r2;
    float ar, ar0, rr0;
    float vr, ve, re, r0e;
    float F, g0, gr, sum;
    int j, k;
    FwdCoil* this_coil;
    int np;
 
    /*
       * Shift to the sphere model coordinates
       */
    Eigen::Vector3f myrd = rd - Eigen::Map<const Eigen::Vector3f>(r0);
    /*
       * Check for a dipole at the origin
       */
    for (k = 0; k < coils.ncoil(); k++)
        if (FWD_IS_MEG_COIL(coils.coils[k]->coil_class))
            Bval[k] = 0.0;
    r = myrd.norm();
    if (r > EPS) { /* The hard job */
 
        Eigen::Vector3f v = Q.cross(myrd);
 
        for (k = 0; k < coils.ncoil(); k++) {
            this_coil = coils.coils[k].get();
            if (FWD_IS_MEG_COIL(this_coil->coil_class)) {
                np = this_coil->np;
 
                for (j = 0, sum = 0.0; j < np; j++) {
                    Eigen::Map<const Eigen::Vector3f> this_pos_raw = this_coil->pos(j);
                    Eigen::Map<const Eigen::Vector3f> this_dir = this_coil->dir(j);
 
                    Eigen::Vector3f pos = this_pos_raw - Eigen::Map<const Eigen::Vector3f>(r0);
 
                    /* Vector from dipole to the field point */
 
                    Eigen::Vector3f a_vec = pos - myrd;
 
                    /* Compute the dot products needed */
 
                    a2 = a_vec.squaredNorm();
                    a = sqrt(a2);
 
                    if (a > 0.0) {
                        r2 = pos.squaredNorm();
                        r = sqrt(r2);
                        if (r > 0.0) {
                            rr0 = pos.dot(myrd);
                            ar = (r2 - rr0);
                            if (std::fabs(ar / (a * r) + 1.0) > CEPS) { /* There is a problem on the negative 'z' axis if the dipole location
                                                    * and the field point are on the same line */
                                ar0 = ar / a;
 
                                ve = v.dot(this_dir);
                                vr = v.dot(pos);
                                re = pos.dot(this_dir);
                                r0e = myrd.dot(this_dir);
 
                                /* The main ingredients */
 
                                F = a * (r * a + ar);
                                gr = a2 / r + ar0 + 2.0 * (a + r);
                                g0 = a + 2 * r + ar0;
 
                                /* Mix them together... */
 
                                sum = sum + this_coil->w[j] * (ve * F + vr * (g0 * r0e - gr * re)) / (F * F);
                            }
                        }
                    }
                } /* All points done */
                Bval[k] = MAG_FACTOR * sum;
            }
        }
    }
    return OK; /* Happy conclusion: this works always */
}
 
//=============================================================================================================
 
int FwdBemModel::fwd_sphere_field_vec(const Eigen::Vector3f& rd, FwdCoilSet& coils, Eigen::Ref<Eigen::MatrixXf> Bval, void* client) /* Client data will be the sphere model origin */
{
    /* This version uses Jukka Sarvas' field computation
         for details, see
 
         Jukka Sarvas:
 
         Basic mathematical and electromagnetic concepts
         of the biomagnetic inverse problem,
 
         Phys. Med. Biol. 1987, Vol. 32, 1, 11-22
 
         The formulas have been manipulated for efficient computation
         by Matti Hamalainen, February 1990
 
         The idea of matrix kernels is from
 
         Mosher, Leahy, and Lewis: EEG and MEG: Forward Solutions for Inverse Methods
 
         which has been simplified here using standard vector notation
 
      */
    auto* r0 = static_cast<float*>(client);
    float a, a2, r, r2;
    float ar, ar0, rr0;
    float re, r0e;
    float F, g0, gr, g;
    int j, k, p;
    FwdCoil* this_coil;
    int np;
    Eigen::Map<const Eigen::Vector3f> r0_vec(r0);
 
    /*
       * Shift to the sphere model coordinates
       */
    Eigen::Vector3f myrd = rd - r0_vec;
    /*
       * Check for a dipole at the origin
       */
    r = myrd.norm();
    for (k = 0; k < coils.ncoil(); k++) {
        this_coil = coils.coils[k].get();
        if (FWD_IS_MEG_COIL(this_coil->coil_class)) {
            if (r < EPS) {
                Bval(0, k) = Bval(1, k) = Bval(2, k) = 0.0;
            } else { /* The hard job */
 
                np = this_coil->np;
                Eigen::Vector3f sum = Eigen::Vector3f::Zero();
 
                for (j = 0; j < np; j++) {
                    Eigen::Map<const Eigen::Vector3f> this_pos_raw = this_coil->pos(j);
                    Eigen::Map<const Eigen::Vector3f> this_dir = this_coil->dir(j);
 
                    Eigen::Vector3f pos = this_pos_raw - r0_vec;
 
                    /* Vector from dipole to the field point */
 
                    Eigen::Vector3f a_vec = pos - myrd;
 
                    /* Compute the dot products needed */
 
                    a2 = a_vec.squaredNorm();
                    a = sqrt(a2);
 
                    if (a > 0.0) {
                        r2 = pos.squaredNorm();
                        r = sqrt(r2);
                        if (r > 0.0) {
                            rr0 = pos.dot(myrd);
                            ar = (r2 - rr0);
                            if (std::fabs(ar / (a * r) + 1.0) > CEPS) { /* There is a problem on the negative 'z' axis if the dipole location
                                                    * and the field point are on the same line */
 
                                /* The main ingredients */
 
                                ar0 = ar / a;
                                F = a * (r * a + ar);
                                gr = a2 / r + ar0 + 2.0 * (a + r);
                                g0 = a + 2 * r + ar0;
 
                                re = pos.dot(this_dir);
                                r0e = myrd.dot(this_dir);
                                Eigen::Vector3f v1 = myrd.cross(this_dir);
                                Eigen::Vector3f v2 = myrd.cross(pos);
 
                                g = (g0 * r0e - gr * re) / (F * F);
                                /*
                     * Mix them together...
                     */
                                sum += this_coil->w[j] * (v1 / F + v2 * g);
                            }
                        }
                    }
                } /* All points done */
                for (p = 0; p < 3; p++)
                    Bval(p, k) = MAG_FACTOR * sum[p];
            }
        }
    }
    return OK; /* Happy conclusion: this works always */
}
 
//=============================================================================================================
 
int FwdBemModel::fwd_sphere_field_grad(const Eigen::Vector3f& rd, const Eigen::Vector3f& Q, FwdCoilSet& coils, Eigen::Ref<Eigen::VectorXf> Bval, Eigen::Ref<Eigen::VectorXf> xgrad, Eigen::Ref<Eigen::VectorXf> ygrad, Eigen::Ref<Eigen::VectorXf> zgrad, void* client) /* Client data to be passed to some foward modelling routines */
/*
 * Compute the derivatives of the sphere model field with respect to
 * dipole coordinates
 */
{
    /* This version uses Jukka Sarvas' field computation
         for details, see
 
         Jukka Sarvas:
 
         Basic mathematical and electromagnetic concepts
         of the biomagnetic inverse problem,
 
         Phys. Med. Biol. 1987, Vol. 32, 1, 11-22
 
         The formulas have been manipulated for efficient computation
         by Matti Hamalainen, February 1990
 
         */
 
    float vr, ve, re, r0e;
    float F, g0, gr, result, G, F2;
 
    int j, k;
    float huu;
    FwdCoil* this_coil;
    int np;
    auto* r0 = static_cast<float*>(client);
    Eigen::Map<const Eigen::Vector3f> r0_vec(r0);
 
    int ncoil = coils.ncoil();
    /*
       * Shift to the sphere model coordinates
       */
    Eigen::Vector3f myrd = rd - r0_vec;
 
    /* Check for a dipole at the origin */
 
    float r = myrd.norm();
    for (k = 0; k < ncoil; k++) {
        if (FWD_IS_MEG_COIL(coils.coils[k]->coil_class)) {
            Bval[k] = 0.0;
            xgrad[k] = 0.0;
            ygrad[k] = 0.0;
            zgrad[k] = 0.0;
        }
    }
    if (r > EPS) { /* The hard job */
 
        Eigen::Vector3f v = Q.cross(myrd);
 
        for (k = 0; k < ncoil; k++) {
            this_coil = coils.coils[k].get();
 
            if (FWD_IS_MEG_COIL(this_coil->coil_class)) {
                np = this_coil->np;
 
                for (j = 0; j < np; j++) {
                    Eigen::Map<const Eigen::Vector3f> this_pos_raw = this_coil->pos(j);
                    /*
           * Shift to the sphere model coordinates
           */
                    Eigen::Vector3f pos = this_pos_raw - r0_vec;
 
                    Eigen::Map<const Eigen::Vector3f> this_dir = this_coil->dir(j);
 
                    /* Vector from dipole to the field point */
 
                    Eigen::Vector3f a_vec = pos - myrd;
 
                    /* Compute the dot and cross products needed */
 
                    float a2 = a_vec.squaredNorm();
                    float a = sqrt(a2);
                    float r2 = pos.squaredNorm();
                    r = sqrt(r2);
                    float rr0 = pos.dot(myrd);
                    float ar = (r2 - rr0) / a;
 
                    ve = v.dot(this_dir);
                    vr = v.dot(pos);
                    re = pos.dot(this_dir);
                    r0e = myrd.dot(this_dir);
 
                    /* eQ = this_dir x Q */
 
                    Eigen::Vector3f eQ = this_dir.cross(Q);
 
                    /* rQ = this_pos x Q */
 
                    Eigen::Vector3f rQ = pos.cross(Q);
 
                    /* The main ingredients */
 
                    F = a * (r * a + r2 - rr0);
                    F2 = F * F;
                    gr = a2 / r + ar + 2.0 * (a + r);
                    g0 = a + 2.0 * r + ar;
                    G = g0 * r0e - gr * re;
 
                    /* Mix them together... */
 
                    result = (ve * F + vr * G) / F2;
 
                    /* The computation of the gradient... */
 
                    huu = 2.0 + 2.0 * a / r;
                    Eigen::Vector3f ga = -a_vec / a;
                    Eigen::Vector3f gar = -(ga * ar + pos) / a;
                    Eigen::Vector3f gg0 = ga + gar;
                    Eigen::Vector3f ggr = huu * ga + gar;
                    Eigen::Vector3f gFF = ga / a - (r * a_vec + a * pos) / F;
                    Eigen::Vector3f gresult = -2.0f * result * gFF + (eQ + gFF * ve) / F +
                        (rQ * G + vr * (gg0 * r0e + g0 * this_dir - ggr * re)) / F2;
 
                    Bval[k] = Bval[k] + this_coil->w[j] * result;
                    xgrad[k] = xgrad[k] + this_coil->w[j] * gresult[0];
                    ygrad[k] = ygrad[k] + this_coil->w[j] * gresult[1];
                    zgrad[k] = zgrad[k] + this_coil->w[j] * gresult[2];
                }
                Bval[k] = MAG_FACTOR * Bval[k];
                xgrad[k] = MAG_FACTOR * xgrad[k];
                ygrad[k] = MAG_FACTOR * ygrad[k];
                zgrad[k] = MAG_FACTOR * zgrad[k];
            }
        }
    }
    return OK; /* Happy conclusion: this works always */
}
 
//=============================================================================================================
 
int FwdBemModel::fwd_mag_dipole_field(const Eigen::Vector3f& rm, const Eigen::Vector3f& M, FwdCoilSet& coils, Eigen::Ref<Eigen::VectorXf> Bval, [[maybe_unused]] void* client) /* Client data will be the sphere model origin */
/*
 * This is for a specific dipole component
 */
{
    int j, k, np;
    FwdCoil* this_coil;
    float sum, dist, dist2, dist5;
 
    Bval.setZero();
    for (k = 0; k < coils.ncoil(); k++) {
        this_coil = coils.coils[k].get();
        if (FWD_IS_MEG_COIL(this_coil->coil_class)) {
            np = this_coil->np;
            /*
           * Go through all points
           */
            for (j = 0, sum = 0.0; j < np; j++) {
                Eigen::Map<const Eigen::Vector3f> dir = this_coil->dir(j);
                Eigen::Vector3f diff = this_coil->pos(j) - rm;
                dist = diff.norm();
                if (dist > EPS) {
                    dist2 = dist * dist;
                    dist5 = dist2 * dist2 * dist;
                    sum = sum + this_coil->w[j] * (3 * M.dot(diff) * diff.dot(dir) - dist2 * M.dot(dir)) / dist5;
                }
            } /* All points done */
            Bval[k] = MAG_FACTOR * sum;
        } else if (this_coil->coil_class == FWD_COILC_EEG)
            Bval[k] = 0.0;
    }
    return OK;
}
 
//=============================================================================================================
 
int FwdBemModel::fwd_mag_dipole_field_vec(const Eigen::Vector3f& rm, FwdCoilSet& coils, Eigen::Ref<Eigen::MatrixXf> Bval, [[maybe_unused]] void* client) /* Client data will be the sphere model origin */
/*
 * This is for all dipole components
 * For EEG this produces a zero result
 */
{
    int j, k, p, np;
    FwdCoil* this_coil;
    float dist, dist2, dist5;
 
    Bval.setZero();
    for (k = 0; k < coils.ncoil(); k++) {
        this_coil = coils.coils[k].get();
        if (FWD_IS_MEG_COIL(this_coil->coil_class)) {
            np = this_coil->np;
            Eigen::Vector3f sum = Eigen::Vector3f::Zero();
            /*
           * Go through all points
           */
            for (j = 0; j < np; j++) {
                Eigen::Map<const Eigen::Vector3f> dir = this_coil->dir(j);
                Eigen::Vector3f diff = this_coil->pos(j) - rm;
                dist = diff.norm();
                if (dist > EPS) {
                    dist2 = dist * dist;
                    dist5 = dist2 * dist2 * dist;
                    for (p = 0; p < 3; p++)
                        sum[p] = sum[p] + this_coil->w[j] * (3 * diff[p] * diff.dot(dir) - dist2 * dir[p]) / dist5;
                }
            } /* All points done */
            for (p = 0; p < 3; p++)
                Bval(p, k) = MAG_FACTOR * sum[p];
        } else if (this_coil->coil_class == FWD_COILC_EEG) {
            for (p = 0; p < 3; p++)
                Bval(p, k) = 0.0;
        }
    }
    return OK;
}