Matrix transposeMultiplyMatrixR(const Matrix A, const Matrix B) {
    /** creates a new matrix that is the product of the A and (B transpose) */

    int i, j, k;                                   /* Variables used as indices */
    Matrix P = makeMatrix(A->row_dim, B->row_dim); /* Product A * B */

    DEBUG(10, "Multiplying Matrix");

    assert(A->row_dim == B->row_dim); /* Verify that the Matrices can be multiplied */

    /* Optimized code */
    for ( i = 0; i < A->row_dim; i++) {
        for ( j = 0; j < B->row_dim; j++) {
            ME( P, i, j) = 0;
        }
    }

    for (k = 0; k < A->col_dim; k++) {
        for ( i = 0; i < A->row_dim; i++) {
            for ( j = 0; j < B->row_dim; j++) {
                ME( P, i, j) += ME(A, i, k) * ME(B, j, k);
            }
        }
    }

    return P;
}

/* This method performs an L2 based distance measure
after solving for a best fit transformation.  The
best fit transformation is a combination of a 2D
scale, rotation and translation.  The algorithm
solves for this transformation using a least squares
solution to a set of transformation aproximations. */
FTYPE GeometrySimLeastSquaresNLS(FaceGraph f1, FaceGraph f2){
    int i;
    FTYPE dist = 0.0;

    Matrix g1 = makeMatrix(f1->geosize*2,1);
    Matrix g2 = makeMatrix(f1->geosize*2,1);

    for (i = 0; i < f1->geosize; i++) {
        FTYPE dedx=0, dedy=0;
        DENarrowingLocalSearch(f1->jets[i],f2->jets[i],&dedx,&dedy);

        ME(g1,2*i,0)   = f1->jets[i]->x ;
        ME(g1,2*i+1,0) = f1->jets[i]->y ;
        ME(g2,2*i,0)   = f2->jets[i]->x + dedx;
        ME(g2,2*i+1,0) = f2->jets[i]->y + dedy;
    }

    TransformLeastSquares(g1,g2);

    dist = L2Dist(g1, g2);

    freeMatrix(g1);
    freeMatrix(g2);

    return dist;
}

void rowMultAdd(Matrix m, int rSrc, int rDest, FTYPE value) {
    int col = 0;

    for(col = 0; col < m->col_dim; col++) {
        ME(m,rDest,col) += value*ME(m,rSrc,col);
    }
}

void fisherVerify(Matrix fisherBasis, Matrix fisherValues, Matrix Sw, Matrix Sb) {
    Matrix SbW = multiplyMatrix(Sb, fisherBasis);
    Matrix SwW = multiplyMatrix(Sw, fisherBasis);
    Matrix D = makeIdentityMatrix(fisherBasis->row_dim);
    Matrix DSwW;
    Matrix zeroMat;
    int i, j;


    MESSAGE("Verifying Fisher Basis.");

    for (i = 0; i < D->row_dim; i++) {
        ME(D, i, i) = ME(fisherValues, i, 0);
    }

    DSwW = multiplyMatrix(D, SwW);
    zeroMat = subtractMatrix(SbW, DSwW);

    for (i = 0; i < zeroMat->row_dim; i++) {
        for (j = 0; j < zeroMat->col_dim; j++) {
            if (!EQUAL_ZERO(ME(zeroMat, i, j), 0.000001)) {
                DEBUG( -1, "Fisher validation failed.");
                printf("Element: (%d,%d) value = %f", i, j, ME(zeroMat, i, j));
                exit(1);
            }
        }
    }
}

/* Return a scale Matrix */
Matrix scaleMatrix(double s){
    Matrix transform = makeIdentityMatrix(3);
    ME(transform,0,0) = s;
    ME(transform,1,1) = s;

    return transform;
}

/* invert a matrix using Gaussian Elimination */
Matrix invertRREF(Matrix m) {
    int prealloc = alloc_matrix;
    int i,j;
    Matrix tmp = makeZeroMatrix(m->row_dim, m->col_dim*2);
    Matrix inverse = makeMatrix(m->row_dim, m->col_dim);
    DEBUG_CHECK(m->row_dim == m->col_dim,"Matrix can only be inverted if it is square");
    /* build the tmp Matrix which will be passed to RREF */
    for( i = 0; i < m->row_dim; i++) {
        for( j = 0; j < m->col_dim; j++) {
            ME(tmp,i,j) = ME(m,i,j);
            if(i == j) {
                ME(tmp,i,j+m->col_dim) = 1;
            }
        }
    }
    matrixRREF(tmp);

    for( i = 0; i < m->row_dim; i++) {
        for( j = 0; j < m->col_dim; j++) {
            ME(inverse,i,j) = ME(tmp,i,j+m->col_dim);
        }
    }

    freeMatrix(tmp);

    if(prealloc != alloc_matrix - 1) {
        printf("Error deallocating matricies <%s>: pre=%d post=%d",__FUNCTION__, prealloc, alloc_matrix);
        exit(1);
    }
    return inverse;
}

Matrix reflectMatrix(int bool_x, int bool_y){
    Matrix transform = makeIdentityMatrix(3);

    if(bool_x) ME(transform,0,0) = -1;
    if(bool_y) ME(transform,1,1) = -1;
    return transform;
}

/* Return a translation matrix */
Matrix translateMatrix(double dx, double dy){
    Matrix transform = makeIdentityMatrix(3);

    ME(transform,0,2) = dx;
    ME(transform,1,2) = dy;

    return transform;
}

void
mean_subtract_images (Matrix images, Matrix mean)
{
    int i, j;
    for (i = 0; i < images->row_dim; i++) {
        for (j = 0; j < images->col_dim; j++) {
            ME(images, i, j) -= ME(mean, i, 0);
        }
    }
}

/* Return a rotation matrix */
Matrix rotateMatrix(double theta){
    Matrix transform = makeIdentityMatrix(3);

    ME(transform,0,0) = cos(theta);
    ME(transform,1,1) = cos(theta);
    ME(transform,0,1) = -sin(theta);
    ME(transform,1,0) = sin(theta);

    return transform;
}

void rowSwap(Matrix m, int rSrc, int rDest) {
    int col = 0;
    FTYPE tmp;

    for(col = 0; col < m->col_dim; col++) {
        tmp = ME(m,rSrc,col);
        ME(m,rSrc,col) = ME(m,rDest,col);
        ME(m,rDest,col) = tmp;
    }
}

Matrix duplicateMatrix(const Matrix mat) {
    Matrix dup = makeMatrix(mat->row_dim, mat->col_dim);
    int i, j;
    for (i = 0; i < mat->row_dim; i++) {
        for (j = 0; j < mat->col_dim; j++) {
            ME(dup, i, j) = ME(mat, i, j);
        }
    }

    return dup;
}

FTYPE L2Dist(Matrix g1, Matrix g2){
    FTYPE dist = 0.0;
    int i;

    for (i = 0; i < g1->row_dim/2; i++) {
        dist += SQR(ME(g1,2*i,0) - ME(g2,2*i,0));
        dist += SQR(ME(g1,2*i+1,0) - ME(g2,2*i+1,0));
    }

    return sqrt(dist);
}

/*
This function reads images in to a vector.  That vector is then mean subtracted
and then projected onto an optimal basis (PCA, LDA or LPP). Returned is a matrix
that contains the images after they have been projected onto the subspace.
*/
Matrix
readAndProjectImages (Subspace *s, char *imageNamesFile, char *imageDirectory, int *numImages, ImageList **srt)
{
    int i, j;
    Matrix images, vector, smallVector;
    char name[FILE_LINE_LENGTH];
    ImageList *subject, *replicate;

    DEBUG(1, "Reading training file names from file");

    *srt = getImageNames(imageNamesFile, numImages);

    DEBUG_CHECK(*srt, "Error: header no imagenames found in file image list file");

    /* Automatically determine number of pixels in images    */
    sprintf(name, "%s/%s", imageDirectory, (*srt)->filename);
    DEBUG(1, "Autodetecting number of pixels, i.e. vector length based on the size of image 0.");
    DEBUG_CHECK (autoFileLength(name) == s->numPixels, "Images sizes do not match subspace basis vector size");
    DEBUG_INT(1, "Vector length", s->numPixels);
    DEBUG_CHECK(s->numPixels > 0, "Error positive value required for a Vector Length");

    /*Images stored in the columns of a matrix */
    DEBUG(1, "Allocating image matrix");

    images = makeMatrix(s->basis->col_dim, *numImages);
    vector = makeMatrix(s->numPixels, 1);

    i = 0;
    for (subject = *srt; subject; subject = subject->next_subject) {
        for (replicate = subject; replicate; replicate = replicate->next_replicate) {
            if (debuglevel > 0)
                printf("%s ", replicate->filename);
            sprintf(name, "%s/%s", imageDirectory, replicate->filename);
            replicate->imageIndex = i;
            readFile(name, 0, vector);

            writeProgress("Reading images", i,*numImages);

            smallVector = centerThenProjectImages(s, vector);

            /* Copy the smaller vector into the image matrix*/
            for (j = 0; j < smallVector->row_dim; j++) {
                ME(images, j, i) = ME(smallVector, j, 0);
            }
            freeMatrix(smallVector);
            i++;  /* increament the image index */
        }
        if (debuglevel > 0)
            printf("\n");
    }

    return images;
}

void matrixRREF(Matrix m) {
    int prealloc = alloc_matrix;
    int pivotCol = 0;
    int pivotRow = 0;
    int row;
    FTYPE absVal;
    int tmp = 0;

    while(1) {
        /* Select the row with the largest absolute value, or move to the next row
        if there is no value int the column */
        absVal = 0.0;
        while( absVal == 0.0 && pivotCol < m->col_dim) {
            absVal = ABS(ME(m,pivotRow,pivotCol));
            tmp = pivotRow;

            for(row = pivotRow+1; row < m->row_dim; row++) {
                if( ABS(ME(m,row,pivotCol)) > absVal ) {
                    absVal = ABS(ME(m,row,pivotCol));
                    tmp = row;
                }
            }
            if(absVal == 0) {
                pivotCol++;
            }
        }

        /* return if the RREF has been found */
        if( pivotCol >= m->col_dim || pivotRow >= m->row_dim) return;

        /* make sure that the pivot row is in the correct position */
        if(pivotRow != tmp) rowSwap(m,tmp,pivotRow);

        /* rescale the Pivot Row */
        rowMult( m, pivotRow,1.0/ME(m,pivotRow,pivotCol) );

        /* This part of the algorithm is not as effecent as it could be,
            but it works for now. */
        for(row = 0; row < m->row_dim; row++) {
            if(row != pivotRow) {
                rowMultAdd(m,pivotRow,row,-ME(m,row,pivotCol));
            }
        }
        pivotRow++;
        pivotCol++;
    }

    if(prealloc != alloc_matrix) {
        printf("Error deallocating matricies <%s>: pre=%d post=%d",__FUNCTION__, prealloc, alloc_matrix);
        exit(1);
    }
}

/**
Verify properties of the eigen basis used for pca.

The eigenbasis should be ortonormal: U'*U - I == 0
The basis should be decomposed such that: X == U*D*V'

returns true if tests fail.
*/
int basis_verify(Matrix X, Matrix U) {
    Matrix UtX = transposeMultiplyMatrixL(U, X);
    Matrix UUtX = multiplyMatrix(U, UtX);
    Matrix UtU = transposeMultiplyMatrixL(U, U);
    Matrix identity = makeIdentityMatrix(U->col_dim);
    Matrix test;
    int i, j;
    int failed = 0;
    const double tol = 1.0e-7;

    freeMatrix(UtX);

    DEBUG(2, "Checking orthogonality of eigenbasis");
    test = subtractMatrix(UtU, identity);
    freeMatrix(UtU);
    freeMatrix(identity);

    for (i = 0; i < test->row_dim; i++) {
        for (j = 0; j < test->col_dim; j++) {
            if (!EQUAL_ZERO(ME(test, i, j), tol)) {
                failed = 1;
                MESSAGE("Eigenbasis is not orthogonal to within tolerance.");
                DEBUG_DOUBLE(1, "Matrix Element", ME(test, i, j));
                DEBUG_DOUBLE(1, "Tolerance", tol);
                exit(1);
            }
        }
    }
    freeMatrix(test);

    DEBUG(2, "Checking reconstruction property of the eigen decomposition");
    test = subtractMatrix(X, UUtX);
    freeMatrix(UUtX);

    for (i = 0; i < test->row_dim; i++) {
        for (j = 0; j < test->col_dim; j++) {
            if (!EQUAL_ZERO(ME(test, i, j), tol)) {
                failed = 1;
                MESSAGE("Data matrix is not reconstructable to within tolerance.");
                DEBUG_DOUBLE(1, "Matrix Element", ME(test, i, j));
                DEBUG_DOUBLE(1, "Tolarence", tol);
                exit(1);
            }
        }
    }
    freeMatrix(test);

    return failed;


}

Matrix matrixCols( const Matrix mat, int col1, int col2) {
    Matrix cols = makeMatrix(mat->row_dim, col2 - col1 + 1);
    int i, j;

    DEBUG_CHECK(col1 <= col2 && col2 < mat->col_dim, "Poorly chosen columns for extract columns operation");

    for (i = col1; i <= col2; i++) {
        for (j = 0; j < mat->row_dim; j++) {
            ME(cols, j, i - col1) = ME(mat, j, i);
        }
    }

    return cols;
}

void addMatrixEquals(Matrix A, const Matrix B) {
    /** output A += B */
    int i, j;
    DEBUG(10, "Adding Matrix");

    assert(A->row_dim == B->row_dim);
    assert(A->col_dim == B->col_dim);

    for (i = 0; i < A->row_dim; i++) {
        for (j = 0; j < A->col_dim; j++) {
            ME(A, i, j) += ME(B, i, j);
        }
    }
}

Matrix transposeMatrix(const Matrix A) {
    /* creates a new matrix that is the transpose of A */
    int i, j;                                      /* Variables used as indices */
    Matrix T = makeMatrix(A->col_dim, A->row_dim); /* Transpose of A */

    DEBUG(10, "Transposing Matrix");

    /** Transpose data */
    for ( i = 0; i < A->row_dim; i++) {
        for ( j = 0; j < A->col_dim; j++) {
            ME(T, j, i) = ME(A, i, j);
        }
    }

    return T;
}

void test_multipole_forces_stackel(int p=0, int q=0,std::string qq="No"){

    TestDensity_Stackel rho(1.,-30.,-10.);
    TriaxialPotential T(&rho,1e8);
    MultipoleExpansion ME(&rho,100,30,16,-1,10.,0.01,100.,false,true,true);
    ME.visualize("me.vis");

    int NMAX = 50;
    VecDoub xx(NMAX,0), exact(NMAX,0), triaxial(NMAX,0), multipole(NMAX,0);

    #pragma omp parallel for schedule(dynamic)
    for(int xn = 0; xn<NMAX; xn++){
        double x = (double)xn+.1;
        xx[xn] = x;
        VecDoub X(3,1);
        X[p]=x;
        if(qq=="general"){
            X[0]=x/2.;X[1]=x/3.;X[2]=x;
        }
        exact[xn] = rho.Forces(X)[q];
        triaxial[xn] = T.Forces(X)[q];
        multipole[xn] = ME.Forces(X)[q];
    }

    Gnuplot G("lines ls 1");
    G.set_xrange(0.9*Min(xx),1.1*Max(xx));
    G.set_yrange(0.9*Min(exact),1.1*Max(exact));
    G.set_xlabel("x").set_ylabel("Potential");
    G.savetotex("multipole_density_test").plot_xy(xx,exact);
    G.set_style("lines ls 2").plot_xy(xx,triaxial);
    G.set_style("lines ls 3").plot_xy(xx,multipole);
    G.outputpdf("multipole_density_test");
}