static char help[] =
"Structured-grid minimal surface equation (MSE) in 2D.  Option prefix mse_.\n"
"Equation is\n"
"  - div ( (1 + |grad u|^2)^q grad u ) = 0\n"
"on the unit square [0,1]x[0,1] subject to Dirichlet boundary\n"
"conditions u = g(x,y).  Power q defaults to -1/2 (i.e. MSE) but is adjustable.\n"
"Catenoid and tent boundary conditions are implemented; catenoid is an exact\n"
"solution.  We re-use the Jacobian from the Poisson equation, but it is suitable\n"
"only for low-amplitude g, or as preconditioning material in -snes_mf_operator.\n"
"Options -snes_fd_color and -snes_grid_sequence K are recommended.\n"
"This code is multigrid (GMG) capable.\n\n";

#include <petsc.h>
#include "../ch6/poissonfunctions.h"
#include "../quadrature.h"

typedef struct {
    double    q,          // the exponent in the diffusivity;
                          // =-1/2 for minimal surface eqn; =0 for Laplace eqn
              tent_H,     // height of tent door along y=0 boundary
              catenoid_c; // parameter in catenoid formula
    int       quaddegree; // quadrature degree used in -mse_monitor
} MinimalCtx;

// Dirichlet boundary conditions
static double g_bdry_tent(double x, double y, double z, void *ctx) {
    PoissonCtx *user = (PoissonCtx*)ctx;
    MinimalCtx *mctx = (MinimalCtx*)(user->addctx);
    if (x < 1.0e-8) {
        return 2.0 * mctx->tent_H * (y < 0.5 ? y : 1.0 - y);
    } else
        return 0;
}

static double g_bdry_catenoid(double x, double y, double z, void *ctx) {
    PoissonCtx   *user = (PoissonCtx*)ctx;
    MinimalCtx   *mctx = (MinimalCtx*)(user->addctx);
    const double c = mctx->catenoid_c;
    return c * cosh(x/c) * sin(acos( (y/c) / cosh(x/c) ));
}

// the coefficient (diffusivity) of minimal surface equation, as a function
//   of  w = |grad u|^2
static double DD(double w, double q) {
    return pow(1.0 + w,q);
}

typedef enum {TENT, CATENOID} ProblemType;
static const char* ProblemTypes[] = {"tent","catenoid",
                                     "ProblemType", "", NULL};

extern PetscErrorCode FormExactFromG(DMDALocalInfo*, Vec, PoissonCtx*);
extern PetscErrorCode FormFunctionLocal(DMDALocalInfo*, double**,
                                        double **FF, PoissonCtx*);
extern PetscErrorCode MSEMonitor(SNES, int, double, void*);

int main(int argc,char **argv) {
    PetscErrorCode ierr;
    DM             da;
    SNES           snes;
    Vec            u_initial, u;
    PoissonCtx     user;
    MinimalCtx     mctx;
    PetscBool      monitor = PETSC_FALSE,
                   exact_init = PETSC_FALSE;
    DMDALocalInfo  info;
    ProblemType    problem = CATENOID;

    // defaults:
    mctx.q = -0.5;
    mctx.tent_H = 1.0;
    mctx.catenoid_c = 1.1;  // case shown in Figure in book
    mctx.quaddegree = 3;
    user.cx = 1.0;
    user.cy = 1.0;
    user.cz = 1.0;

    PetscInitialize(&argc,&argv,NULL,help);
    ierr = PetscOptionsBegin(PETSC_COMM_WORLD,"mse_",
                             "minimal surface equation solver options",""); CHKERRQ(ierr);
    ierr = PetscOptionsReal("-catenoid_c",
                            "parameter for problem catenoid; c >= 1 required",
                            "minimal.c",mctx.catenoid_c,&(mctx.catenoid_c),NULL); CHKERRQ(ierr);
    ierr = PetscOptionsBool("-exact_init",
                            "initial Newton iterate = continuum exact solution; only for catenoid",
                            "minimal.c",exact_init,&(exact_init),NULL);CHKERRQ(ierr);
    ierr = PetscOptionsBool("-monitor",
                            "print surface area and diffusivity bounds at each SNES iteration",
                            "minimal.c",monitor,&(monitor),NULL);CHKERRQ(ierr);
    ierr = PetscOptionsReal("-q",
                            "power of (1+|grad u|^2) in diffusivity",
                            "minimal.c",mctx.q,&(mctx.q),NULL); CHKERRQ(ierr);
    ierr = PetscOptionsInt("-quaddegree",
                            "quadrature degree (=1,2,3) used in -mse_monitor",
                            "minimal.c",mctx.quaddegree,&(mctx.quaddegree),NULL); CHKERRQ(ierr);
    ierr = PetscOptionsEnum("-problem",
                            "problem type determines boundary conditions",
                            "minimal.c",ProblemTypes,(PetscEnum)problem,(PetscEnum*)&problem,
                            NULL); CHKERRQ(ierr);
    ierr = PetscOptionsReal("-tent_H",
                            "'door' height for problem tent",
                            "minimal.c",mctx.tent_H,&(mctx.tent_H),NULL); CHKERRQ(ierr);
    ierr = PetscOptionsEnd(); CHKERRQ(ierr);

    user.addctx = &mctx;   // attach MSE-specific parameters
    switch (problem) {
        case TENT:
            if (exact_init) {
                SETERRQ(PETSC_COMM_WORLD,2,
                    "initialization with exact solution only possible for -mse_problem catenoid\n");
            }
            user.g_bdry = &g_bdry_tent;
            break;
        case CATENOID:
            if (mctx.catenoid_c < 1.0) {
                SETERRQ(PETSC_COMM_WORLD,3,
                    "catenoid exact solution only valid if c >= 1\n");
            }
            if ((exact_init) && (mctx.q != -0.5)) {
                SETERRQ(PETSC_COMM_WORLD,4,
                    "initialization with catenoid exact solution only possible if q=-0.5\n");
            }
            user.g_bdry = &g_bdry_catenoid;
            break;
        default:
            SETERRQ(PETSC_COMM_WORLD,5,"unknown problem type\n");
    }

    ierr = DMDACreate2d(PETSC_COMM_WORLD, DM_BOUNDARY_NONE, DM_BOUNDARY_NONE,
                        DMDA_STENCIL_BOX,  // contrast with fish2
                        3,3,PETSC_DECIDE,PETSC_DECIDE,1,1,NULL,NULL,&da); CHKERRQ(ierr);
    ierr = DMSetApplicationContext(da,&user); CHKERRQ(ierr);
    ierr = DMSetFromOptions(da); CHKERRQ(ierr);
    ierr = DMSetUp(da); CHKERRQ(ierr);  // this must be called BEFORE SetUniformCoordinates
    ierr = DMDASetUniformCoordinates(da,0.0,1.0,0.0,1.0,0.0,1.0); CHKERRQ(ierr);

    ierr = SNESCreate(PETSC_COMM_WORLD,&snes); CHKERRQ(ierr);
    ierr = SNESSetDM(snes,da); CHKERRQ(ierr);
    ierr = DMDASNESSetFunctionLocal(da,INSERT_VALUES,
               (DMDASNESFunction)FormFunctionLocal,&user); CHKERRQ(ierr);
    // this is the Jacobian of the Poisson equation, thus ONLY APPROXIMATE;
    //     generally use -snes_fd_color or -snes_mf_operator
    ierr = DMDASNESSetJacobianLocal(da,
               (DMDASNESJacobian)Poisson2DJacobianLocal,&user); CHKERRQ(ierr);
    if (monitor) {
        ierr = SNESMonitorSet(snes,MSEMonitor,&user,NULL); CHKERRQ(ierr);
    }
    ierr = SNESSetFromOptions(snes); CHKERRQ(ierr);

    ierr = DMGetGlobalVector(da,&u_initial); CHKERRQ(ierr);
    if ((problem == CATENOID) && (mctx.q == -0.5) && (exact_init)) {
        ierr = DMDAGetLocalInfo(da,&info); CHKERRQ(ierr);
        ierr = FormExactFromG(&info,u_initial,&user); CHKERRQ(ierr);
    } else {
        // initial iterate has u=g on boundary and u=0 in interior
        ierr = InitialState(da, ZEROS, PETSC_TRUE, u_initial, &user); CHKERRQ(ierr);
    }

//STARTSNESSOLVE
    ierr = SNESSolve(snes,NULL,u_initial); CHKERRQ(ierr);
    ierr = DMRestoreGlobalVector(da,&u_initial); CHKERRQ(ierr);
    ierr = DMDestroy(&da); CHKERRQ(ierr);
    ierr = SNESGetDM(snes,&da); CHKERRQ(ierr);
    ierr = DMDAGetLocalInfo(da,&info); CHKERRQ(ierr);
    ierr = PetscPrintf(PETSC_COMM_WORLD,"done on %d x %d grid and problem %s",
                       info.mx,info.my,ProblemTypes[problem]); CHKERRQ(ierr);
    ierr = SNESGetSolution(snes,&u); CHKERRQ(ierr);
//ENDSNESSOLVE

    // evaluate numerical error in exact solution case
    if ((problem == CATENOID) && (mctx.q == -0.5)) {
        Vec    u_exact;
        double errnorm;
        ierr = VecDuplicate(u,&u_exact); CHKERRQ(ierr);
        ierr = FormExactFromG(&info,u_exact,&user); CHKERRQ(ierr);
        ierr = VecAXPY(u,-1.0,u_exact); CHKERRQ(ierr);    // u <- u + (-1.0) uexact
        ierr = VecDestroy(&u_exact); CHKERRQ(ierr);
        ierr = VecNorm(u,NORM_INFINITY,&errnorm); CHKERRQ(ierr);
        ierr = PetscPrintf(PETSC_COMM_WORLD,
                           ":  error |u-uexact|_inf = %.5e\n",errnorm); CHKERRQ(ierr);
    } else {
        ierr = PetscPrintf(PETSC_COMM_WORLD," ...\n"); CHKERRQ(ierr);
    }

    ierr = SNESDestroy(&snes); CHKERRQ(ierr);
    return PetscFinalize();
}

PetscErrorCode FormExactFromG(DMDALocalInfo *info, Vec uexact,
                         PoissonCtx *user) {
    PetscErrorCode ierr;
    int     i, j;
    double  xymin[2], xymax[2], hx, hy, x, y, **auexact;
    ierr = DMDAGetBoundingBox(info->da,xymin,xymax); CHKERRQ(ierr);
    hx = (xymax[0] - xymin[0]) / (info->mx - 1);
    hy = (xymax[1] - xymin[1]) / (info->my - 1);
    ierr = DMDAVecGetArray(info->da,uexact,&auexact); CHKERRQ(ierr);
    for (j = info->ys; j < info->ys + info->ym; j++) {
        y = j * hy;
        for (i = info->xs; i < info->xs + info->xm; i++) {
            x = i * hx;
            auexact[j][i] = user->g_bdry(x,y,0.0,user);
        }
    }
    ierr = DMDAVecRestoreArray(info->da,uexact,&auexact); CHKERRQ(ierr);
    return 0;
}

PetscErrorCode FormFunctionLocal(DMDALocalInfo *info, double **au,
                                 double **FF, PoissonCtx *user) {
    PetscErrorCode ierr;
    MinimalCtx *mctx = (MinimalCtx*)(user->addctx);
    int        i, j;
    double     xymin[2], xymax[2], hx, hy, hxhy, hyhx, x, y,
               ue, uw, un, us, une, use, unw, usw,
               dux, duy, De, Dw, Dn, Ds;
    ierr = DMDAGetBoundingBox(info->da,xymin,xymax); CHKERRQ(ierr);
    hx = (xymax[0] - xymin[0]) / (info->mx - 1);
    hy = (xymax[1] - xymin[1]) / (info->my - 1);
    hxhy = hx / hy;
    hyhx = hy / hx;
    for (j = info->ys; j < info->ys + info->ym; j++) {
        y = j * hy;
        for (i = info->xs; i < info->xs + info->xm; i++) {
            x = i * hx;
            if (j==0 || i==0 || i==info->mx-1 || j==info->my-1) {
                FF[j][i] = au[j][i] - user->g_bdry(x,y,0.0,user);
            } else {
                // assign neighbor values with either boundary condition or
                //     current u at that point (==> symmetric matrix)
                ue  = (i+1 == info->mx-1) ? user->g_bdry(x+hx,y,0.0,user)
                                          : au[j][i+1];
                uw  = (i-1 == 0)          ? user->g_bdry(x-hx,y,0.0,user)
                                          : au[j][i-1];
                un  = (j+1 == info->my-1) ? user->g_bdry(x,y+hy,0.0,user)
                                          : au[j+1][i];
                us  = (j-1 == 0)          ? user->g_bdry(x,y-hy,0.0,user)
                                          : au[j-1][i];
                if (i+1 == info->mx-1 || j+1 == info->my-1) {
                    une = user->g_bdry(x+hx,y+hy,0.0,user);
                } else {
                    une = au[j+1][i+1];
                }
                if (i-1 == 0 || j+1 == info->my-1) {
                    unw = user->g_bdry(x-hx,y+hy,0.0,user);
                } else {
                    unw = au[j+1][i-1];
                }
                if (i+1 == info->mx-1 || j-1 == 0) {
                    use = user->g_bdry(x+hx,y-hy,0.0,user);
                } else {
                    use = au[j-1][i+1];
                }
                if (i-1 == 0 || j-1 == 0) {
                    usw = user->g_bdry(x-hx,y-hy,0.0,user);
                } else {
                    usw = au[j-1][i-1];
                }
                // gradient  (dux,duy)   at east point  (i+1/2,j):
                dux = (ue - au[j][i]) / hx;
                duy = (un + une - us - use) / (4.0 * hy);
                De = DD(dux * dux + duy * duy, mctx->q);
                // ...                   at west point  (i-1/2,j):
                dux = (au[j][i] - uw) / hx;
                duy = (unw + un - usw - us) / (4.0 * hy);
                Dw = DD(dux * dux + duy * duy, mctx->q);
                // ...                  at north point  (i,j+1/2):
                dux = (ue + une - uw - unw) / (4.0 * hx);
                duy = (un - au[j][i]) / hy;
                Dn = DD(dux * dux + duy * duy, mctx->q);
                // ...                  at south point  (i,j-1/2):
                dux = (ue + use - uw - usw) / (4.0 * hx);
                duy = (au[j][i] - us) / hy;
                Ds = DD(dux * dux + duy * duy, mctx->q);
                // evaluate residual
                FF[j][i] = - hyhx * (De * (ue - au[j][i]) - Dw * (au[j][i] - uw))
                           - hxhy * (Dn * (un - au[j][i]) - Ds * (au[j][i] - us));
            }
        }
    }
    return 0;
}

// compute surface area and bounds on diffusivity using Q^1 elements and
// tensor product gaussian quadrature
PetscErrorCode MSEMonitor(SNES snes, int its, double norm, void *user) {
    PetscErrorCode ierr;
    PoissonCtx     *pctx = (PoissonCtx*)(user);
    MinimalCtx     *mctx = (MinimalCtx*)(pctx->addctx);
    DM             da;
    Vec            u, uloc;
    DMDALocalInfo  info;
    const Quad1D   q = gausslegendre[mctx->quaddegree-1];   // from ../quadrature.h
    double         xymin[2], xymax[2], hx, hy, **au, x_i, y_j, x, y,
                   ux, uy, W, D,
                   Dminloc = PETSC_INFINITY, Dmaxloc = 0.0, Dmin, Dmax,
                   arealoc = 0.0, area;
    int            i, j, r, s;
    MPI_Comm       comm;
    ierr = SNESGetDM(snes, &da); CHKERRQ(ierr);
    ierr = SNESGetSolution(snes, &u); CHKERRQ(ierr);
    ierr = DMGetLocalVector(da, &uloc); CHKERRQ(ierr);
    ierr = DMGlobalToLocalBegin(da, u, INSERT_VALUES, uloc); CHKERRQ(ierr);
    ierr = DMGlobalToLocalEnd(da, u, INSERT_VALUES, uloc); CHKERRQ(ierr);
    ierr = DMDAGetLocalInfo(da,&info); CHKERRQ(ierr);
    ierr = DMDAGetBoundingBox(info.da,xymin,xymax); CHKERRQ(ierr);
    hx = (xymax[0] - xymin[0]) / (info.mx - 1);
    hy = (xymax[1] - xymin[1]) / (info.my - 1);
    ierr = DMDAVecGetArrayRead(da,uloc,&au); CHKERRQ(ierr);
//STARTMONITORLOOP
    // loop over rectangular cells in grid
    for (j = info.ys; j < info.ys + info.ym; j++) {
        if (j == 0)
            continue;
        y_j = j * hy;  // NE corner of cell is (x_i,y_j)
        for (i = info.xs; i < info.xs + info.xm; i++) {
            if (i == 0)
                continue;
            x_i = i * hx;
            // loop over quadrature points in cell
            for (r = 0; r < q.n; r++) {
                x = x_i - hx + hx * 0.5 * (q.xi[r] + 1);
                for (s = 0; s < q.n; s++) {
                    y = y_j - hy + hy * 0.5 * (q.xi[s] + 1);
                    // gradient of u(x,y) at a quadrature point
                    ux =   (au[j][i] - au[j][i-1])     * (y - (y_j - hy))
                         + (au[j-1][i] - au[j-1][i-1]) * (y_j - y);
                    ux /= hx * hy;
                    uy =   (au[j][i] - au[j-1][i])     * (x - (x_i - hx))
                         + (au[j][i-1] - au[j-1][i-1]) * (x_i - x);
                    uy /= hx * hy;
                    W = ux * ux + uy * uy;
                    // min and max of diffusivity at quadrature points
                    D = DD(W,mctx->q);
                    Dminloc = PetscMin(Dminloc,D);
                    Dmaxloc = PetscMax(Dmaxloc,D);
                    // apply quadrature in surface area formula
                    arealoc += q.w[r] * q.w[s] * PetscSqrtReal(1.0 + W);
                }
            }
        }
    }
//ENDMONITORLOOP
    ierr = DMDAVecRestoreArrayRead(da,uloc,&au); CHKERRQ(ierr);
    ierr = DMRestoreLocalVector(da, &uloc); CHKERRQ(ierr);

    // do global reductions and report
    ierr = PetscObjectGetComm((PetscObject)da,&comm); CHKERRQ(ierr);
    arealoc *= hx * hy / 4.0;  // from change of variables formula
    ierr = MPI_Allreduce(&arealoc,&area,1,MPI_DOUBLE,MPI_SUM,comm); CHKERRQ(ierr);
    ierr = MPI_Allreduce(&Dminloc,&Dmin,1,MPI_DOUBLE,MPI_MIN,comm); CHKERRQ(ierr);
    ierr = MPI_Allreduce(&Dmaxloc,&Dmax,1,MPI_DOUBLE,MPI_MAX,comm); CHKERRQ(ierr);
    ierr = PetscPrintf(PETSC_COMM_WORLD,"area = %.8f; %.4f <= D <= %.4f\n",
               area,Dmin,Dmax); CHKERRQ(ierr);
    return 0;
}


#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_multifit.h>
#include <gsl/gsl_blas.h>

static int
hilbert_matrix(gsl_matrix * m)
{
  const size_t N = m->size1;
  const size_t M = m->size2;
  size_t i, j;

  for (i = 0; i < N; i++)
    {
      for (j = 0; j < M; j++)
        {
          gsl_matrix_set(m, i, j, 1.0/(i+j+1.0));
        }
    }

  return GSL_SUCCESS;
}

int
main()
{
  const size_t n = 10; /* number of observations */
  const size_t p = 8;  /* number of model parameters */
  size_t i;
  gsl_matrix *X = gsl_matrix_alloc(n, p);
  gsl_vector *y = gsl_vector_alloc(n);

  /* construct Hilbert matrix and rhs vector */
  hilbert_matrix(X);

  {
    double val = 1.0;
    for (i = 0; i < n; ++i)
      {
        gsl_vector_set(y, i, val);
        val *= -1.0;
      }
  }

  {
    const size_t npoints = 200;                   /* number of points on L-curve and GCV curve */
    gsl_multifit_linear_workspace *w =
      gsl_multifit_linear_alloc(n, p);
    gsl_vector *c = gsl_vector_alloc(p);          /* OLS solution */
    gsl_vector *c_lcurve = gsl_vector_alloc(p);   /* regularized solution (L-curve) */
    gsl_vector *c_gcv = gsl_vector_alloc(p);      /* regularized solution (GCV) */
    gsl_vector *reg_param = gsl_vector_alloc(npoints);
    gsl_vector *rho = gsl_vector_alloc(npoints);  /* residual norms */
    gsl_vector *eta = gsl_vector_alloc(npoints);  /* solution norms */
    gsl_vector *G = gsl_vector_alloc(npoints);    /* GCV function values */
    double lambda_l;                              /* optimal regularization parameter (L-curve) */
    double lambda_gcv;                            /* optimal regularization parameter (GCV) */
    double G_gcv;                                 /* G(lambda_gcv) */
    size_t reg_idx;                               /* index of optimal lambda */
    double rcond;                                 /* reciprocal condition number of X */
    double chisq, rnorm, snorm;

    /* compute SVD of X */
    gsl_multifit_linear_svd(X, w);

    rcond = gsl_multifit_linear_rcond(w);
    fprintf(stderr, "matrix condition number = %e\n", 1.0 / rcond);

    /* unregularized (standard) least squares fit, lambda = 0 */
    gsl_multifit_linear_solve(0.0, X, y, c, &rnorm, &snorm, w);
    chisq = pow(rnorm, 2.0);

    fprintf(stderr, "\n=== Unregularized fit ===\n");
    fprintf(stderr, "residual norm = %g\n", rnorm);
    fprintf(stderr, "solution norm = %g\n", snorm);
    fprintf(stderr, "chisq/dof = %g\n", chisq / (n - p));

    /* calculate L-curve and find its corner */
    gsl_multifit_linear_lcurve(y, reg_param, rho, eta, w);
    gsl_multifit_linear_lcorner(rho, eta, &reg_idx);

    /* store optimal regularization parameter */
    lambda_l = gsl_vector_get(reg_param, reg_idx);

    /* regularize with lambda_l */
    gsl_multifit_linear_solve(lambda_l, X, y, c_lcurve, &rnorm, &snorm, w);
    chisq = pow(rnorm, 2.0) + pow(lambda_l * snorm, 2.0);

    fprintf(stderr, "\n=== Regularized fit (L-curve) ===\n");
    fprintf(stderr, "optimal lambda: %g\n", lambda_l);
    fprintf(stderr, "residual norm = %g\n", rnorm);
    fprintf(stderr, "solution norm = %g\n", snorm);
    fprintf(stderr, "chisq/dof = %g\n", chisq / (n - p));

    /* calculate GCV curve and find its minimum */
    gsl_multifit_linear_gcv(y, reg_param, G, &lambda_gcv, &G_gcv, w);

    /* regularize with lambda_gcv */
    gsl_multifit_linear_solve(lambda_gcv, X, y, c_gcv, &rnorm, &snorm, w);
    chisq = pow(rnorm, 2.0) + pow(lambda_gcv * snorm, 2.0);

    fprintf(stderr, "\n=== Regularized fit (GCV) ===\n");
    fprintf(stderr, "optimal lambda: %g\n", lambda_gcv);
    fprintf(stderr, "residual norm = %g\n", rnorm);
    fprintf(stderr, "solution norm = %g\n", snorm);
    fprintf(stderr, "chisq/dof = %g\n", chisq / (n - p));

    /* output L-curve and GCV curve */
    for (i = 0; i < npoints; ++i)
      {
        printf("%e %e %e %e\n",
               gsl_vector_get(reg_param, i),
               gsl_vector_get(rho, i),
               gsl_vector_get(eta, i),
               gsl_vector_get(G, i));
      }

    /* output L-curve corner point */
    printf("\n\n%f %f\n",
           gsl_vector_get(rho, reg_idx),
           gsl_vector_get(eta, reg_idx));

    /* output GCV curve corner minimum */
    printf("\n\n%e %e\n",
           lambda_gcv,
           G_gcv);

    gsl_multifit_linear_free(w);
    gsl_vector_free(c);
    gsl_vector_free(c_lcurve);
    gsl_vector_free(reg_param);
    gsl_vector_free(rho);
    gsl_vector_free(eta);
    gsl_vector_free(G);
  }

  gsl_matrix_free(X);
  gsl_vector_free(y);

  return 0;
}


#ifndef KABSCH_RMSD
#define KABSCH_RMSD

#include <cblas.h>
#include <lapacke.h>

#include <iterator>

#include <iostream>
#include <fstream>

#include <tuple>

namespace kabsch
{

/**

	Calculates the RMSD

	@param P - coordinates of molecule P
	@param Q - coordinates of molecule Q
	@param N - number of atoms

	@return double - the RMSD

**/
template <class M>
double rmsd(
	M *P,
	M *Q,
	const unsigned int N)
{
    double rmsd {0.0};
    const unsigned int D {3};
	const unsigned int size = N*D;

    for(unsigned int i = 0; i < size; ++i) {
        rmsd += (P[i] - Q[i])*(P[i] - Q[i]);
    }

    return sqrt(rmsd/N);
}

template <class M>
M* centroid(
	M *coordinates,
	unsigned int n_atoms)
{
    double x {0};
    double y {0};
    double z {0};
    // unsigned int size = sizeof(coordinates);
    // unsigned int n_atoms = size / 3;

	const unsigned int size = n_atoms*3;

    unsigned int i = 0;
    while(i<size)
    {
        x += coordinates[i++];
        y += coordinates[i++];
        z += coordinates[i++];
    }

    x /= n_atoms;
    y /= n_atoms;
    z /= n_atoms;

	M *centroid = new M[3];
	centroid[0] = x;
	centroid[1] = y;
	centroid[2] = z;

    return centroid;
}

template <class Matrix>
Matrix* multiply(Matrix *A, Matrix *B,
    const int M,
    const int N,
    const int K)
{
    double one = 1.0;

	// TODO Matrix C[M*N] = {0}
	// Compilers doesnt like the following syntax
	Matrix *C = new Matrix[M*N] {0};

    cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans,
        M, N, K, one,
        A, K,
        B, N, one,
        C, N);

    return C;
}

template <class Matrix>
Matrix* transpose_multiply(
	Matrix *A,
	Matrix *B,
    const int M,
    const int N,
    const int K)
{
    double one = 1.0;

    Matrix *C = new Matrix[M*N] {0};

    cblas_dgemm(CblasRowMajor, CblasTrans, CblasNoTrans,
        M, N, K, one,
        A, M,
        B, N, one,
        C, N);

    return C;
}

template <class Matrix>
std::tuple<Matrix*, Matrix*, Matrix*> matrix_svd(
	Matrix *A,
	int rows,
	int cols)
{
    // lapack_int LAPACKE_dgesvd( int matrix_layout, char jobu, char jobvt,
    //     lapack_int m, lapack_int n,
    //     double* a, lapack_int lda,
    //     double* s, double* u, lapack_int ldu,
    //     double* vt, lapack_int ldvt,
    //     double* superb );

    Matrix *U = new Matrix[cols*rows] {0};
    Matrix *S = new Matrix[rows] {0};
    Matrix *VT = new Matrix[cols*rows] {0};
    Matrix *superb = new Matrix[cols*rows] {0};
    int info;

    info = LAPACKE_dgesvd(LAPACK_ROW_MAJOR, 'A', 'A',
        rows, cols,
        A, rows,
        S,
        U, rows,
        VT, rows,
        superb);

    if(info > 0)
    {
        // TODO What if failed
    }

	delete[] superb;

    return std::make_tuple(U, S, VT);
}

template <class M>
double determinant3x3(M A)
{
    // determinant of a square 3x3 matrix
    double det = A[0]*A[4]*A[8]
        +A[1]*A[5]*A[6]
        +A[2]*A[3]*A[7]
        -A[2]*A[4]*A[6]
        -A[1]*A[3]*A[8]
        -A[0]*A[5]*A[7];

    return det;
}

template <class M, class T>
M* kabsch(
	M *P,
	M *Q,
	const T n_atoms)
{
	// M *U = new M[3*3] {0};
	// U[0] = 1.0;
	// U[4] = 1.0;
	// U[8] = 1.0;
	// return U;

    M *U;
    M *S;
    M *V;

    M *C = transpose_multiply(P, Q, 3, 3, n_atoms);

	std::tie(U, S, V) = matrix_svd(C, 3, 3);

    // Getting the sign of the det(U)*(V) to decide whether we need to correct
    // our rotation matrix to ensure a right-handed coordinate system.
    if(determinant3x3(U)*determinant3x3(V) < 0.0)
    {
		U[3*0+2] = -U[3*0+2];
		U[3*2+2] = -U[3*1+2];
		U[3*1+2] = -U[3*2+2];
    }

    M *rotation = multiply(U, V, 3, 3, 3);

	delete[] C;
	delete[] U;
	delete[] S;
	delete[] V;

    return rotation;
}

template <class M, class T>
M* kabsch_rotate(
	M *P,
	M *Q,
	T n_atoms)
{
    M *U = kabsch(P, Q, n_atoms);
    M *product = multiply(P, U, n_atoms, 3, 3);
	delete[] U;
    return product;
}

template <class M, class T>
double kabsch_rmsd(
	M *P,
	M *Q,
	T n_atoms)
{
    M *P_rotated = kabsch_rotate(P, Q, n_atoms);
	double rmsdval = rmsd(P_rotated, Q, n_atoms);
	delete[] P_rotated;
	return rmsdval;
}

} // namespace rmsd

#endif


#include <stdio.h>
#include <gsl/gsl_fit.h>

int main (void)
{
  int n = 11;
  double x[11] = {10.0, 8.0, 13.0, 9.0, 11.0,
                  14.0 ,6.0, 4.0 , 12.0,7.0,5.0};
  double y[11] = {8.04, 6.95,7.68, 8.81, 8.33,
                  9.96, 7.24,4.26,10.84, 4.82,5.68 };

  double c0, c1, cov00, cov01, cov11, sumsq;

  gsl_fit_linear(x, 1, y, 1, n,
                 &c0, &c1, &cov00, &cov01, &cov11,
                 &sumsq);

  printf ("best fit: Y = %g + %g X\n", c0, c1);
  printf ("covariance matrix:\n");
  printf ("[ %g, %g\n  %g, %g]\n", cov00, cov01, cov01, cov11);
  printf ("sumsq = %g\n", sumsq);
  printf ("\n");

  // plot
  FILE *pipe = popen("gnuplot -persist", "w"); // Open a pipe to gnuplot
  if (pipe) // If gnuplot is found
  {
     fprintf(pipe, "set term wx\n");         // set the terminal
     fprintf(pipe, "set xlabel 'X'\n");
     fprintf(pipe, "set ylabel 'Y'\n");
     fprintf(pipe, "set xrange [0:20]\n");
     fprintf(pipe, "set yrange [2:14]\n");
     fprintf(pipe, "set title '<X,Y> and Linear fit:y=%.4f*x+%.4f'\n",c1,c0);

     /* In this case, the datafile is written directly to the gnuplot pipe with no need for a temporary file.
           The special filename '-' specifies that the data are inline; i.e., they follow the command.
        1 sending gnuplot the plot '-' command
        2 followed by data points
        3 followed by the letter "e"
     */

     // 1 sending gnuplot the plot '-' command
     fprintf(pipe, "plot '-' title '<x,y>' with points  pt 7 lc rgb 'blue',\
                         '-' title 'Line' with  linespoints  pt  6 lc rgb 'red'\n");

     // 2 followed by data points: <x,y>
     for (int i = 0; i < n; i++)
     {
        fprintf(pipe, "%lf %lf\n", x[i], y[i]);
     }
     // 3 followed by the letter "e"
     fprintf(pipe, "e");

     // linear fit
     fprintf(pipe,"\n"); // start a new draw item
     fprintf(pipe, "%lf %lf\n", 0.0, c0+c1*0,0);
     for (int i = 0; i < n; i++)
     {
        fprintf(pipe, "%lf %lf\n", x[i], c0+c1*x[i]);
     }
     fprintf(pipe, "%lf %lf\n", 20.0,c0+c1*20,0);
     fprintf(pipe, "e");

     fflush(pipe);
     fprintf(pipe, "exit \n"); // exit gnuplot
     pclose(pipe);             //close pipe
  }

  return 0;
}