double do_work(void) {
  double **a,           /* matrix A to be multiplied */
         **b,           /* matrix B to be multiplied */
         **c;           /* result matrix C */
  a = allocateMatrix(NRA, NCA);
  b = allocateMatrix(NCA, NCB);
  c = allocateMatrix(NRA, NCB);

  /*** Spawn a parallel region explicitly scoping all variables ***/

  initialize(a, NRA, NCA);
  initialize(b, NCA, NCB);
  initialize(c, NRA, NCB);

  compute(a, b, c, NRA, NCA, NCB);
  compute_interchange(a, b, c, NRA, NCA, NCB);

  double result = c[0][1];

  freeMatrix(a, NRA, NCA);
  freeMatrix(b, NCA, NCB);
  freeMatrix(c, NCA, NCB);

  return result;
}

/* 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;
}

int main(void)
{
    init();
    matrix_data_t m = {0};
    filesToMatrix(&m, "A.txt", "B.txt");

    /* A*B=C using thread pool */
    mulMatrix(&m);

    /* return codes of threads */

    printMatrix(&m.A);
    printf("---\n");
    printMatrix(&m.B);
    printf("---\n");
    printMatrix(&m.C);
    printf("---\n");

    /* mutex use show off */
    /* VERBOSE fprintf(stderr, "Sum C with threads\n"); */
    sumCMatrixElements(&m);
    printf("Sum = %f\n", theSum);

    freeMatrix(&m.A);
    freeMatrix(&m.B);
    freeMatrix(&m.C);
    return 0;
}

void DOsvd(float *a,float *res,float *comp,float *values,int nframes,int framesize,int compressedsize)
{
	int *remap;
	int i,j;
	int nrows;
	nrows=nframes;
	if (nrows<framesize)
		nrows=framesize;
	double **da=allocMatrix(nrows,framesize);
	double **v=allocMatrix(framesize,framesize);
	double *w=allocVect(framesize);
	float mx;
	int bestat;

	for (j=0;j<framesize;j++)
	{
		for (i=0;i<nframes;i++)
			da[j+1][i+1]=a[i*framesize+j];
		for (;i<nrows;i++)
			da[j+1][i+1]=0.0;
	}

	svdcmp(da,nrows,framesize,w,v);

	remap=new int[framesize];


	for (i=0;i<framesize;i++)
		remap[i]=-1;
	for (j=0;j<compressedsize;j++)
	{
		mx=-1.0f;
		for (i=0;i<framesize;i++)
		{
			if (remap[i]<0&&fabs(w[i+1])>mx)
			{
				mx=fabs(w[i+1]);
				bestat=i;
			}
		}
		assert(mx>-.5f);
		remap[bestat]=j;
	}
	// josh **DO NOT** put your dof>nframes mod here
	for (i=0;i<framesize;i++)
	{
		if (remap[i]<0)
			w[i+1]=0.0;
		else
		{
			values[remap[i]]=w[i+1];
			for (j=0;j<framesize;j++)
				res[remap[i]*framesize+j]=v[j+1][i+1];
		}
	}
	freeVect(w);
	freeMatrix(v,framesize);
	freeMatrix(da,nrows);
	delete[] remap;
}

 double capacitor(double a, double b, double c, double d, int n, double tol, double rel){
    double h=0.5*c/n; // Grid size.

    int na=round(0.5*a/h);
    // double x=linspace(0, 0.5*c, n+1);
    int m=round(0.5*d/h);
    int mb=round(0.5*b/h);
    // double y=linspace(0, 0.5*d, m+1);

    // Initialize potential and mask array.
    double** f = createMatrix(n+1, m+1, 0);
    double** mask = createMatrix(n+1, m+1, rel);

    for(int ii=0;ii<=na;ii++){
        for(int jj=0;jj<=mb;jj++){
            mask[ii][jj] = 0;
            f[ii][jj] = 1;
        }
    }
    double oldcap = 0, cap = 0;
    for(int iter=0;iter < 1000; iter++){
        seidel(f, mask, n, m, na, mb); 
        cap = gauss(n, m, h, f);
        if(fabs(cap-oldcap)/cap < tol)
            break;
        else
            oldcap = cap;
    }
    freeMatrix(f);
    freeMatrix(mask);
    return cap;
 }

void myMxM(Matrix A, Matrix v, Matrix u)
{
  Matrix temp = createMatrix(A->rows, v->cols);
#pragma omp parallel
  {
    int* displ, *cols;
    splitVector(v->cols, num_threads(), &cols, &displ);
    MxM2(A, v, temp, displ[get_thread()], cols[get_thread()],
         displ[get_thread()], 1.0, 0.0);
    free(cols);
    free(displ);
  }
#ifdef HAVE_MPI
  for (int i=0;i<v->as_vec->comm_size;++i) {
    Matrix t = subMatrix(temp, v->as_vec->displ[i]/v->cols,
                         v->as_vec->sizes[i]/v->cols, 0, v->cols);
    MPI_Reduce(t->data[0], u->data[0], v->as_vec->sizes[i],
               MPI_DOUBLE, MPI_SUM, i, *v->as_vec->comm);
    freeMatrix(t);
  }
#else
  memcpy(u->data[0], temp->data[0], u->as_vec->len*sizeof(double));
#endif
  freeMatrix(temp);
}

int main(int argc, char** argv)
{
  if (argc < 3) {
    printf("need two parameters, the matrix size and the number of vectors\n");
    return 1;
  }
  int N=atoi(argv[1]);
  int K=atoi(argv[2]);

  Matrix A = createMatrix(N,N);
  // identity matrix
  for (int i=0;i<N;++i)
    A->data[i][i] = 1.0;

  Matrix v = createMatrix(N,K);

  // fill with column number
  for (int i=0;i<K;++i)
    for (int j=0;j<N;++j)
      v->data[i][j] = i;

  Matrix v2 = createMatrix(N,K);
  double time = WallTime();
  MxM(A, v, v2, 1.0, 0.0);
  double sum = innerproduct(v->as_vec, v2->as_vec);

  printf("sum: %f\n", sum);
  printf("elapsed: %f\n", WallTime()-time);
  freeMatrix(v2);
  freeMatrix(v);
  freeMatrix(A);
  return 0;
}

int main(int argc, char ** argv) {
	int res;
	Matrix * A = readFromFile(argv[1]);
	Matrix * b = readFromFile(argv[2]);
	Matrix * x;

	if (A == NULL) return -1;
	if (b == NULL) return -2;
	printToScreen(A);
	printToScreen(b);

	res = eliminate(A,b);
	x = createMatrix(b->r, 1);
	if (x != NULL) {
		res = backsubst(x,A,b);

		printToScreen(x);
	  freeMatrix(x);
	} else {
					fprintf(stderr,"Błąd! Nie mogłem utworzyć wektora wynikowego x.\n");
	}

	freeMatrix(A);
	freeMatrix(b);

	return 0;
}

void freeImage(image * img)
{
    freeMatrix(img->red);
    freeMatrix(img->green);
    freeMatrix(img->blue);
    free(img);
}

int main(int argc, char** argv)
{
  int N, K, i, j;
  Matrix A,v;
  double time, sum;

  if (argc < 3) {
    printf("need two parameters, the matrix size and the number of vectors\n");
    return 1;
  }

  N=atoi(argv[1]);
  K=atoi(argv[2]);

  A = createMatrix(N,N);
  // identity matrix
  for (i=0;i<N;++i)
    A->data[i][i] = 1.0;

  v = createMatrix(N,K);
  // fill with column number
  for (i=0;i<K;++i)
    for (j=0;j<N;++j)
      v->data[i][j] = i;

  time = WallTime();
  sum = dosum(A,v);
  printf("sum: %f\n", sum);
  printf("elapsed: %f\n", WallTime()-time);
  freeMatrix(v);
  freeMatrix(A);

  return 0;
}

void cg(eval_t A, Matrix b, double tolerance, void* ctx)
{
  Matrix r = createMatrix(b->rows, b->cols);
  Matrix p = createMatrix(b->rows, b->cols);
  Matrix buffer = createMatrix(b->rows, b->cols);
  double dotp = 1000;
  double rdr = dotp;
  copyVector(r->as_vec,b->as_vec);
  fillVector(b->as_vec, 0.0);
  int i=0;
  while (i < b->as_vec->len && rdr > tolerance) {
    ++i;
    if (i == 1) {
      copyVector(p->as_vec,r->as_vec);
      dotp = innerproduct(r->as_vec,r->as_vec);
    } else {
      double dotp2 = innerproduct(r->as_vec,r->as_vec);
      double beta = dotp2/dotp;
      dotp = dotp2;
      scaleVector(p->as_vec,beta);
      axpy(p->as_vec,r->as_vec,1.0);
    }
    A(buffer,p,ctx);
    double alpha = dotp/innerproduct(p->as_vec,buffer->as_vec);
    axpy(b->as_vec,p->as_vec,alpha);
    axpy(r->as_vec,buffer->as_vec,-alpha);
    rdr = sqrt(innerproduct(r->as_vec,r->as_vec));
  }
  printf("%i iterations\n",i);
  freeMatrix(r);
  freeMatrix(p);
  freeMatrix(buffer);
}

/*===========================================================================
 * eigenvector
 * This algorithm determines the eigenvector of a matrix given an eigenvalue.
 *=========================================================================*/
matrix* eigenvector(matrix* a, double eigenvalue) {
    matrix* b; // This matrix will store A-eI
    matrix* zero; // This matrix will store a column vector of zeros
    matrix* out;
    double* ptr;
    int i;

    assert(a->width == a->height, "Matrix must be square.");

    // Create our column vector of zeros
    zero = makeMatrix(1, a->height);

    // Copy A
    b = copyMatrix(a);

    // Subtract eigenvalue from the diagonal elements
    ptr = b->data;
    for (i = 0; i < b->height; i++) {
        *ptr -= eigenvalue;
        ptr += b->width + 1;
    }

    // Find the eigenvector
    out = solver(b, zero);

    freeMatrix(b);
    freeMatrix(zero);
    return out;
}

/*===========================================================================
 * powerMethod
 * This algorithm determines the largest eigenvalue of a matrix using the
 * power method.
 *
 * This was described to me in a Randomized Algoirthms course.
 *=========================================================================*/
double powerMethod(matrix* a) {
    matrix* x;
    matrix* xp1; // x plus 1
    const double EPSILON = 0.001;
    double sum;
    int i;
    int k = 0;
    int converge;

    assert(a->width == a->height, "Matrix must be square.");

    srand(time(0)); // Initalize our RNG

    // Let's initalize x to a random vector
    x = makeMatrix(1, a->height);
    for (i = 0; i < a->height; i++) {
        x->data[i] = (double) rand() / RAND_MAX;
    }

    // Iterate until the x vector converges.
    while (1) {
        k++;

        // Multiply A * x
        xp1 = multiplyMatrix(a, x);

        // Add up all of the values in xp1
        sum = 0;
        for (i = 0; i < a->height; i++) {
            sum += xp1->data[i];
        }

        // Divide each value in xp1 by sum. (Normalize)
        for (i = 0; i < a->height; i++) {
            xp1->data[i] /= sum;
        }

        // Test to see if we need to quit.
        converge = 1; // Converged
        for (i = 0; i < a->height; i++) {
            if (fabs(x->data[i] - xp1->data[i]) >= EPSILON) {
                converge = 0; // Not converged.
                break;
            }
        }

        // Set up for the next loop.
        freeMatrix(x);
        x = copyMatrix(xp1);
        freeMatrix(xp1);

        // Really test for quit.
        if (converge == 1) {
            break;
        }
    }

    freeMatrix(x);
    return sum;
}

void matrixSquareRoot(Matrix* A, Matrix* S)
{
  //Approach is to do an SVD of A, then set S to V * sqrt(Sigma) * Vtransposed
  int i, j;

  Matrix* U = allocateMatrix(A->rows, A->columns);
  Matrix* Vtransposed = allocateMatrix(A->rows, A->columns);
  Matrix* Sigma = allocateMatrix(A->rows, A->columns);

  //First do an SVD
  singularValueDecomposition(A, U, Vtransposed, Sigma);

  //Now calculate sqrt(Sigma) * Vtransposed and stick it in U
  for(i = 0; i < Sigma->rows; i++)
  {
    for(j = 0; j < Sigma->columns; j++)
    {
      U->pointer[i + j * U->rows] = Vtransposed->pointer[i + j * Vtransposed->rows] * sqrt(Sigma->pointer[i + i * Sigma->rows]);
    }
  }

  multiplyMatrices(Vtransposed, U, S, 1, 0);

  freeMatrix(U);
  freeMatrix(Vtransposed);
  freeMatrix(Sigma);
}

int main(int argc, char *argv[])
{

	if(argc < 2) {
		printf("Usage: matrix-thread nThreads\n");
		return -1;
	}
	numThreads = atoi(argv[1]);
	A = readMatrix(IN_1);
	B = readMatrix(IN_2);
	if(A->numCols != B->numRows) {
		fprintf(stderr, "Wrong matrix dimensions\n");
		return -1;
	}
	C = makeMatrix(A->numRows, B->numCols);

	dispatchThreads();

	writeMatrix(OUTPUT_FILE, C);

	freeMatrix(A);
	freeMatrix(B);
	freeMatrix(C);
	return 0;
}

void free_BS(Barklemstruct *bs)
{
  free(bs->neff1);
  free(bs->neff2);

  freeMatrix((void **) bs->cross);
  freeMatrix((void **) bs->alpha);
}

Matrix
build_A(int m){
    Matrix K = build_K(2*m);
    Matrix L = build_L(2*m);
    Matrix A = matrixMult(K, L);
    freeMatrix(K);
    freeMatrix(L);
    return A;
}

/**
eigentrain

computes the eigen space for matrix images.

This function is used in the training procedure of the face recognition pca
algorithm.
INPUT:  the data matrix of images
OUTPUT: mean: the mean value of the images
eigen_values: eigenvalues
eigen_base: eigenvectors

The data matrix is mean centered, and this is a side effect.
*/
void eigentrain(Matrix *mean, Matrix *eigen_vals,
                Matrix *eigen_base, Matrix images) 
{
    double p = 0.0;
    Matrix M, eigenvectors;

    DEBUG(1, "Calculating mean image.");
    *mean = get_mean_image(images);

    DEBUG(1, "Calculating the mean centered images for the training set.");
    mean_subtract_images(images, *mean);

    MESSAGE2ARG("Calculating Covariance Matrix: M = images' * images. M is a %d by %d Matrix.", images->col_dim, images->col_dim);
    M = transposeMultiplyMatrixL(images, images);
    DEBUG_INT(3, "Covariance Matrix Rows", M->row_dim);
    DEBUG_INT(3, "Covariance Matrix Cols", M->col_dim);

    DEBUG(2, "Allocating memory for eigenvectors and eigenvalues.");
    eigenvectors = makeMatrix(M->row_dim, M->col_dim);
    *eigen_vals = makeMatrix(M->row_dim, 1);


    MESSAGE("Computing snap shot eigen vectors using the double precision cv eigensolver.");

    cvJacobiEigens_64d(M->data, eigenvectors->data, (*eigen_vals)->data, images->col_dim, p, 1);

    freeMatrix(M);

    DEBUG(1, "Verifying the eigen vectors");
    /* Reconstruct M because it is destroyed in cvJacobiEigens */
    M = transposeMultiplyMatrixL(images, images);
    if (debuglevel >= 3)
        eigen_verify(M, *eigen_vals, eigenvectors);
    freeMatrix(M);

    *eigen_base = multiplyMatrix(images, eigenvectors);
    MESSAGE2ARG("Recovered the %d by %d high resolution eigen basis.", (*eigen_base)->row_dim, (*eigen_base)->col_dim);

    DEBUG(1, "Normalizing eigen basis");
    basis_normalize(*eigen_base);

    /*Remove last elements because they are unneeded.  Mean centering the image
    guarantees that the data points define a hyperplane that passes through
    the origin. Therefore all points are in a k - 1 dimensional subspace.
    */
    (*eigen_base)->col_dim -= 1;
    (*eigen_vals)->row_dim -= 1;
    eigenvectors->col_dim -= 1;

    DEBUG(1, "Verifying eigenbasis");
    if (debuglevel >= 3)
        basis_verify(images, *eigen_base);

    /* The eigenvectors for the smaller covariance (snap shot) are not needed */
    freeMatrix(eigenvectors);
}

void me_o(double *X, int *N, char **labels, char **treeStr, int *nni)
{
  double **D, **A;
  set *species, *slooper;
  node *addNode;
  tree *T;
  char *str;
  int n, nniCount;

  n = *N;
  T = NULL;
  nniCount = 0;
  species = (set *) malloc(sizeof(set));
  species->firstNode = NULL;
  species->secondNode = NULL;
  str = (char *)R_alloc(MAX_INPUT_SIZE, sizeof(char));
  /* added by EP */
  if (strlen(str))
    strncpy(str, "", strlen(str));
  /* end */

  D = loadMatrix (X, labels, n, species);
  A = initDoubleMatrix(2 * n - 2);

  for(slooper = species; NULL != slooper; slooper = slooper->secondNode)
  {
    addNode = copyNode(slooper->firstNode);
    T = GMEaddSpecies(T,addNode,D,A);
  }
  makeOLSAveragesTable(T,D,A);
  // Compute NNI
  if (*nni == 1)
    NNI(T,A,&nniCount,D,n);
  assignOLSWeights(T,A);

  NewickPrintTreeStr(T,str);

  if (strlen (str) < MAX_INPUT_SIZE -1)
    {
      *treeStr = (char *)R_alloc(MAX_INPUT_SIZE, sizeof(char));
      /* added by EP */
      if (strlen(*treeStr))
	strncpy(*treeStr, "", strlen(*treeStr));
      /* end */
      strncpy (*treeStr, str, strlen (str));
    }

 /*  free (str); */
  freeMatrix(D,n);
  freeMatrix(A,2*n - 2);
  freeSet(species);
  freeTree(T);
  T = NULL;

  return;
}

void
gaussianEstimator_Pred ( Matrix *xEst, Matrix *CEst, Matrix *U, Matrix *Cw, Matrix (*afun)(Matrix m, float t), float *dt, Matrix *m_opt)
{
float D = elem(sizeOfMatrix(*m_opt),0,1)+1;       //printf("%f\n", D);
float w_opt = 1/D;                                //printf("%f\n", w_opt);
float Nx = elem(sizeOfMatrix(*xEst),0,0);         //printf("%f\n", Nx);
float d = Nx*(D-1) + 1;                           //printf("%f\n", d);
float w = 1/d;                                    //printf("%f\n", w);

/* Eigenvectors, Eigenvalues */
int dimC = elem ( sizeOfMatrix(*CEst), 0, 0 );
Matrix Vec = zeroMatrix(dimC, dimC);    
Matrix Val = zeroMatrix(dimC, dimC);
eig ( CEst, &Vec, &Val );

/* m1 = vec*sqrtf(val) */
int i;
for ( i = 0; i < dimC; ++i )
    setElem(Val, i, i, sqrtf(fabs(elem(Val, i,i))));
Matrix m1 = mulMatrix(Vec, Val);

freeMatrix(Vec);
freeMatrix(Val);

/*  rotate & scale samples: m = m1*S */
Matrix m = scaledSamplePoints(m1, *m_opt);

/* x = x*ones(1,d) */
Matrix x = fillMatrix(*xEst, d);

/* shift samples: m = m + x */
m = addMatrix(m, x);              //printMatrix(m);

/* afun */
/* Prediction: mean
   xPredDiracs = feval(afun,m, [], [], t);
   xPred = w*sum(xPredDiracs, 2);*/
   
Matrix xPredDiracs = (*afun) (m, *dt);                   //printMatrix(xPredDiracs);
Matrix xPredDiracsSum = sumMatrix(xPredDiracs, 2);       //printMatrix(xPredDiracsSum);
Matrix xPred = mulScalarMatrix(w, xPredDiracsSum);       //printMatrix(xPred);

//mxDiracs = xPredDiracs-repmat(xPred, 1, d);
//CPred = w_opt*mxDiracs*mxDiracs';
Matrix mxDiracs = subMatrix(xPredDiracs, fillMatrix(xPred, d));   //printMatrix(mxDiracs);
Matrix CPred = mulScalarMatrix(w_opt, mulMatrix(mxDiracs, transposeMatrix(mxDiracs)));  //printMatrix(CPred);


//RETURN
*xEst = xPred;        //printMatrix(*xEst);
*CEst = CPred;        //printMatrix(*CEst);

freeMatrix(m);
freeMatrix(xPredDiracs);
freeMatrix(xPredDiracsSum);
}