SOURCEVEL.cpp

#include <iostream>
#include <cmath>
#include <cassert>
#include <atomic>
#include <thread>
#include <mutex>
#include <condition_variable>
#include <vector>
#include <chrono>

#include <fsi_thread.h>

#ifdef FSI_OPENMP
#include <omp.h>
#endif
#ifdef FSI_MPI
#include <mpi.h>
#endif

/*------------------------------------------------------------------------
 * Calculate the induced velocity by the doublet
 * on the any arbitrary points. Originally a MATLAB MEX function.
 * ---------------------------------------------------------------------- */

using namespace std;
using namespace chrono;

#define X 0
#define Y 1
#define Z 2

extern condition_variable cv_parent;
extern vector<double **> tandem_output;
extern int world_rank;

static mutex false_sharing_mtx;
static FILE *fp = nullptr;

static inline void SOURCE_VEL(double *pU,
                const double colx, const double coly, const double colz,
                const double x1, const double y1, const double z1,
                const double x2, const double y2, const double z2,
                const double x3, const double y3, const double z3,
                const double x4, const double y4, double z4,
                const double miu)
{
    // % ???????? comment error in MATLAB source? INFLUENCE(x,y,z,x1,y1,z1,x2,y2,z2,x3,y3,z3,x4,y4,z4,miu) calculates the
    // % pertubation potential on the given point by the surface doublet and
    // % source elements.
    // caculating the tangent and normal vectos
    // calculating chordwise tangent

    const double A1 =((x4+x3)-(x1+x2))/2;
    const double A2 =((y4+y3)-(y1+y2))/2;
    const double A3 =((z4+z3)-(z1+z2))/2;
    const double AA =sqrt(A1*A1 + A2*A2 + A3*A3);
    const double tx[] = {A1/AA, A2/AA, A3/AA};

    // next another (?) vector in this plan
    const double b1 = x2 - x1;
    const double b2 = y2 - y1;
    const double b3 = z2 - z1;
    const double bb=sqrt(b1*b1 + b2*b2 + b3*b3);
    const double bt[]= {b1/bb, b2/bb, b3/bb};

    // normal vector
    const double v1 = tx[1] * bt[2] - tx[2] * bt[1];
    const double v2 = tx[2] * bt[0] - tx[0] * bt[2];
    const double v3 = tx[0] * bt[1] - tx[1] * bt[0];
    const double vv=sqrt(v1*v1 + v2*v2 + v3*v3);
    const double n[]= {v1/vv, v2/vv, v3/vv};

    // tangential vector in spanwise direction
    double ty[] = {n[1]*tx[2] - n[2]*tx[1],
                   n[2]*tx[0] - n[0]*tx[2],
                   n[0]*tx[1] - n[1]*tx[0]};

    const double tt=sqrt(ty[0]*ty[0] + ty[1]*ty[1] + ty[2]*ty[2]);
    ty[0] = ty[0] / tt;
    ty[1] = ty[1] / tt;
    ty[2] = ty[2] / tt;

    // calculation of area
    const double e11=x3-x1;
    const double e22=y3-y1;
    const double e33=z3-z1;
    const double f1=x2-x1;
    const double f2=y2-y1;
    const double f3=z2-z1;

    //normal area
    const double s11 = f2*b3 - f3*b2;
    const double s12 = b1*f3 - f1*b3;
    const double s13 = f1*b2 - f2*b1;
    const double s21 = b2*e33 - b3*e22;
    const double s22 = e11*b3 - b1*e33;
    const double s23 = b1*e22 - b2*e11;
    const double s= 0.5 * (sqrt(s11*s11 + s12*s12 + s13*s13) +
                           sqrt(s21*s21 + s22*s22 + s23*s23));
    const double pi=3.14159265358979;
    const double FF=5;
    const double FF_sqr = FF*FF;
    const double eror=1.0e-11;
    const double fourpi=miu/4/pi;

    // coetroied of the qurd
    const double xc=0.25*(x1+x2+x3+x4);
    const double yc=0.25*(y1+y2+y3+y4);
    const double zc=0.25*(z1+z2+z3+z4);

    // panel node coordinates (into local CS)
    const double xl1 = (x1-xc)*tx[0] + (y1-yc)*tx[1] + (z1-zc)*tx[2];
    const double xl2 = (x2-xc)*tx[0] + (y2-yc)*tx[1] + (z2-zc)*tx[2];
    const double xl3 = (x3-xc)*tx[0] + (y3-yc)*tx[1] + (z3-zc)*tx[2];
    const double xl4 = (x4-xc)*tx[0] + (y4-yc)*tx[1] + (z4-zc)*tx[2];

    const double yl1 = (x1-xc)*ty[0] + (y1-yc)*ty[1] + (z1-zc)*ty[2];
    const double yl2 = (x2-xc)*ty[0] + (y2-yc)*ty[1] + (z2-zc)*ty[2];
    const double yl3 = (x3-xc)*ty[0] + (y3-yc)*ty[1] + (z3-zc)*ty[2];
    const double yl4 = (x4-xc)*ty[0] + (y4-yc)*ty[1] + (z4-zc)*ty[2];

    // zl1 = (x1-xc)*n1 + (y1-yc)*n2 + (z1-zc)*n3;
    // zl2 = (x2-xc)*n1 + (y2-yc)*n2 + (z2-zc)*n3;
    // zl3 = (x3-xc)*n1 + (y3-yc)*n2 + (z3-zc)*n3;
    // zl4 = (x4-xc)*n1 + (y4-yc)*n2 + (z4-zc)*n3;
    //transformation of influence points into LCS

    const double cpx = (colx-xc)*tx[0] + (coly-yc)*tx[1] + (colz-zc)*tx[2];
    const double cpy = (colx-xc)*ty[0] + (coly-yc)*ty[1] + (colz-zc)*ty[2];
    const double cpz = (colx-xc) *n[0] + (coly-yc) *n[1] + (colz-zc) *n[2];

// panel side lengths in local co-ordinate system

    const double d1 = sqrt((xl2-xl1)*(xl2-xl1) + (yl2-yl1)*(yl2-yl1));
    const double d2 = sqrt((xl3-xl2)*(xl3-xl2) + (yl3-yl2)*(yl3-yl2));
    const double d3 = sqrt((xl4-xl3)*(xl4-xl3) + (yl4-yl3)*(yl4-yl3));
    const double d4 = sqrt((xl1-xl4)*(xl1-xl4) + (yl1-yl4)*(yl1-yl4));
// calculation of the diagonals in local co-ordinate system

    //a1= (xl3-xl1);
    //a2= (yl3-yl1);
    //b11=-(xl2-xl4);
    //b22=-(yl2-yl4);
    // unused D1=sqrt(a1*a1+a2*a2);
    // unused D2=sqrt(b11*b11+b22*b22);

    const double rad = ((cpx-xc)*(cpx-xc) + (cpy-yc)*(cpy-yc) + (cpz)*(cpz));

    const double cpx1 = cpx - xl1;
    const double cpx2 = cpx - xl2;
    const double cpx3 = cpx - xl3;
    const double cpx4 = cpx - xl4;

    const double cpy1 = cpy - yl1;
    const double cpy2 = cpy - yl2;
    const double cpy3 = cpy - yl3;
    const double cpy4 = cpy - yl4;

    const double e1 = cpx1*cpx1+cpz*cpz;
    const double e2 = cpx2*cpx2+cpz*cpz;
    const double e3 = cpx3*cpx3+cpz*cpz;
    const double e4 = cpx4*cpx4+cpz*cpz;

    const double r1 = sqrt(e1 + cpy1*cpy1);
    const double r2 = sqrt(e2 + cpy2*cpy2);
    const double r3 = sqrt(e3 + cpy3*cpy3);
    const double r4 = sqrt(e4 + cpy4*cpy4);

    const double x21 = xl2-xl1;
    const double x32 = xl3-xl2;
    const double x43 = xl4-xl3;
    const double x14 = xl1-xl4;

    const double y21 = yl2-yl1;
    const double y32 = yl3-yl2;
    const double y43 = yl4-yl3;
    const double y14 = yl1-yl4;

    const double h1 = cpx1*cpy1;
    const double h2 = cpx2*cpy2;
    const double h3 = cpx3*cpy3;
    const double h4 = cpx4*cpy4;

    // % calculation of gradients
    // m12=y21/x21;
    // m23=y32/x32;
    // m34=y43/x43;
    // m41=y14/x14;

    // if distance of panel from influenced point is greater
    // then product of longer diagonal and "far field" coefficient
    if (sqrt(rad) > 5*FF_sqr) {
        const double spowrad = s * pow(rad, -1.5);
        *pU += fourpi * (cpx - xc) * spowrad;
        pU++;
        *pU += fourpi * (cpy - yc) * spowrad;
        pU++;
        *pU += fourpi * (cpz - zc) * spowrad;
        return;
    }
    if (sqrt(cpz*cpz)< eror) {
        pU[2] += -0.5;
        return;
    }
    *pU += fourpi*(y21/d1 * log((r1+r2-d1)/(r1+r2+d1)) +
                   y32/d2 * log((r2+r3-d2)/(r2+r3+d2)) +
                   y43/d3 * log((r3+r4-d3)/(r3+r4+d3)) +
                   y14/d4 * log((r4+r1-d4)/(r4+r1+d4)));
    pU++;

    *pU += -fourpi*(x21/d1 * log((r1+r2-d1)/(r1+r2+d1)) +
                    x32/d2 * log((r2+r3-d2)/(r2+r3+d2)) +
                    x43/d3 * log((r3+r4-d3)/(r3+r4+d3)) +
                    x14/d4 * log((r4+r1-d4)/(r4+r1+d4)));
    pU++;

    const double F1 = y21*e1 - x21*h1;
    const double G1 = y21*e2 - x21*h2;
    const double s1 = atan2(cpz * x21 * (F1*r2-G1*r1), cpz*cpz * x21*x21 * r1*r2 + F1*G1);

    const double F2 = y32*e2 - x32*h2;
    const double G2 = y32*e3 - x32*h3;
    const double s2 = atan2(cpz * x32 * (F2*r3-G2*r2), cpz*cpz * x32*x32 * r2*r3 + F2*G2);

    const double F3 = y43*e3 - x43*h3;
    const double G3 = y43*e4 - x43*h4;
    const double s3 = atan2(cpz * x43 * (F3*r4-G3*r3), cpz*cpz * x43*x43 * r3*r4 + F3*G3);

    const double F4 = y14*e4 - x14*h4;
    const double G4 = y14*e1 - x14*h1;
    const double s4 = atan2(cpz * x14 * (F4*r1-G4*r4), cpz*cpz * x14*x14 * r4*r1 + F4*G4);

    *pU += fourpi * (s1 + s2 + s3 + s4);
}

void sourcevel_thread::mex_thread_init(const int t, const double *sig, double *uwakeS) {
    // init variables across threads
    this->t = t;
    this->sig = sig;
    this->uwakeS = uwakeS;

    // init convenience constants (for current run)
    t1 = t + 1;
    t1n1 = t1 * n1;
    workinc = calc_wake_workinc(t1n1, workfactor);

    // init synchronized variables
    order = 0;
    threads_finished = 0;

#ifdef FSI_OPENMP
    TSTART(mex_time, steady_clock::now());  // start mex timing
    omp_set_num_threads(nthreads);
    #pragma omp parallel
    {
        const int thrdid = omp_get_thread_num();
#ifdef FSI_STATS
#ifdef FSI_OPENMP
        int openmp_denied = 0;
        if (thrdid == 0) {  // only for one thread
            int nthrds = omp_get_num_threads();
            if (nthreads != nthrds) openmp_denied += nthreads - nthrds;
        }
#endif
        int ndoublets = 0;
        const steady_clock::time_point start = steady_clock::now();
#endif
        double * const uwakerecv = tandem_output[thrdid][sourcevel_index];
        int ord;
        while ((ord = order.fetch_add(workinc)) < t1n1) {  // while work remains
            double *puwake = uwakerecv;
            for (int i = 0, iord = ord; i < workinc && iord < t1n1; i++, iord++) {
                // decode for row, col
                const double * const pC = &C[coloc(iord)]; // indexes uwakeS(nts, n1)

                assert(pC - C <= ((nts * n1) - 1) * 3);
                assert(puwake - uwakerecv < outsize);
                doublet(puwake,
                        pC[X], pC[Y], pC[Z],
                        B,
                        sig,
                        nx, ny, m2);
                puwake += 3;
#ifdef FSI_STATS
                ndoublets++;
#endif
            }
            // write computations from packed buffer
            const double *puwakerecv = uwakerecv;
            {  // start false sharing critical section
                #pragma omp critical
                for (int i = 0, iord = ord; i < workinc && iord < t1n1; i++, iord++) {
                    // decode for row, col
                    puwake = &uwakeS[coloc(iord)];

                    assert(puwake - uwakeS <= 3 * (nts * (ny + 1) - 1)); // cube uvwwakeS(3, nts, n + 1);
                    //cerr << "puwake=" << puwake - uwakeS << " recv=" << *puwakerecv << endl;
                    //cerr << "puwake=" << puwake - uwakeS << " max=" << 3 * (nts * (ny + 1) - 1) << endl;
                    *puwake++ += *puwakerecv++;  // X
                    *puwake++ += *puwakerecv++;  // Y
                    *puwake += *puwakerecv++;  // Z
                }
            }  // end false sharing critical section
        }  // while work remains
#ifdef FSI_STATS
        // per-thread statistics
        mex_stat *pmex_stat = (*thread_stat_map[thrdid])[this];
        pmex_stat->call_count++;
        pmex_stat->elapsed += duration_cast<mex_thread_time_units_t>(steady_clock::now() - start);
        pmex_stat->doublets += ndoublets;
#ifdef FSI_OPENMP
        if (openmp_denied != 0) pmex_stat->openmp_denied += openmp_denied;
#endif
#endif
    }  // end parallel
    TSTOP(mex_time, steady_clock::now());  // stop mex timing and add elapsed
#else  // low-level threading model
    mex_parent();  // awaken child threads and wait for them to finish
#endif
}

#define INIT(B, m2) const double \
    *B##i1j = B + 3, \
    *B##ij1 = B + (3 * m2), \
    *B##i1j1 = B##i1j + (3 * m2)

#define ITER(B) \
    B = B##i1j, \
    B##i1j += 3, \
    B##ij1 = B##i1j1, \
    B##i1j1 += 3

#define LASTITER(B) \
    B += (3 * 2); \
    B##i1j += (3 * 2); \
    B##ij1 += (3 * 2); \
    B##i1j1 += (3 * 2)

#define XYZOFFSET(B, offset) \
    B##offset[X], \
    B##offset[Y], \
    B##offset[Z]

inline void sourcevel_thread::doublet(double * const pwakeS,
                           const double CXitin, const double CYitin, const double CZitin,
                           const double *pB,
                           const double *psmn,
                           const int m, const int n, const int m2) {
    pwakeS[X] = 0;
    pwakeS[Y] = 0;
    pwakeS[Z] = 0;
#ifndef NDEBUG
    const double * const pBinit = pB;
#endif
    INIT(pB, m2);  // init pointers for boundary points
    for (int j = 0; j < n; j++) {
        for (int i = 0; i < m; i++, psmn++) {
            assert(pBi1j1 - pBinit < 3 * m2 * (n + 1));
            SOURCE_VEL(pwakeS,
                       CXitin, CYitin, CZitin,
                       pB[X], pB[Y], pB[Z],  //*pBXij, *pBYij, *pBZij,
                       XYZOFFSET(pB, ij1), //*pBXij1, *pBYij1, *pBZij1,
                       XYZOFFSET(pB, i1j1), //*pBXi1j1, *pBYi1j1, *pBZi1j1,
                       XYZOFFSET(pB, i1j), //*pBXi1j, *pBYi1j, *pBZi1j,
                       *psmn);
#ifdef FSI_MEX_DEBUG
            if (fp != nullptr) fprintf(fp,"uwakeS=%f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n",
                       *pwakeS,
                       CXitin, CYitin, CZitin,
                       pB[X], pB[Y], pB[Z],  //*pBXij, *pBYij, *pBZij,
                       XYZOFFSET(pB, ij1), //*pBXij1, *pBYij1, *pBZij1,
                       XYZOFFSET(pB, i1j1), //*pBXi1j1, *pBYi1j1, *pBZi1j1,
                       XYZOFFSET(pB, i1j), //*pBXi1j, *pBYi1j, *pBZi1j,
                       *psmn);
#endif
            ITER(pB);
        }  // for each column
        // skip over last boundary point and wrapping boundary point
        LASTITER(pB);
    }  // for each row
    //*(uw+ic+jc*nw1)+=  UVW[0];
}

#ifdef FSI_MEX_DEBUG
void outbug(const int t, const char *label) {
    if (world_rank != 0) {
        char buf[256];
        snprintf(buf, sizeof buf - 1, "/home/wm/ml/WLM_PM_CODE/data/SOURCEVEL_T%d.dat", t);
        fp = fopen(buf, world_rank == -1 ? "w" : "a");  // for MPI, this is called repeatedly, so append
        if (fp == nullptr) cerr << "Couldn't open %b " << buf << endl;
            else {
            fprintf(fp, "%s\n", label);
            cerr << "Writing " << label << " SOURCEVEL debugging file: " << buf << endl;
        }
    }
}
#endif

void sourcevel_thread::mex_thread_run (const int partner_rank) {
    // WX(nts, ny + 1)
    // BX(m2, n1)
    // uwakeS(nts, n1) - row ranges 0:t

    double * const uwakerecv = tandem_output[partner_rank][sourcevel_index];
    //double * const uwakerecv = new double [outsize];
#ifdef FSI_MPI
    bool send_sig = true;  // send coefficients only once per run
#endif
#ifdef FSI_MEX_DEBUG
    outbug(t, "threaded");
#endif

#ifdef FSI_STATS
    int ndoublets = 0;
    int nsyncs = 0;
#endif
    for (;;) {  // until no more data
        const int ord = order.fetch_add(workinc);
#ifdef FSI_STATS
        nsyncs++;
#endif
        if (ord >= t1n1) {
#ifdef FSI_STATS
#ifndef FSI_OPENMP
            // statistics
            mex_stat *pmex_stat = (*thread_stat_map[this_thread::get_id()])[this];
            pmex_stat->doublets += ndoublets;
            pmex_stat->nsyncs += nsyncs;
#endif
#endif
            //if (itin == ntsn1 + nthreads - 1)...
            // handle synchronization with parent in class
            if (is_last_thread()) {
                if (fp != nullptr) fclose(fp);
            }
            return;  // to wait state
        }
        double *puwake;
#ifdef FSI_MPI
        int status = MPI_Send(sig, send_sig ? sigsize : 0, MPI_DOUBLE, partner_rank, sourcevel_mask | ord, MPI_COMM_WORLD);
        assert(status == MPI_SUCCESS);
        send_sig = false;   // send to remote child only once per time step
        status = MPI_Recv(uwakerecv, outsize, MPI_DOUBLE, partner_rank, MPI_ANY_TAG, MPI_COMM_WORLD, MPI_STATUS_IGNORE);
        assert(status == MPI_SUCCESS);
#else
        puwake = uwakerecv;
        for (int i = 0, iord = ord; i < workinc && iord < t1n1; i++, iord++, puwake += 3) {
            // decode for row, col
            const double * const pC = &C[coloc(iord)]; // indexes uwakeS(nts, n1)

            assert(pC - C <= ((nts * n1) - 1) * 3);
            assert(puwake - uwakerecv < outsize);
            doublet(puwake,
                    pC[X], pC[Y], pC[Z],
                    B,
                    sig,
                    nx, ny, m2);
        }
#endif
        // write computations from packed buffer
        const double *puwakerecv = uwakerecv;
        {  // start false sharing critical section
            if (nthreads > 1) unique_lock<mutex> lck(false_sharing_mtx);
            for (int i = 0, iord = ord; i < workinc && iord < t1n1; i++, iord++) {
                // decode for row, col
                puwake = &uwakeS[coloc(iord)];

                assert(puwake - uwakeS <= 3 * (nts * (ny + 1) - 1)); // cube uvwwakeS(3, nts, n + 1);
                //cerr << "puwake=" << puwake - uwakeS << " recv=" << *puwakerecv << endl;
                //cerr << "puwake=" << puwake - uwakeS << " max=" << 3 * (nts * (ny + 1) - 1) << endl;
                *puwake++ += *puwakerecv++;  // X
                *puwake++ += *puwakerecv++;  // Y
                *puwake += *puwakerecv++;  // Z
#ifdef FSI_STATS
                ndoublets++;
#endif
            }
        }  // end false sharing critical section
    }
}

#ifdef FSI_MPI
void sourcevel_thread::compute(int ord, const double * const sig,  const int pt) {

#ifdef FSI_MEX_DEBUG
    outbug(pt, "computed");
#endif

    t = pt;
    // init convenience constants (for current run)
    t1 = t + 1;
    t1n1 = t1 * n1;

    workinc = calc_wake_workinc(t1n1, workfactor);
    //workinc = t1n1 / nthreads * workfactor + 1;  // zero here causes deadlock

    // SOURCEVEL - mat sig1(2 * m, n);
    // cube uwake(3, nts, n + 1); ZERO(uwake);  // former uwakeS...

    // reserve space for both A and B
    if (uwakeS == nullptr) {
        uwakeS = new double [outsize]; //% Influence co-efficient matrix of surface doublet distribution.
    }
    double *puwake = uwakeS;
    for (int nwork = 0; nwork < workinc && ord < t1n1; nwork++, ord++, puwake += 3) {
        // decode for row, col
        const double * const pC = &C[coloc(ord)]; // indexes uwakeS(nts, n1)

        assert(pC - C <= ((nts * n1) - 1) * 3);
        assert(puwake - uwakeS < outsize);
        doublet(puwake,  // packed XYZ &uwakeS[itin],
                pC[X], pC[Y], pC[Z],
                B,
                sig,
                nx, ny, m2);
    }
    assert(puwake - uwakeS <= outsize);
    int status = MPI_Send(uwakeS, puwake - uwakeS, MPI_DOUBLE, 0, 0, MPI_COMM_WORLD);
    assert(status == MPI_SUCCESS);
#ifdef FSI_DEBUG
    if (fp != nullptr) fclose(fp);
#endif
}
#endif