#include <math.h>
#include <string.h>

#include "mex.h"

/* boompf.c */

/* Computational subroutines */

/* removeCol
 * Remove column i of the m*n matrix mat; the columns at the right of column i are shifted to the left
 * mat: pointer to the original matrix
 * new_mat: pointer to the matrix mat with column i removed and columns at the right of column i shifted to the left
 * m: number of lines of mat
 * n: number of columns of mat
 * i: index of the column to be removed
*/
void removeCol(double *new_mat, double *mat, int m, int n, int i)
{
  int ind, ind2;
  --i;

  for(ind = 0; ind < i; ++ind)
  {
    for (ind2 = 0; ind2 < m; ++ind2)
	{
	  *(new_mat + ind * m + ind2) = *(mat + ind * m + ind2);
	}
  }

  for(ind = i; ind < n - 1; ++ind)
  {
    for (ind2 = 0; ind2 < m; ++ind2)
	{
	  *(new_mat + ind * m + ind2) = *(mat + (ind + 1) * m + ind2);
	}
  }
}



/* prodMatrices
 * Product of matrices: result = mat1*mat2
 * result: pointer to the matrix of the product
 * mat1: pointer to the first matrix
 * m1: number of lines of mat1
 * n1: number of columns of mat1
 * mat2: pointer to the second matrix
 * m2: number of lines of mat2
 * n2: number of columns of mat2
*/
void prodMatrices(double *result, double *mat1, int m1, int n1, double *mat2, int m2, int n2)
{
  int ind, ind2, ind3;
  if (n1 != m2)
  {
    printf("Multiplication (%d*%d)*(%d*%d) impossible\n",m1, n1, m2, n2);
    return;
  }

  for (ind=0; ind<m1; ind++)
  {
    for (ind2=0; ind2<n2; ind2++)
    {
      double product = 0;
      for (ind3 = 0; ind3<n1; ind3++)
      {
		  product += *(mat1 + ind3 * m1 + ind) * *(mat2 + ind2 * m2 + ind3);
      }
      *(result + ind2 * m1 + ind) = product;
    }
  }
}



/* subMatrices
 * Substraction of matrices: result = mat1-mat2
 * result: pointer to the result matrix
 * mat1: pointer to the first matrix
 * m1: number of lines of mat1
 * n1: number of columns of mat1
 * mat2: pointer to the second matrix
 * m2: number of lines of mat2
 * n2: number of columns of mat2
*/
void subMatrices(double *result, double *mat1, int m1, int n1, double *mat2, int m2, int n2)
{
  int ind;
  if ((m1 != m2) || (n1 != n2))
  {
    printf("Substraction (%d*%d)*(%d*%d) impossible\n",m1, n1, m2, n2);
    return;
  }

  for (ind=0; ind<m1*n2; ind++)
  {
      *(result + ind) = *(mat1 + ind) - *(mat2 + ind);
  }
}



/* addMatrices
 * Addition of matrices: result = mat1+mat2
 * result: pointer to the result matrix
 * mat1: pointer to the first matrix
 * m1: number of lines of mat1
 * n1: number of columns of mat1
 * mat2: pointer to the second matrix
 * m2: number of lines of mat2
 * n2: number of columns of mat2
*/
void addMatrices(double *result, double *mat1, int m1, int n1, double *mat2, int m2, int n2)
{
  int ind;
  if ((m1 != m2) || (n1 != n2))
  {
    printf("Addition (%d*%d)*(%d*%d) impossible\n",m1, n1, m2, n2);
    return;
  }

  for (ind=0; ind<m1*n2; ind++)
  {
      *(result + ind) = *(mat1 + ind) + *(mat2 + ind);
  }
}



/* transpose
 * Transpose: result = mat'
 * result: pointer to the result matrix
 * mat: pointer to the original matrix
 * m: number of lines of mat
 * n: number of columns of mat
*/
void transpose(double *result, double *mat, int m, int n)
{
  int ind, ind2;
  for (ind=0; ind<m; ind++)
  {
    for (ind2=0; ind2<n; ind2++)
    {
      *(result + ind * n + ind2) = *(mat + ind2 * m + ind);
    }
  }
}



/* extractCol
 * Extraction of column(s)
 * mat1: pointer to the first matrix
 * i1: index of the 1st column to extract
 * j1: index of the last column to extract (included)
 * mat2: pointer to the second matrix
 * i2: index of the 1st column to extract
 * j2: index of the last column to extract (included)
 * m: number of lines (same for both matrices)
 * n1: number of columns of mat1
 * n2: number of columns of mat2
*/
void extractCol(double *mat1, int i1, int j1, double *mat2, int i2, int j2, int m, int n1, int n2)
{
  int ind, ind2;
  --i1;
  --i2;
  --j1;
  --j2;
  if ((j1 - i1) != (j2 - i2))
  {
    printf("Range of columns differ\n");
    return;
  }

  for (ind=0; ind<(j1 - i1 +1); ind++)
  {
    for (ind2=0; ind2<m; ind2++)
	{
	  *(mat2 + m * (i2 + ind) + ind2) = *(mat1 + m * (i1 + ind) + ind2);
	}
  }
}



/* injectEl
 * Inject elements from one matrix to another
 * mat1: pointer to the first matrix
 * i1: index of the 1st column to extract
 * j1: index of the last column to extract (included)
 * k1: index of the 1st line to extract
 * l1: index of the last line to extract (included)
 * mat2: pointer to the second matrix
 * i2: index of the 1st column to extract
 * j2: index of the last column to extract (included)
 * k2: index of the 1st line to extract
 * l2: index of the last line to extract (included)
 * m1: number of lines of mat1
 * n1: number of columns of mat1
 * m2: number of lines of mat2
 * n2: number of columns of mat2
*/
void injectEl(double *mat1, int i1, int j1, int k1, int l1, double *mat2, int i2, int j2, int k2, int l2, int m1, int n1, int m2, int n2)
{
  int ind, ind2;
  --i1;
  --i2;
  --j1;
  --j2;
  --k1;
  --k2;
  --l1;
  --l2;
  if (((j1 - i1) != (j2 - i2)) || ((l1 - k1) != (l2 - k2)))
  {
    printf("Range of lines/columns differ\n");
    return;
  }

  for (ind=0; ind<(l1 - k1 + 1); ind++)
  {
    for (ind2=0; ind2<(j1 - i1 + 1); ind2++)
    {
      *(mat2 + m2 * (i2 + ind2) + (k2 + ind)) = *(mat1 + m1 * (i1 + ind2) + (k1 + ind));
    }
  }
}



/* normVec
 * Compute norm of a vector
 * mat: pointer to the initial matrix
 * n: number of elements of mat
*/
void normVec(double *mat, int n, double *norm)
{
  int ind;
  double norm2=0;

  for (ind=0; ind<n; ++ind)
  {
    norm2 += *(mat + ind) * *(mat + ind);
  }
  *norm=sqrt(norm2);
}



/* assignPr
 * Assign the content of a pointer to another without changing its address
 * pr1, pr2: pointers
 * m: length of pointer
*/
void assignPr(double *pr1, double  *pr2, int m)
{
  int ind;

  for (ind=0; ind<m; ++ind)
  {
    *(pr1 + ind) = *(pr2 + ind);
  }
}



/* swapCols
 * Swap columns of a matrix
 * mat: pointer to the matrix
 * i, j: index of columns to swap
 * m: number of lines of the matrix
*/
void swapCols(double *mat, int i, int j, int m)
{
  double *matTemp = malloc(m * sizeof(double));

  int ind;

  for (ind = 0; ind < m; ind++)
  {
    *(matTemp + ind) = *(mat + (i - 1)*m + ind);

	*(mat + (i - 1)*m + ind) = *(mat + (j - 1)*m + ind);
	*(mat + (j - 1)*m + ind) = *(matTemp + ind);
  }
  free(matTemp);
}



/* function [D,psin,beta]=biodictdel(D,psin,beta,j) */
void biodictdel(double *new_D, double *new_psin, double *new_beta, double *D, double *psin, double *beta, int j, int L, int N_D, int N)
{
  char *name="BIODICTDEL";
  int ind; 
  double *betaj = malloc(L * sizeof(double));
  double *normbetaj = malloc(sizeof(double));
  double *b = malloc(L * sizeof(double));
  double *beta_temp = malloc((L * (N - 1)) * sizeof(double));
  double *trb = malloc(L * sizeof(double));
  double *dummyprod = malloc(L * L * sizeof(double));
  double *dummyprod2 = malloc(L * (N - 1) * sizeof(double));
  double *dummySub = malloc(L * (N - 1) * sizeof(double));
  double *D_temp = malloc(L * N * sizeof(double));
  double *psin_temp = malloc(L * N_D * sizeof(double));
  double *dummypsin = malloc(L * N * sizeof(double));
  double *trdummypsin = malloc(L * N * sizeof(double));
  double *R = malloc(N * N * sizeof(double));
  double *dummy = malloc(N * sizeof(double));
  double *R_temp = malloc(N * N * sizeof(double));
  double *ud = malloc(L * sizeof(double));
  double *dummyD = malloc(L * sizeof(double));
  double *trdummyD = malloc(L * sizeof(double));
  double *dummyprodIech = malloc (L * L * sizeof(double));
  double *dummyprod3 = malloc (L * sizeof(double));
  int indTest;

  memcpy(psin_temp, psin, L * N_D * sizeof(double));

  if (j < 0 || j > N - 1)
  {
    printf("%s: Required index %d is out of range (%d atoms)\n", name, j + 1, N);
    return;
  }

  ++j;

  /* b=beta(:,j)/norm(beta(:,j)) */
  extractCol(beta, j, j, betaj, 1, 1, L, N, 1);
  normVec(betaj, L, normbetaj);
  for (ind = 0; ind < L; ++ind)
  {
    *(b + ind) = *(betaj + ind) / *normbetaj;
  }

  /* beta(:,j)=[] */
  removeCol(beta_temp, beta, L, N, j);

  /* beta=beta-b*(b'*beta) */
  transpose(trb, b, L, 1);
  prodMatrices(dummyprod, b, L, 1, trb, 1, L);
  prodMatrices(dummyprod2, dummyprod, L, L, beta_temp, L, N - 1);
  subMatrices(beta_temp, beta_temp, L, N - 1, dummyprod2, L, N - 1);

  free(trb);
  free(dummyprod);
  free(dummyprod2);
  free(dummySub);

  /* R=psin(:,1:kb)'*D(:,1:kb); */
  extractCol(D, 1, N, D_temp, 1, N, L, N, N);
  extractCol(psin, 1, N, dummypsin, 1, N, L, N, N);
  transpose(trdummypsin, dummypsin, L, N);
  prodMatrices(R, trdummypsin, N, L, D_temp, L, N);
  free(D_temp);
  free(dummypsin);
  free(trdummypsin);

  /* dummy=R(:,j); */
  extractCol(R, j, j, dummy, 1, 1, N, N, 1);

  /* R(:,j)=[]; */
  removeCol(R_temp, R, N, N, j);

  /* R(:,kb)=dummy; */
  extractCol(dummy, 1, 1, R_temp, N, N, N, 1, N);
  
  for (ind = j; ind < N; ++ind)
  {
    double *R_temp2 = malloc(2 * sizeof(double));
    mxArray *array_ptr;
    mxArray *input_array[1], *output_array[2];
    int num_in=1; int num_out=2;
    double *G = malloc(4 * sizeof(double));
    double *R_temp3 = malloc(2 * sizeof(double));
    double *R_temp4 = malloc(2 * (N - ind) * sizeof(double));
    double *dummyprod = malloc(2 * (N - ind) * sizeof(double));
    double *psinp = malloc(L * 2 * sizeof(double));
    double *trG = malloc(4 * sizeof(double));
    double *dummyprod2 = malloc(L * 2 * sizeof(double));
	int p[2];
    p[0] = ind; p[1] = ind + 1;

//printf("p[0]=%d\t p[1]=%d\n", p[0], p[1]);
	
	injectEl(R_temp, ind, ind, p[0], p[1], R_temp2, 1, 1, 1, 2, N, N, 2, 1);

//printf("R_temp[%d][%d]=%f\t R_temp[%d][%d]=%f\n", p[0]-1, ind-1, *(R_temp+(ind-1)*N+p[0]-1), p[1]-1, ind-1, *(R_temp+(ind-1)*N+p[1]-1));
//printf("R_temp2[0][0]=%f\t R_temp2[1][0]=%f\n", *R_temp2, *(R_temp2+1));

	/* [G,R(p,i)]=planerot(R(p,i)); %find appropriate Givens rotation */
    array_ptr = mxCreateDoubleMatrix(2, 1, mxREAL);
	memcpy(mxGetPr(array_ptr), R_temp2, 2*sizeof(double));

    input_array[0]=array_ptr;
    mexCallMATLAB(num_out, output_array, num_in, input_array, "planerot");

    G=mxGetPr(output_array[0]);
//printf("G[0][0]=%f\t G[1][0]=%f\t G[0][1]=%f\t G[1][1]=%f\n", *G, *(G+1), *(G+2), *(G+3));
	R_temp3=mxGetPr(output_array[1]);
//printf("R_temp3[0][0]=%f\t R_temp3[1][0]=%f\n", *R_temp3, *(R_temp3+1));
	free(array_ptr);
	free(input_array);
	free(output_array);

    injectEl(R_temp3, 1, 1, 1, 2, R_temp, ind, ind, p[0], p[1], 2, 1, N, N);
//printf("R_temp[%d][%d]=%f\t R_temp[%d][%d]=%f\n", p[0]-1, ind-1, *(R_temp+(ind-1)*N+p[0]-1), p[1]-1, ind-1, *(R_temp+(ind-1)*N+p[1]-1));

	/* R(p,i+1:kb)=G*R(p,i+1:kb);   %apply Givens rotation to R(p,i+1:k) */
    injectEl(R_temp, ind + 1, N, p[0], p[1], R_temp4, 1, N - ind, 1, 2, N, N, 2, N - ind);
//for (indTest=0; indTest<N-ind; ++indTest)
//{
//printf("R_temp4[%d][%d]=%f\t R_temp4[%d][%d]=%f\n", 0, indTest, *(R_temp4+indTest*2), 1, indTest, *(R_temp4+indTest*2+1));
//}
    prodMatrices(dummyprod, G, 2, 2, R_temp4, 2, N - ind);
//for (indTest=0; indTest<N-ind; ++indTest)
//{
//printf("dummyprod[%d][%d]=%f\t dummyprod[%d][%d]=%f\n", 0, indTest, *(dummyprod+indTest*2), 1, indTest, *(dummyprod+indTest*2+1));
//}
	injectEl(dummyprod, 1, N - ind, 1, 2, R_temp, ind + 1, N, p[0], p[1], 2, N - ind, N, N);
//for (indTest=0; indTest<N-ind; ++indTest)
//{
//printf("R_temp[%d][%d]=%f\t R_temp[%d][%d]=%f\n", p[0]-1, ind+indTest, *(R_temp+(ind+indTest)*N+p[0]-1), p[1]-1, ind+indTest, *(R_temp+(ind+indTest)*N+p[1]-1));
//}
	free(R_temp3);
	free(R_temp4);
	free(dummyprod);

	/* psin(:,p)=psin(:,p)*G';      %apply Givens rotation to psin(:,p) */
    injectEl(psin, p[0], p[1], 1, L, psinp, 1, 2, 1, L, L, N_D, L, 2);
    transpose(trG, G, 2, 2);
    prodMatrices(dummyprod2, psinp, L, 2, trG, 2, 2);
//for (indTest=0; indTest<L; ++indTest)
//{
//printf("dummyprod2[%d][%d]=%f\n", 0, indTest, *(dummyprod2+indTest));
//}
    injectEl(dummyprod2, 1, 2, 1, L, psin_temp, p[0], p[1], 1, L, L, 2, L, N_D);
//for (indTest=0; indTest<10; ++indTest)
//{
//printf("psin_temp[%d][%d]=%f\t psin_temp[%d][%d]=%f\n", indTest, p[0]-1, *(psin_temp+indTest+L*(p[0]-1)), indTest, p[1]-1, *(psin_temp+indTest+L*(p[1]-1)));
//}
//for (indTest=L-10; indTest<L; ++indTest)
//{
//printf("psin_temp[%d][%d]=%f\t psin_temp[%d][%d]=%f\n", indTest, p[0]-1, *(psin_temp+indTest+L*(p[0]-1)), indTest, p[1]-1, *(psin_temp+indTest+L*(p[1]-1)));
//}
	free(psinp);
	free(trG);
	free(dummyprod2);
  }

  /* D = D(:,[1:j-1 j+1:end j]); */
  injectEl(D, 1, j - 1, 1, L, new_D, 1, j - 1, 1, L, L, N_D, L, N_D);
  injectEl(D, j + 1, N_D, 1, L, new_D, j, N_D - 1, 1, L, L, N_D, L, N_D);
  injectEl(D, j, j, 1, L, new_D, N_D, N_D, 1, L, L, N_D, L, N_D);

  /* ud=psin(:,kb); */
  extractCol(psin_temp, N, N, ud, 1, 1, L, N_D, 1);

  /* psin(:,1:end-1) = psin(:,[1:kb-1 kb+1:end]); */
  injectEl(psin_temp, 1, N - 1, 1, L, new_psin, 1, N - 1, 1, L, L, N_D, L, N_D);
  injectEl(psin_temp, N + 1, N_D, 1, L, new_psin, N, N_D - 1, 1, L, L, N_D, L, N_D);

  /* psin(:,end) = ud*(D(:,k)'*ud); */
  extractCol(new_D, N_D, N_D, dummyD, 1, 1, L, N_D, 1);
  transpose(trdummyD, dummyD, L, 1);
  prodMatrices(dummyprodIech, ud, L, 1, trdummyD, 1, L);
//for (indTest=0; indTest<L; ++indTest)
//{
//printf("ud[%d]=%f\t dummyprodIech[%d]=%f\n", indTest, *(ud+indTest), indTest, *(dummyprodIech+indTest));
//}
  prodMatrices(dummyprod3, dummyprodIech, L, L, ud, L, 1);
  injectEl(dummyprod3, 1, 1, 1, L, new_psin, N_D, N_D, 1, L, L, 1, L, N_D);
  free(dummyD);
  free(trdummyD);
  free(dummyprodIech);
  free(dummyprod3);

  for (ind = N; ind < N_D; ++ind)
  {
    double *psini = malloc(L * sizeof(double));
	double *trud = malloc(L * sizeof(double));
    double *Di = malloc(L * sizeof(double));
	double *dummyprod = malloc(sizeof(double));
    double *dummyprod2 = malloc(L * sizeof(double));
    double *psini2 = malloc(L * sizeof(double));

    extractCol(psin, ind, ind, psini, 1, 1, L, N_D, 1);
    transpose(trud, ud, L, 1);
    extractCol(D, ind, ind, Di, 1, 1, L, N_D, 1);
    prodMatrices(dummyprod, trud, 1, L, Di, L, 1);
    prodMatrices(dummyprod2, ud, L, 1, dummyprod, 1, 1);
    addMatrices(psini2, psini, L, 1, dummyprod2, L, 1);
    injectEl(psini2, 1, 1, 1, L, new_psin, ind, ind, 1, L, L, 1, L, N_D);

	free(psini);
	free(trud);
	free(Di);
	free(dummyprod);
	free(dummyprod2);
	free(psini2);
  }

  memcpy(new_beta, beta_temp, L * (N - 1) * sizeof(double));
}



/* function [D,Di,Q,beta,c]=boompf(f,D,Di,Q,beta,tol,No); */
void boompf(double *new_D, double *new_Di, double *new_Q, double *new_beta, double *new_c, double *f, double *D, double *Di, double *Q, double *beta, float tol, int No, int L, int N, int N_D)
{
  char *name="BOOMPF";
  double *C = malloc(N * sizeof(double));
  double *c = malloc(N * sizeof(double));
  double *trC = malloc(N * sizeof(double));
  double *dummyD = malloc((L * N) * sizeof(double));
  double *dummyprod = malloc(L * sizeof(double));
  double *trf = malloc(L * sizeof(double));
  double *dummySub = malloc(L * sizeof(double));
  double *nor1 = malloc(sizeof(double));
  double *nor2 = malloc(sizeof(double));
  int s;
  int ind;

  if (No>N)
  {
    printf("%s: You want to reduce %d atoms into %d atoms!\n",name, N, No);
    return;
  }

  /* C=f*beta; */
  prodMatrices(C, f, 1, L, beta, L, N);

  /* c=C; */
  assignPr(c, C, N);

  /* SQRT NOT COMPUTED SINCE delta=1*/
  /* nor1=norm(f'-D(:,1:N)*C')*sqrt(delta);
     nor2=nor1; */
  transpose(trC, C, 1, N);
  extractCol(D, 1, N, dummyD, 1, N, L, N, N);
  prodMatrices(dummyprod, dummyD, L, N, trC, N, 1);
  transpose(trf, f, 1, L);
  subMatrices(dummySub, trf, L, 1, dummyprod, L, 1);
  normVec(dummySub, L, nor1);
  nor2=nor1;
  free(trC);
  free(dummyD);
  free(dummyprod);
  free(trf);
  free(dummySub);

  if ((*nor1>tol) && (No==0))
  {
    printf("\n%s: Approximation is too coarse, I cannot satisfy the tolerance condition\n",name);
  }
  else
  {
    int ind, s = N;
    double *trnew_c, *dummyD, *dummyprod, *trf, *dummySub;
	
	for (ind=0; ind<N - No; ++ind)
    {
      /* s=size(beta,2); */
	  double *dd = malloc(s * sizeof(double));
      int ind2;
      double Cp=1000000, conjC, dummyprod;
      int p;
      double *trbeta = malloc(L * s * sizeof(double));
      double *dummybeta = malloc(L * sizeof(double));
      double *temp = malloc(s * sizeof(double));
      double *trtemp = malloc(s * sizeof(double));
      double *trC = malloc(s * sizeof(double));
      double *dummyD = malloc((L * s) * sizeof(double));
      double *dummyprod2 = malloc(L * sizeof(double));
      double *trf = malloc(L * sizeof(double));
      double *dummySub = malloc(L * sizeof(double));
      double *norSub = malloc(sizeof(double));
      double *dummy = malloc(sizeof(double));
      double *Di_temp = malloc((N_D-1)*sizeof(double));

	  for (ind2=0; ind2<s; ++ind2)
      {
        /* dd(i)=beta(:,i)'*beta(:,i); */
        double *dummybeta = malloc(L * sizeof(double));
        double *trdummybeta = malloc(L * sizeof(double));
        double *dummyprod = malloc(sizeof(double));
		extractCol(beta, ind2 + 1, ind2 + 1, dummybeta, 1, 1, L, s, 1);
        transpose(trdummybeta, dummybeta, L, 1);
        prodMatrices(dummyprod, trdummybeta, 1, L, dummybeta, L, 1);
        *(dd + ind2)=*dummyprod;
		free(dummybeta);
		free(trdummybeta);
		free(dummyprod);
      }

      /* [Cp,p]=min((C.*conj(C))./dd); % p-th atom to be removed */
      for (ind=0; ind<N; ++ind)
      {
        conjC = *(C + ind);
        dummyprod = *(C + ind) * conjC / *(dd + ind);

        if (dummyprod < Cp)
        {
          Cp = dummyprod;
          p = ind;
        }
      }

	  transpose(trbeta, beta, L, s);
      extractCol(beta, p + 1, p + 1, dummybeta, 1, 1, L, s, 1);
      prodMatrices(temp, trbeta, s, L, dummybeta, L, 1);
      for (ind=0; ind<s; ++ind)
      {
        *(temp + ind) = *(temp + ind) * *(C + p) / *(dd + p);
      }
	  free(trbeta);
	  free(dummybeta);

      /* C=C-temp'; */
	  transpose(trtemp, temp, s, 1);
      subMatrices(C, C, 1, s, trtemp, 1, s);
	  free(temp);
	  free(trtemp);

      /* stopping criterion
         if (tol~=0) & (norm(f'-D(:,1:s)*C')*sqrt(delta)>tol); break; end */
      /* TO BE FINISHED PROPERLY 
         RETURN */
      transpose(trC, C, 1, s);
      extractCol(D, 1, s, dummyD, 1, s, L, N, s);
      prodMatrices(dummyprod2, dummyD, L, s, trC, s, 1);
      transpose(trf, f, 1, L);
      subMatrices(dummySub, trf, L, 1, dummyprod2, L, 1);
      normVec(dummySub, L, norSub);
	  free(trC);
	  free(dummyD);
	  free(dummyprod2);
	  free(trf);
	  free(dummySub);

	  if ((tol != 0) && *norSub>tol)
      {
	    assignPr(new_D, D, L * N_D);
		assignPr(new_Q, Q, L * N_D);
		assignPr(new_beta, beta, L * N);
		assignPr(new_Di, Di, N_D);
		assignPr(new_c, c, N);
        goto stopCond;
      }

      /* [D,Q,beta]=biodictdel(D,Q,beta,p); */
      /* TO BE FINISHED PROPERLY 
         ALLOCATION OF POINTERS */
      biodictdel(new_D, new_Q, new_beta, D, Q, beta, p, L, N_D, N);

      /* dummy=Di(p);
         Di(p)=[];
         Di(N)=dummy;
         C(p)=[];
         c=C; */

      extractCol(Di, p + 1, p + 1, dummy, 1, 1, 1, N_D, 1);
      removeCol(Di_temp, Di, 1, N_D, p + 1);
      injectEl(dummy, 1, 1, 1, 1, Di_temp, N, N, 1, 1, 1, 1, 1, N_D);
	  free(dummy);
	  memcpy(new_Di, Di_temp, (N_D - 1) * sizeof(double));

      removeCol(C, C, 1, s, p+1);
      assignPr(new_c, C, N);

	  s = L - (ind + 1);
    }

    stopCond:
	/* nor2=norm(f'-D(:,1:s)*c')*sqrt(delta); */
    trnew_c = malloc(s * sizeof(double));
    transpose(trnew_c, new_c, 1, s);
    dummyD = malloc((L * s) * sizeof(double));
    extractCol(new_D, 1, s, dummyD, 1, s, L, N, s);
    dummyprod = malloc(L * sizeof(double));
    prodMatrices(dummyprod, dummyD, L, s, trnew_c, s, 1);
    trf = malloc(L * sizeof(double));
    transpose(trf, f, 1, L);
    dummySub = malloc(L * sizeof(double));
    subMatrices(dummySub, trf, L, 1, dummyprod, L, 1);
	normVec(dummySub, L, nor2);
  }

  printf("%s reduces the space of %d functions into a space of %d functions\n", name, N, s);
  printf("Previous norm ||f-f_approx|| was %f, new norm is %f\n", *nor1, *nor2);
  if (No != 0)
  {
    printf("You have required %d atoms in decomposition\n", No);
  }
  else
  {
    printf("Required precision was tol = %f\n", tol);
  }
  /* printf("%s ran for %f seconds\n", name, toc); */

  return;
}



/* **************************************************************************************************** */
/* Gateway routine */
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
  double *f, *D, *Di, *Q, *beta;
  double tol;
  int No, L, N, N_D, delta=1;
  double *new_D, *new_Di, *new_Q, *new_beta, *c;



  if (nrhs > 7)
    mexErrMsgTxt("Too many input arguments.");
  else if (nlhs > 5)
    mexErrMsgTxt("Too many output arguments.");

  f=mxGetPr(prhs[0]);
  D=mxGetPr(prhs[1]);
  Di=mxGetPr(prhs[2]);
  Q=mxGetPr(prhs[3]);
  beta=mxGetPr(prhs[4]);

  /* [L,N]=size(beta); */
  N_D=mxGetN(prhs[1]);
  L=mxGetM(prhs[4]);
  N=mxGetN(prhs[4]);

  /* if nargin==5; tol=1.0e-2;end;
     if nargin==6; No=0;end; */
  if (nrhs > 6)
  {
    tol=mxGetScalar(prhs[5]);
    No=(int)mxGetScalar(prhs[6]);
  }
  else if (nrhs > 5)
  {
    tol=mxGetScalar(prhs[5]);
    No=0;
  }
  else
  {
    tol=1.0e-2;
    No=0;
  }

  plhs[0]=mxCreateDoubleMatrix(L, N_D, mxREAL);
  plhs[1]=mxCreateDoubleMatrix(1, N_D, mxREAL);
  plhs[2]=mxCreateDoubleMatrix(L, N_D, mxREAL);
  plhs[3]=mxCreateDoubleMatrix(L, N, mxREAL);
  plhs[4]=mxCreateDoubleMatrix(1, N, mxREAL);
  new_D=mxGetPr(plhs[0]);
  new_Di=mxGetPr(plhs[1]);
  new_Q=mxGetPr(plhs[2]);
  new_beta=mxGetPr(plhs[3]);
  c=mxGetPr(plhs[4]);

  boompf(new_D, new_Di, new_Q, new_beta, c, f, D, Di, Q, beta, tol, No, L, N, N_D);
}