/*
 * =============================================================
 * FEM Kernel for diffusioin equation
 * Author: Qianqian Fang(fangq < at > nmr.mgh.harvard.edu)
 * Date: 2007/11/21
 * Version: 0.5.0
 *
 * Model:
 *    [A]*c=-OC
 * where [A]=<D delPhi_i,delPhi_j>
 * OC is the vector for oxygen consumption at each node
 * boundary condition is dot(delc,normal)=0
 * =============================================================
 */

#include <string.h>
#include <exception>
#include <stdio.h>

#include "mex.h"
#include "rbfemmatrix.h"

#define N137_REFF 0.468407140351385

void mexFunction(int nlhs,       mxArray* plhs[],
                 int nrhs, const mxArray* prhs[]) {
    Config cfg;
    tetmesh mesh;
    Forward femdata;
    Jacobian jac;
    int isjacobian = 0;
    int nfields, ifield;

    if (nrhs < 1) {
        rb3_usage();
        return;
    }

    /* Get the matrices from the input list */

    if (!mxIsStruct(prhs[0])) {
        mexErrMsgTxt("Input must be a structure.");
    }

    try {
        config_init(&cfg);
        mesh_init(&mesh);
        forward_init(&femdata, &mesh);
        jacobian_init(&jac);

        nfields = mxGetNumberOfFields(prhs[0]);

        for (ifield = 0; ifield < nfields; ifield++) { /* how many input struct fields */
            mxArray* tmp = mxGetFieldByNumber(prhs[0], 0, ifield);

            if (tmp == NULL) {
                continue;
            }

            mcx_set_field(prhs[0], tmp, ifield, &cfg, &mesh, &jac);
        }

        if (mesh.nn <= 0 || mesh.ne <= 0) {
            mexErrMsgTxt("dimensions of the input or output parameters are incorrect.");
        }

        if (nrhs > 1) {
            const mwSize* arraydim;
            arraydim = mxGetDimensions(prhs[1]);

            if (arraydim[1] < 3 || arraydim[0] == 0) {
                MEXERROR("sd must have at least 3 columns");
            }

            jac.sd = mxGetPr(prhs[1]);
            jac.nsd = arraydim[0];
            jac.nsdcol = arraydim[1];
            isjacobian = nlhs;

            if (nrhs <= 2) {
                MEXERROR("sd and phi must be given to calculate Jacobians");
            }

            arraydim = mxGetDimensions(prhs[2]);

            if (arraydim[0] != (const mwSize)mesh.nn) {
                MEXERROR("phi's row number must be the same as the number of node");
            }

            jac.Phir = mxGetPr(prhs[2]); // phi: [mesh.nn x noptodes]

            if (mxIsComplex(prhs[2])) {
                jac.Phii = mxGetPi(prhs[2]);    // phi: [mesh.nn x noptodes]
            }

            if (nrhs > 3) {
                arraydim = mxGetDimensions(prhs[3]);

                if (arraydim[0] != (const mwSize)mesh.ne || arraydim[1] != 10) {
                    MEXERROR("deldotdel must have be a 2D array with size: elem number x 10");
                }

                jac.deldotdel = mxGetPr(prhs[3]); // phi: [mesh.nn x jac.nsd]
            }

            if (nrhs > 4) {
                jac.isnodal = ((int) * (mxGetPr(prhs[4])));
            }

            if (nrhs > 8) {
                jac.isnodal = 1; // build nodal Jmua and Jd on a coarse mesh
                arraydim = mxGetDimensions(prhs[5]);

                if (arraydim[0]*arraydim[1] != (const mwSize)mesh.nn) {
                    MEXERROR("elemid must have the same length has the forward mesh node number");
                }

                jac.elemid = mxGetPr(prhs[5]);

                arraydim = mxGetDimensions(prhs[6]);

                if (arraydim[0] != (const mwSize)mesh.nn && arraydim[1] != 4) {
                    MEXERROR("elembary must have the same lengt has the forward mesh node number and 4 columns");
                }

                jac.elembary = mxGetPr(prhs[6]);
                jac.nn = ((int) * (mxGetPr(prhs[7])));

                if (jac.nn > mesh.nn) {
                    MEXERROR("reconstruction mesh must be coarser than or at most the same as the forward mesh");
                }

                arraydim = mxGetDimensions(prhs[8]);

                if (arraydim[1] != 4) {
                    MEXERROR("relem must have 4 columns");
                }

                jac.ne = arraydim[0];
                jac.relem = mxGetPr(prhs[8]);
            }
        }

        /* Assign a pointer to the output. */
        if (cfg.omega > 0.f) {
            if (nlhs >= 1) {
                if (isjacobian == 0) {
                    plhs[0] = mxCreateDoubleMatrix(1, mesh.nn, mxCOMPLEX);
                    femdata.Di = mxGetPi(plhs[0]);
                } else {
                    plhs[0] = mxCreateDoubleMatrix(jac.nsd, (jac.nn ? jac.nn : (jac.isnodal ? mesh.nn : mesh.ne)), mxCOMPLEX);
                    jac.Jmuai = mxGetPi(plhs[0]);
                }
            }

            if (nlhs >= 2) {
                if (isjacobian == 0) {
                    plhs[1] = mxCreateDoubleMatrix(1, mesh.ntot, mxCOMPLEX);
                    femdata.Ai = mxGetPi(plhs[1]);
                } else {
                    plhs[1] = mxCreateDoubleMatrix(jac.nsd, (jac.nn ? jac.nn : (jac.isnodal ? mesh.nn : mesh.ne)), mxCOMPLEX);
                    jac.Jdi = mxGetPi(plhs[1]);
                }
            }
        } else {
            if (isjacobian == 0) {
                if (nlhs >= 1) {
                    plhs[0] = mxCreateDoubleMatrix(1, mesh.nn, mxREAL);
                }

                if (nlhs >= 2) {
                    plhs[1] = mxCreateDoubleMatrix(1, mesh.ntot, mxREAL);
                }
            } else {
                if (nlhs >= 1) {
                    plhs[0] = mxCreateDoubleMatrix(jac.nsd, (jac.nn ? jac.nn : (jac.isnodal ? mesh.nn : mesh.ne)), mxREAL);
                }

                if (nlhs >= 2) {
                    plhs[1] = mxCreateDoubleMatrix(jac.nsd, (jac.nn ? jac.nn : (jac.isnodal ? mesh.nn : mesh.ne)), mxREAL);
                }
            }
        }

        if (isjacobian == 0) {
            if (nlhs >= 1) {
                femdata.Dr = mxGetPr(plhs[0]);
            }

            if (nlhs >= 2) {
                femdata.Ar = mxGetPr(plhs[1]);
            }
        } else {
            if (nlhs >= 1) {
                jac.Jmuar = mxGetPr(plhs[0]);
            }

            if (nlhs >= 2) {
                jac.Jdr = mxGetPr(plhs[1]);
            }
        }

        if (nlhs >= 3) {
            plhs[2] = mxCreateDoubleMatrix(10, mesh.ne, mxREAL);
            femdata.deldotdel = mxGetPr(plhs[2]);
        } else {
            femdata.deldotdel = (double*)calloc(mesh.ne, 10 * sizeof(double));
        }

        if (isjacobian == 0) {
            rb_deldotdel(&cfg, &mesh, &femdata);

            if (mesh.ntype == mesh.ne) {
                rb_femmatrix_elem (&cfg, &mesh, &femdata);
            } else if (mesh.ntype == mesh.nn) {
                rb_femmatrix_nodal(&cfg, &mesh, &femdata);
            } else {
                MEXERROR("cfg.seg must have a length that matches the length of either cfg.node or cfg.elem");
            }

            rb_fem_bc(&cfg, &mesh, &femdata);
        } else {
            rb_femjacobian(&cfg, &mesh, &jac);
        }

        if (nlhs < 3 && femdata.deldotdel) {
            delete (femdata.deldotdel);
        }

        /** \subsection sclean End the simulation */
        mesh_clear(&mesh);

    } catch (const char* err) {
        mexPrintf("Error: %s\n", err);
    } catch (const std::exception& err) {
        mexPrintf("C++ Error: %s\n", err.what());
    } catch (...) {
        mexPrintf("Unknown Exception");
    }
}

void mcx_set_field(const mxArray* root, const mxArray* item, int idx, Config* cfg, tetmesh* mesh, Jacobian* jac) {
    const char* name = mxGetFieldNameByNumber(root, idx);
    const mwSize* arraydim;
    int i, j;

    GET_1ST_FIELD(cfg, omega)
    GET_VEC34_FIELD(cfg, srcpos)
    GET_VEC34_FIELD(cfg, srcdir)
    GET_ONE_FIELD(mesh, e0)
    GET_ONE_FIELD(mesh, isreoriented)
    GET_ONE_FIELD(cfg, reff)
    else if (strcmp(name, "node") == 0) {
        arraydim = mxGetDimensions(item);

        if (arraydim[0] <= 0 || arraydim[1] != 3) {
            MEXERROR("the 'node' field must have 3 columns (x,y,z)");
        }

        double* val = mxGetPr(item);
        mesh->nn = arraydim[0];

        if (mesh->node) {
            free(mesh->node);
        }

        mesh->node = (float3*)calloc(sizeof(float3), mesh->nn);

        for (j = 0; j < 3; j++)
            for (i = 0; i < mesh->nn; i++) {
                ((double*)(&mesh->node[i]))[j] = val[j * mesh->nn + i];
            }

        PRINTF(("rbm.nn=%d;\n", mesh->nn));
    } else if (strcmp(name, "elem") == 0) {
        arraydim = mxGetDimensions(item);

        if (arraydim[0] <= 0 || arraydim[1] != 4) {
            MEXERROR("the 'elem' field must have 4 columns (e1,e2,e3,e4)");
        }

        double* val = mxGetPr(item);
        mesh->ne = arraydim[0];

        if (mesh->elem) {
            free(mesh->elem);
        }

        mesh->elem = (int4*)calloc(sizeof(int4), mesh->ne);

        for (j = 0; j < 4; j++)
            for (i = 0; i < mesh->ne; i++) {
                ((int*)(&mesh->elem[i]))[j] = (int)val[j * mesh->ne + i] - 1;
            }

        PRINTF(("rbm.ne=%d;\n", mesh->ne));
    } else if (strcmp(name, "face") == 0) {
        arraydim = mxGetDimensions(item);

        if (arraydim[0] <= 0 || arraydim[1] != 3) {
            MEXERROR("the 'face' field must have 3 columns (e1,e2,e3)");
        }

        double* val = mxGetPr(item);
        mesh->nf = arraydim[0];

        if (mesh->face) {
            free(mesh->face);
        }

        mesh->face = (int3*)calloc(sizeof(int3), mesh->nf);

        for (j = 0; j < 3; j++)
            for (i = 0; i < mesh->nf; i++) {
                ((int*)(&mesh->face[i]))[j] = (int)val[j * mesh->nf + i] - 1;
            }

        PRINTF(("rbm.nf=%d;\n", mesh->nf));
    } else if (strcmp(name, "seg") == 0) {
        arraydim = mxGetDimensions(item);

        if (MAX(arraydim[0], arraydim[1]) == 0) {
            MEXERROR("the 'seg' field can not be empty");
        }

        double* val = mxGetPr(item);
        mesh->ntype = MAX(arraydim[0], arraydim[1]);

        if (mesh->type) {
            free(mesh->type);
        }

        mesh->type = (int*)malloc(sizeof(int ) * mesh->ntype);

        for (i = 0; i < mesh->ntype; i++) {
            mesh->type[i] = val[i];
        }

        PRINTF(("rbm.seg=<%d>;\n", mesh->ntype));
    } else if (strcmp(name, "evol") == 0) {
        arraydim = mxGetDimensions(item);

        if (MAX(arraydim[0], arraydim[1]) == 0) {
            MEXERROR("the 'evol' field can not be empty");
        }

        double* val = mxGetPr(item);
        mesh->ne = MAX(arraydim[0], arraydim[1]);

        if (mesh->evol) {
            free(mesh->evol);
        }

        mesh->evol = (double*)malloc(sizeof(double) * mesh->ne);

        for (i = 0; i < mesh->ne; i++) {
            mesh->evol[i] = val[i];
        }

        PRINTF(("rbm.evol=%d;\n", mesh->ne));
    } else if (strcmp(name, "area") == 0) {
        arraydim = mxGetDimensions(item);

        if (MAX(arraydim[0], arraydim[1]) == 0) {
            MEXERROR("the 'evol' field can not be empty");
        }

        double* val = mxGetPr(item);
        mesh->nf = MAX(arraydim[0], arraydim[1]);

        if (mesh->area) {
            free(mesh->area);
        }

        mesh->area = (double*)malloc(sizeof(double) * mesh->nf);

        for (i = 0; i < mesh->nf; i++) {
            mesh->area[i] = val[i];
        }

        PRINTF(("rbm.area=%d;\n", mesh->nf));
    } else if (strcmp(name, "idxcount") == 0) {
        arraydim = mxGetDimensions(item);

        if (MAX(arraydim[0], arraydim[1]) == 0) {
            MEXERROR("the 'evol' field can not be empty");
        }

        double* val = mxGetPr(item);
        mesh->nn = MAX(arraydim[0], arraydim[1]);

        if (mesh->idxcount) {
            free(mesh->idxcount);
        }

        mesh->idxcount = (int*)malloc(sizeof(int) * mesh->nn);

        for (i = 0; i < mesh->nn; i++) {
            mesh->idxcount[i] = val[i];
        }

        PRINTF(("rbm.idxcount=%d;\n", mesh->nn));
    } else if (strcmp(name, "idxsum") == 0) {
        arraydim = mxGetDimensions(item);

        if (MAX(arraydim[0], arraydim[1]) == 0) {
            MEXERROR("the 'evol' field can not be empty");
        }

        double* val = mxGetPr(item);
        mesh->nn = MAX(arraydim[0], arraydim[1]);

        if (mesh->idxsum) {
            free(mesh->idxsum);
        }

        mesh->idxsum = (int*)malloc(sizeof(int) * mesh->nn);

        for (i = 0; i < mesh->nn; i++) {
            mesh->idxsum[i] = val[i];
        }

        PRINTF(("rbm.idxsum=%d;\n", mesh->nn));
    } else if (strcmp(name, "rows") == 0) {
        arraydim = mxGetDimensions(item);

        if (MAX(arraydim[0], arraydim[1]) == 0) {
            MEXERROR("the 'rows' field can not be empty");
        }

        double* val = mxGetPr(item);
        mesh->ntot = MAX(arraydim[0], arraydim[1]);

        if (mesh->rows) {
            free(mesh->rows);
        }

        mesh->rows = (int*)malloc(sizeof(int) * mesh->ntot);

        for (i = 0; i < mesh->ntot; i++) {
            mesh->rows[i] = (int)val[i] - 1;
        }

        PRINTF(("rbm.rows=%d;\n", mesh->ntot));
    } else if (strcmp(name, "cols") == 0) {
        arraydim = mxGetDimensions(item);

        if (MAX(arraydim[0], arraydim[1]) == 0) {
            MEXERROR("the 'cols' field can not be empty");
        }

        double* val = mxGetPr(item);
        mesh->ntot = MAX(arraydim[0], arraydim[1]);

        if (mesh->cols) {
            free(mesh->cols);
        }

        mesh->cols = (int*)malloc(sizeof(int) * mesh->ntot);

        for (i = 0; i < mesh->ntot; i++) {
            mesh->cols[i] = (int)val[i] - 1;
        }

        PRINTF(("rbm.cols=%d;\n", mesh->ntot));
    } else if (strcmp(name, "prop") == 0) {
        arraydim = mxGetDimensions(item);

        if (arraydim[0] > 0 && arraydim[1] != 4) {
            MEXERROR("the 'prop' field must have 4 columns (mua,mus,g,n)");
        }

        double* val = mxGetPr(item);
        mesh->prop = arraydim[0] - 1;

        if (mesh->med) {
            free(mesh->med);
        }

        mesh->med = (medium*)calloc(sizeof(medium), mesh->prop + 1);

        for (j = 0; j < 4; j++)
            for (i = 0; i <= mesh->prop; i++) {
                ((double*)(&mesh->med[i]))[j] = val[j * (mesh->prop + 1) + i];
            }

        PRINTF(("rbm.prop=%d;\n", mesh->prop));
    } else if (strcmp(name, "deldotdel") == 0) {
        if (jac->deldotdel == NULL) {
            arraydim = mxGetDimensions(item);

            if (arraydim[1] != 10) {
                MEXERROR("deldotphi must have be a 2D array with size: node number x 10");
            }

            jac->deldotdel = mxGetPr(item); // phi: [mesh.nn x jac.nsd]
            PRINTF(("rbm.deldotdel=<%d,10>;\n", arraydim[0]));
        }
    } else if (strcmp(name, "detpos") == 0) {
        // do nothing
    } else if (strcmp(name, "srctype") == 0) {
        // do nothing
    } else if (strcmp(name, "srcparam1") == 0) {
        // do nothing
    } else if (strcmp(name, "srcparam2") == 0) {
        // do nothing
    } else if (strcmp(name, "srcpattern") == 0) {
        // do nothing
    } else if (strcmp(name, "dettype") == 0) {
        // do nothing
    } else if (strcmp(name, "detparam1") == 0) {
        // do nothing
    } else if (strcmp(name, "detparam2") == 0) {
        // do nothing
    } else if (strcmp(name, "detpattern") == 0) {
        // do nothing
    } else if (strcmp(name, "srcpos0") == 0) {
        // do nothing
    } else if (strcmp(name, "detpos0") == 0) {
        // do nothing
    } else if (strcmp(name, "detdir") == 0) {
        // do nothing
    } else if (strcmp(name, "nvol") == 0) {
        // do nothing
    } else if (strcmp(name, "mua") == 0) {
        // do nothing
    } else if (strcmp(name, "musp0") == 0) {
        // do nothing
    } else {
        printf("WARNING: redundant field '%s'\n", name);
    }

    mexEvalString("pause(.0001);");
}

void config_init(Config* cfg) {
    cfg->omega = 0.f;
    cfg->reff = N137_REFF;
    memset(&(cfg->srcpos), 0, sizeof(float4));
    memset(&(cfg->srcdir), 0, sizeof(float4));
}

void forward_init(Forward* fem, tetmesh* m) {
    fem->mesh = m;
    fem->Ar = NULL;
    fem->Ai = NULL;
    fem->Dr = NULL;
    fem->Di = NULL;
    fem->deldotdel = NULL;
}

void jacobian_init(Jacobian* jac) {
    jac->isnodal = 1;
    jac->nsd = 0;
    jac->nsdcol = 0;
    jac->nn = 0;
    jac->Jmuar = NULL;
    jac->Jmuai = NULL;
    jac->Jdr = NULL;
    jac->Jdi = NULL;
    jac->Phir = NULL;
    jac->Phii = NULL;
    jac->sd = NULL;
    jac->deldotdel = NULL;
    jac->elemid = NULL;
    jac->elembary = NULL;
    jac->relem = NULL;
}

void mesh_init(tetmesh* mesh) {
    mesh->nn = 0;
    mesh->ne = 0;
    mesh->nf = 0;
    mesh->ntype = 0;
    mesh->prop = 0;
    mesh->node = NULL;
    mesh->elem = NULL;
    mesh->face = NULL;
    mesh->type = NULL;
    mesh->med = NULL;
    mesh->evol = NULL;
    mesh->area = NULL;
    mesh->rows = NULL;
    mesh->cols = NULL;
    mesh->idxcount = NULL;
    mesh->idxsum = NULL;
}

void mesh_clear(tetmesh* mesh) {
    mesh->nn = 0;
    mesh->ne = 0;
    mesh->nf = 0;
    mesh->ntype = 0;
    mesh->ntot = 0;
    mesh->prop = 0;

    if (mesh->node) {
        free(mesh->node);
        mesh->node = NULL;
    }

    if (mesh->elem) {
        free(mesh->elem);
        mesh->elem = NULL;
    }

    if (mesh->face) {
        free(mesh->face);
        mesh->face = NULL;
    }

    if (mesh->area) {
        free(mesh->area);
        mesh->area = NULL;
    }

    if (mesh->type) {
        free(mesh->type);
        mesh->type = NULL;
    }

    if (mesh->med) {
        free(mesh->med);
        mesh->med = NULL;
    }

    if (mesh->evol) {
        free(mesh->evol);
        mesh->evol = NULL;
    }

    if (mesh->rows) {
        free(mesh->rows);
        mesh->rows = NULL;
    }

    if (mesh->cols) {
        free(mesh->cols);
        mesh->cols = NULL;
    }

    if (mesh->idxcount) {
        free(mesh->idxcount);
        mesh->idxcount = NULL;
    }

    if (mesh->idxsum) {
        free(mesh->idxsum);
        mesh->idxsum = NULL;
    }
}

void rb_femjacobian(Config* cfg, tetmesh* mesh, Jacobian* jac) {
    int t, sd, sid, rid, i, j;
    int pairs[3][10] = {{0, 0, 0, 1, 1, 2}, {1, 2, 3, 2, 3, 3}, {1, 2, 3, 5, 6, 8}}; //pairs[0]<->[1], local node pairs, [3] pos in deldotdel
    int inode[4] = {0, 4, 7, 9}; // position in deldotdel for diagonal

    int colnum = (jac->nn ? jac->nn : (jac->isnodal ? mesh->nn : mesh->ne));

    if (jac->Jmuar) {
        memset(jac->Jmuar, 0, jac->nsd * colnum * sizeof(double));
    }

    if (jac->Jmuai) {
        memset(jac->Jmuai, 0, jac->nsd * colnum * sizeof(double));
    }

    if (jac->Jdr) {
        memset(jac->Jdr, 0, jac->nsd * colnum * sizeof(double));
    }

    if (jac->Jdi) {
        memset(jac->Jdi, 0, jac->nsd * colnum * sizeof(double));
    }

    // loop over all elements
    for (t = 0; t < mesh->ne; t++) {
        int* ee = (int*)(mesh->elem + t);

        // first calculate real-part of Jmua and Jd
        for (sd = 0; sd < jac->nsd; sd++) { // loop over sd pairs
            double jdr = 0.0, jmuar = 0.0, dtemp;
            sid = (int)jac->sd[sd] - 1;
            rid = (int)jac->sd[sd + jac->nsd] - 1;

            // accummulate diagonal terms
            for (i = 0; i < 4; i++) {
                dtemp = jac->Phir[sid * mesh->nn + ee[i]] * jac->Phir[rid * mesh->nn + ee[i]];

                if (jac->Phii) {
                    dtemp -= jac->Phii[sid * mesh->nn + ee[i]] * jac->Phii[rid * mesh->nn + ee[i]];
                }

                jmuar += dtemp;
                jdr += jac->deldotdel[inode[i] * mesh->ne + t] * dtemp;
            }

            // accummulate off-diagonal terms
            for (i = 0; i < 6; i++) {
                dtemp = jac->Phir[sid * mesh->nn + ee[pairs[0][i]]] * jac->Phir[rid * mesh->nn + ee[pairs[1][i]]] +
                        jac->Phir[rid * mesh->nn + ee[pairs[0][i]]] * jac->Phir[sid * mesh->nn + ee[pairs[1][i]]];

                if (jac->Phii)
                    dtemp -= jac->Phii[sid * mesh->nn + ee[pairs[0][i]]] * jac->Phii[rid * mesh->nn + ee[pairs[1][i]]] +
                             jac->Phii[rid * mesh->nn + ee[pairs[0][i]]] * jac->Phii[sid * mesh->nn + ee[pairs[1][i]]];

                jmuar += dtemp * 0.5;
                jdr += jac->deldotdel[pairs[2][i] * mesh->ne + t] * dtemp;
            }

            jmuar *= -0.1;
            jdr = -jdr;

            if (jac->nn && jac->nn < mesh->nn) {
                jmuar *= 0.25 * mesh->evol[t];

                for (i = 0; i < 4; i++) {
                    int eid = (int)(jac->elemid[ee[i]] - 0.5);

                    if (jac->elemid[ee[i]] != jac->elemid[ee[i]]) { /*if elemid is nan, the forward node is outside of recon mesh*/
                        continue;
                    }

                    for (j = 0; j < 4; j++) {
                        jac->Jmuar[(int)(jac->relem[j * jac->ne + eid] - 0.5)*jac->nsd + sd] += jmuar * jac->elembary[j * mesh->nn + ee[i]];
                    }
                }
            } else if (jac->isnodal) {
                for (i = 0; i < 4; i++) {
                    jac->Jmuar[ee[i]*jac->nsd + sd] += jmuar * 0.25 * mesh->evol[t];
                }
            } else { // element-based jacobian
                jac->Jmuar[t * jac->nsd + sd] += jmuar * mesh->evol[t];
            }

            if (jac->Jdr) {
                if (jac->nn && jac->nn < mesh->nn) { // support dual-mesh
                    jdr *= 0.25;

                    for (i = 0; i < 4; i++) {
                        int eid = (int)(jac->elemid[ee[i]] - 0.5);

                        if (jac->elemid[ee[i]] != jac->elemid[ee[i]]) { /*if elemid is nan, the forward node is outside of recon mesh*/
                            continue;
                        }

                        for (j = 0; j < 4; j++) {
                            jac->Jdr[(int)(jac->relem[j * jac->ne + eid] - 0.5)*jac->nsd + sd] += jdr * jac->elembary[j * mesh->nn + ee[i]];
                        }
                    }
                } else if (jac->isnodal) {
                    for (i = 0; i < 4; i++) {
                        jac->Jdr[ee[i]*jac->nsd + sd] += jdr * 0.25;
                    }
                } else { // element-based jacobian
                    jac->Jdr[t * jac->nsd + sd] += jdr;
                }
            }
        }
    }

    // if measurement has complex val, then calculate the imag part of J
    if (jac->Phii) {
        for (t = 0; t < mesh->ne; t++) {
            int* ee = (int*)(mesh->elem + t);

            // first calculate real-part of Jmua and Jd
            for (sd = 0; sd < jac->nsd; sd++) { // loop over sd pairs
                double jdi = 0.0, jmuai = 0.0, dtemp;
                sid = (int)jac->sd[sd] - 1;
                rid = (int)jac->sd[sd + jac->nsd] - 1;

                // accummulate diagonal terms
                for (i = 0; i < 4; i++) {
                    dtemp = jac->Phir[sid * mesh->nn + ee[i]] * jac->Phii[rid * mesh->nn + ee[i]];

                    if (jac->Phii) {
                        dtemp += jac->Phii[sid * mesh->nn + ee[i]] * jac->Phir[rid * mesh->nn + ee[i]];
                    }

                    jmuai += dtemp;
                    jdi += jac->deldotdel[inode[i] * mesh->ne + t] * dtemp;
                }

                // accummulate off-diagonal terms
                for (i = 0; i < 6; i++) {
                    dtemp = jac->Phir[sid * mesh->nn + ee[pairs[0][i]]] * jac->Phii[rid * mesh->nn + ee[pairs[1][i]]] +
                            jac->Phir[rid * mesh->nn + ee[pairs[0][i]]] * jac->Phii[sid * mesh->nn + ee[pairs[1][i]]];

                    if (jac->Phii)
                        dtemp += jac->Phii[sid * mesh->nn + ee[pairs[0][i]]] * jac->Phir[rid * mesh->nn + ee[pairs[1][i]]] +
                                 jac->Phii[rid * mesh->nn + ee[pairs[0][i]]] * jac->Phir[sid * mesh->nn + ee[pairs[1][i]]];

                    jmuai += dtemp * 0.5;
                    jdi += jac->deldotdel[pairs[2][i] * mesh->ne + t] * dtemp;
                }

                jmuai *= -0.1;
                jdi = -jdi;

                if (jac->nn && jac->nn < mesh->nn) {
                    jmuai *= 0.25 * mesh->evol[t];

                    for (i = 0; i < 4; i++) {
                        int eid = (int)(jac->elemid[ee[i]] - 0.5);

                        if (jac->elemid[ee[i]] != jac->elemid[ee[i]]) { /*if elemid is nan, the forward node is outside of recon mesh*/
                            continue;
                        }

                        for (j = 0; j < 4; j++) {
                            jac->Jmuai[(int)(jac->relem[j * jac->ne + eid] - 0.5)*jac->nsd + sd] += jmuai * jac->elembary[j * mesh->nn + ee[i]];
                        }
                    }
                } else if (jac->isnodal) { // nodal-based Jacobian
                    for (i = 0; i < 4; i++) {
                        jac->Jmuai[ee[i]*jac->nsd + sd] += jmuai * 0.25 * mesh->evol[t];
                    }
                } else { // element-based jacobian
                    jac->Jmuai[t * jac->nsd + sd] += jmuai * mesh->evol[t];
                }

                if (jac->Jdi) {
                    if (jac->nn && jac->nn < mesh->nn) { // support dual-mesh
                        jdi *= 0.25;

                        for (i = 0; i < 4; i++) {
                            int eid = (int)(jac->elemid[ee[i]] - 0.5);

                            if (jac->elemid[ee[i]] != jac->elemid[ee[i]]) { /*if elemid is nan, the forward node is outside of recon mesh*/
                                continue;
                            }

                            for (j = 0; j < 4; j++) {
                                jac->Jdi[(int)(jac->relem[j * jac->ne + eid] - 0.5)*jac->nsd + sd] += jdi * jac->elembary[j * mesh->nn + ee[i]];
                            }
                        }
                    } else if (jac->isnodal) {
                        for (i = 0; i < 4; i++) {
                            jac->Jdi[ee[i]*jac->nsd + sd] += jdi * 0.25;
                        }
                    } else { // element-based jacobian
                        jac->Jdi[t * jac->nsd + sd] += jdi;
                    }
                }
            }
        }
    }
}

void rb_deldotdel(Config* cfg, tetmesh* mesh, Forward* fem) {
    float3* N;
    int i1, i2, i3, i4, i, j, t;
    double derx[4], dery[4], derz[4];
    double Ve, RVe6;

    /* map coordinate pointers*/
    N = mesh->node;

    /* loop over all elements*/
    for (t = 0; t < mesh->ne; t++) {
        RVe6 = 1.0 / (mesh->evol[t] * 6.0);
        Ve = mesh->evol[t];

        i1 = mesh->elem[t].x;
        i2 = mesh->elem[t].y;
        i3 = mesh->elem[t].z;
        i4 = mesh->elem[t].w;

        /*calculate del_x,del_y and del_z for 4 basis functions per elem*/
        derx[0] = -((N[i3].y * N[i4].z - N[i3].z * N[i4].y) - N[i2].y * (N[i4].z - N[i3].z)
                    + N[i2].z * (N[i4].y - N[i3].y)) * RVe6;
        dery[0] = ((N[i3].x * N[i4].z - N[i4].x * N[i3].z) - N[i2].x * (N[i4].z - N[i3].z)
                   + N[i2].z * (N[i4].x - N[i3].x)) * RVe6;
        derz[0] = -((N[i3].x * N[i4].y - N[i3].y * N[i4].x) - N[i2].x * (N[i4].y - N[i3].y)
                    + N[i2].y * (N[i4].x - N[i3].x)) * RVe6;

        derx[1] = ((N[i3].y * N[i4].z - N[i3].z * N[i4].y) - N[i1].y * (N[i4].z - N[i3].z)
                   + N[i1].z * (N[i4].y - N[i3].y)) * RVe6;
        dery[1] = -((N[i3].x * N[i4].z - N[i4].x * N[i3].z) - N[i1].x * (N[i4].z - N[i3].z)
                    + N[i1].z * (N[i4].x - N[i3].x)) * RVe6;
        derz[1] = ((N[i3].x * N[i4].y - N[i3].y * N[i4].x) - N[i1].x * (N[i4].y - N[i3].y)
                   + N[i1].y * (N[i4].x - N[i3].x)) * RVe6;

        derx[2] = -((N[i2].y * N[i4].z - N[i2].z * N[i4].y) - N[i1].y * (N[i4].z - N[i2].z)
                    + N[i1].z * (N[i4].y - N[i2].y)) * RVe6;
        dery[2] = ((N[i2].x * N[i4].z - N[i4].x * N[i2].z) - N[i1].x * (N[i4].z - N[i2].z)
                   + N[i1].z * (N[i4].x - N[i2].x)) * RVe6;
        derz[2] = -((N[i2].x * N[i4].y - N[i2].y * N[i4].x) - N[i1].x * (N[i4].y - N[i2].y)
                    + N[i1].y * (N[i4].x - N[i2].x)) * RVe6;

        derx[3] = ((N[i2].y * N[i3].z - N[i2].z * N[i3].y) - N[i1].y * (N[i3].z - N[i2].z)
                   + N[i1].z * (N[i3].y - N[i2].y)) * RVe6;
        dery[3] = -((N[i2].x * N[i3].z - N[i3].x * N[i2].z) - N[i1].x * (N[i3].z - N[i2].z)
                    + N[i1].z * (N[i3].x - N[i2].x)) * RVe6;
        derz[3] = ((N[i2].x * N[i3].y - N[i2].y * N[i3].x) - N[i1].x * (N[i3].y - N[i2].y)
                   + N[i1].y * (N[i3].x - N[i2].x)) * RVe6;

        /*loop over upper triangle*/
        for (i = 0; i < 4; i++)
            for (j = i; j < 4; j++) {
                fem->deldotdel[t * 10 + (i << 2) + j - i - (i > 1) - ((i > 2) << 1)] = (derx[i] * derx[j] + dery[i] * dery[j] + derz[i] * derz[j]) * Ve;
            }
    }
}

void rb_femmatrix_elem(Config* cfg, tetmesh* mesh, Forward* fem) {
    double mua, musp, dcoeff, nref;
    int i, j, t, ii, jj, ij, *ee;
    double ra, ri, Ve, deldotdel, sm;

    /*initialize output variables*/
    if (fem->Dr) {
        memset(fem->Dr, 0, mesh->nn * sizeof(double));
    }

    if (fem->Di) {
        memset(fem->Di, 0, mesh->nn * sizeof(double));
    }

    if (fem->Ar) {
        memset(fem->Ar, 0, mesh->ntot * sizeof(double));
    }

    if (fem->Ai) {
        memset(fem->Ai, 0, mesh->ntot * sizeof(double));
    }

    /* loop over all elements*/
    for (t = 0; t < mesh->ne; t++) {
        Ve = mesh->evol[t];
        ee = (int*)(mesh->elem + t);

        mua = mesh->med[mesh->type[t]].mua;
        musp = mesh->med[mesh->type[t]].mus;
        nref = mesh->med[mesh->type[t]].n;

        dcoeff = 1.0 / (3.0 * (mua + musp));

        /*loop over index i*/
        for (i = 0; i < 4; i++) {
            ii = ee[i];

            /*loop over index j, upper triangle only*/
            for (j = i; j < 4; j++) {
                jj = ee[j];
                deldotdel = fem->deldotdel[t * 10 + (i << 2) + j - i - (i > 1) - ((i > 2) << 1)];

                sm = Ve * 0.05;

                if (i == j) {
                    sm = Ve * 0.1;
                }

                /* D*Ve*delFi*delF */
                ra = dcoeff * deldotdel + mua * sm;
                ri = cfg->omega * R_C0 * nref * sm;

                /*add the values to the vectorized sparse matrix*/
                if (ii == jj) {
                    fem->Dr[ii] += ra;

                    if (fem->Di) {
                        fem->Di[ii] += ri;
                    }
                } else {
                    ij = sub2ind(mesh, ii, jj);
                    fem->Ar[ij] += ra;

                    if (fem->Ai) {
                        fem->Ai[ij] += ri;
                    }
                }
            }
        }
    }
}

void rb_femmatrix_nodal(Config* cfg, tetmesh* mesh, Forward* fem) {
    double mua, musp, dcoeff, nref;
    int i, j, k, t, ii, jj, ij, *ee;
    double ra, ri, Ve, deldotdel, sm;

    /*initialize output variables*/
    if (fem->Dr) {
        memset(fem->Dr, 0, mesh->nn * sizeof(double));
    }

    if (fem->Di) {
        memset(fem->Di, 0, mesh->nn * sizeof(double));
    }

    if (fem->Ar) {
        memset(fem->Ar, 0, mesh->ntot * sizeof(double));
    }

    if (fem->Ai) {
        memset(fem->Ai, 0, mesh->ntot * sizeof(double));
    }

    /* loop over all elements*/
    for (t = 0; t < mesh->ne; t++) {
        Ve = mesh->evol[t];
        ee = (int*)(mesh->elem + t);

        /*loop over index i*/
        for (i = 0; i < 4; i++) {
            ii = ee[i];

            /*loop over index j*/
            for (j = i; j < 4; j++) {
                jj = ee[j];
                deldotdel = fem->deldotdel[t * 10 + (i << 2) + j - i - (i > 1) - ((i > 2) << 1)] * 0.25;

                ra = 0.0;
                ri = 0.0;

                for (k = 0; k < 4; k++) {
                    sm = Ve * (1.0 / 120.);

                    if (i == j || i == k || j == k) {
                        sm = Ve * (1.0 / 60.);
                    }

                    if (i == j && i == k) {
                        sm = Ve * (1.0 / 20.);
                    }

                    mua = mesh->med[mesh->type[ee[k]]].mua;
                    musp = mesh->med[mesh->type[ee[k]]].mus;
                    nref = mesh->med[mesh->type[ee[k]]].n;

                    dcoeff = 1.0 / (3.0 * (mua + musp));

                    /* D*Ve*delFi*delF */
                    ra += dcoeff * deldotdel + mua * sm;
                    ri += cfg->omega * R_C0 * nref * sm;
                }

                /*add the values to the vectorized sparse matrix*/
                if (ii == jj) {
                    fem->Dr[ii] += ra;

                    if (fem->Di) {
                        fem->Di[ii] += ri;
                    }
                } else {
                    ij = sub2ind(mesh, ii, jj);
                    fem->Ar[ij] += ra;

                    if (fem->Ai) {
                        fem->Ai[ij] += ri;
                    }
                }
            }
        }
    }
}


void rb_fem_bc(Config* cfg, tetmesh* mesh, Forward* fem) {
    int t;
    double val;
    double Reff = (1.0 - cfg->reff) / (1.0 + cfg->reff) * (1.0 / 12.0);

    for (t = 0; t < mesh->nf; t++) {
        val = mesh->area[t] * Reff;
        fem->Dr[mesh->face[t].x] += val;
        fem->Dr[mesh->face[t].y] += val;
        fem->Dr[mesh->face[t].z] += val;

        val = val * 0.5;
        fem->Ar[sub2ind(mesh, mesh->face[t].x, mesh->face[t].y)] += val;
        fem->Ar[sub2ind(mesh, mesh->face[t].x, mesh->face[t].z)] += val;
        fem->Ar[sub2ind(mesh, mesh->face[t].y, mesh->face[t].z)] += val;
    }
}

int sub2ind(tetmesh* mesh, int i, int j) {
    int base, ij = -1, k;
    base = (int)(mesh->idxsum[i] - mesh->idxcount[i]);

    for (k = 0; k < (int)(mesh->idxcount[i]); k++) {
        ij = k + base;

        if ((int)(mesh->cols[ij]) == j) {
            break;
        }
    }

    return ij;
}

extern "C" int rb3_throw_exception(const int id, const char* msg, const char* filename, const int linenum) {
    printf("Redbird ERROR (%d): %s in unit %s:%d\n", id, msg, filename, linenum);
    throw (msg);
    return id;
}

void rb3_usage() {
    printf("Usage:\n    [flux,detphoton]=rbforward(cfg);\n\nPlease run 'help mmclab' for more details.\n");
}

extern "C" void rb3_flush() {
#ifndef MATLAB_MEX_FILE
    mexEvalString("fflush(stdout);");
#else
    mexEvalString("pause(.0001);");
#endif
}
