#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));

  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 f=errortest(D,beta); */
void errortest(double *f, double *D, int N_D, double *beta, int N, int L)
{
/* ERRORTEST tests orthogonality of a sequence or biorthogonality of two sequences.
 
 Usage: errortest(D,beta);
        errortest(Q);

 Inputs:
   D    sequence of vectors 
   beta (optional) biorthogonal sequence  

 Output:
   f    orthogonality of D or biorthogonality D w.r.t beta

 See http://www.ncrg.aston.ac.uk/Projects/BiOrthog/ for more details */

  char *name="ERRORTEST";
  double *trbeta = malloc(L * N * sizeof(double));
  double *PP = malloc(N * N_D * sizeof(double));
  double *eye = malloc(N * N_D * sizeof(double));
  int ind;
  double *dummySub = malloc(N * N_D * sizeof(double));
  mxArray *array_ptr;
  mxArray *output_array[1], *input_array[1];
  int num_out=1;
  int num_in=1;

  transpose(trbeta, beta, L, N);
  prodMatrices(PP, trbeta, N, L, D, L, N_D);

  for (ind = 0; ind < N * N_D; ++ind)
  {
    *(eye + ind) = 0;
  }

  for (ind = 0; ind < N; ++ind)
  {
    *(eye + ind * (L + 1)) = 1;
  }

  subMatrices(dummySub, eye, N, N_D, PP, N, N_D);

  mxSetPr(array_ptr, dummySub);

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

  f=mxGetPr(output_array[0]);

  printf("Orthogonality or Biorthogonality: %g\n", f);

/* Copyright (C) 2006 Miroslav ANDRLE and Laura REBOLLO-NEIRA

This program is free software; you can redistribute it and/or modify it under the terms 
of the GNU General Public License as published by the Free Software Foundation; either 
version 2 of the License, or (at your option) any later version.

This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
See the GNU General Public License for more details.

You should have received a copy of the GNU General Public License along with this program;
if not, write to the Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor,
Boston, MA  02110-1301, USA. */
}



/* function [D,psin,beta]=bioinsert(D,psin,beta,atom,tol); */
void bioinsert(double *new_D, double *new_psin, double *new_beta, double *D, double *psin, double *beta, double *atom, double tol, int L, int N, int N_D)
{
/* BIOINSERT adds an atom to a basis. It also appropriately
 modifies the corresponding biorthogonal basis and 
 orthonormal basis (obtained by modified Gram-Schmidt).
  
 Usage: [D,psin,beta]=bioinsert(D,psin,beta,atom,tol);
  
 Inputs:
   D    already selected basis 
   psin orthonormal basis spanning the same space as D
   beta biorthogonal basis to D
   atom new atom to be incorporated into basis 
   tol  tolerance (optional parameter) to decide linear dependence
        default value = 1.0000e-7
  
 Outputs:
   D    updated basis   
   psin updated orthonormal basis spanning the same space as D
   beta updated biorthogonal functions to D
  
 References:
    L. Rebollo-Neira, "Recursive bi-orthogonalisation approach and orthogonal projectors", 
      math-ph/0209026 (2002).

 See also NBIOINSERT, NBIODICTINS, BIODICTINS. 

 See http://www.ncrg.aston.ac.uk/Projects/BiOrthog/ for more details */

  char *name = "BIOINSERT";
  double *u = malloc(L * sizeof(double));
  double *psi_norm;
  int z, i;
  double *temp = malloc(L * sizeof(double));
  int ind;
  double *tratom = malloc(L * sizeof(double));
  double *dummyprod = malloc(L * L * sizeof(double));
  double *dummyprod2 = malloc(L * N * sizeof(double));
  double *dummySub = malloc(L * N * sizeof(double));

  assignPr(u, atom, L);

  if (N == 0)
  {
    int ind;
    normVec(u, L, psi_norm);

	if (*psi_norm < tol)
	{
      printf("%s: The first atom has zero norm! (norm=%g)\n", name, *psi_norm);
	  return;
	}

	assignPr(new_D, u, L);
	for (ind = 0; ind < L; ++ind)
	{
	  *(new_psin + ind) = *(u + ind) / *psi_norm;
	  *(new_beta + ind) = *(new_psin + ind) / *psi_norm;
	}

	return;
  }

  /* re-orthogonalization (twice) */
  for (z = 0; z < 2; ++z)
  {
    for (i = 0; i < N; ++i)
	{
	  double *psini = malloc(L * sizeof(double));
	  double *trpsini = malloc(L * sizeof(double));
	  double *dummyprod = malloc(L * L * sizeof(double));
	  double *dummyprod2 = malloc(L * sizeof(double));
	  double *dummysub = malloc(L * sizeof(double));
	  int ind;

      /* u=u-psin(:,i)*(psin(:,i)'*u); */
      extractCol(psin, i, i, psini, 1, 1, L, N_D, 1);
      transpose(trpsini, psini, L, 1);
      prodMatrices(dummyprod, psini, L, 1, trpsini, 1, L);
	  prodMatrices(dummyprod2, dummyprod, L, L, u, L, 1);
      subMatrices(dummysub, u, L, 1, dummyprod2, L, 1);

	  for (ind = 0; ind < L; ++ind)
	  {
	    *(u + ind) = *(dummysub + ind);
	  }

      free(psini);
	  free(trpsini);
	  free(dummyprod);
	  free(dummyprod2);
	  free(dummysub);
	}
  }

  /* psi_norm=norm(u); */
  normVec(u, L, psi_norm);
  
  /* test of linear dependancy */
  if (*psi_norm < tol)
  {
    printf("%s: New atom was found dependant.\n Its norm after subtraction its component in selected space is %g\n", name, psi_norm);
	return;
  }

  for (ind = 0; ind < L; ++ind)
  {
    /* psin(:,k+1)=u/psi_norm; */
    *(psin + N * L + ind) = *(u +ind) / *psi_norm;
    /* temp=psin(:,k+1)/psi_norm; */
	*(temp + ind) = *(psin + N * L + ind) / *psi_norm;
  }

  for (i = 0; ind < N; ++i)
  {
    double *psini = malloc(L * sizeof(double));
    double *trpsini = malloc(L * sizeof(double));
    double *dummyprod = malloc(L * L * sizeof(double));
    double *dummyprod2 = malloc(L * sizeof(double));
    double *dummysub = malloc(L * sizeof(double));

    /* temp=temp-psin(:,i)*(psin(:,i)'*temp); */
    extractCol(psin, i, i, psini, 1, 1, L, N_D, 1);
    transpose(trpsini, psini, L, 1);
    prodMatrices(dummyprod, psini, L, 1, trpsini, 1, L);
    prodMatrices(dummyprod2, dummyprod, L, L, temp, L, 1);
    subMatrices(dummysub, temp, L, 1, dummyprod2, L, 1);

    for (ind = 0; ind < L; ++ind)
    {
      *(temp + ind) = *(dummysub + ind);
    }

	free(psini);
    free(trpsini);
    free(dummyprod);
    free(dummyprod2);
    free(dummysub);
  }

  /* beta=beta-temp*(atom'*beta); */
  transpose(tratom, atom, L, 1);
  prodMatrices(dummyprod, temp, L, 1, tratom, 1, L);
  prodMatrices(dummyprod2, dummyprod, L, L, beta, L, N);
  subMatrices(dummySub, beta, L, N, dummyprod2, L, N);

  for (ind = 0; ind < L * N; ++ind)
  {
    *(beta + ind) = *(dummySub + ind);
  }

  free(tratom);
  free(dummyprod);
  free(dummyprod2);
  free(dummySub);

  assignPr(new_beta, beta, L * N);

  /* beta(:,k+1)=temp; */
  for (ind = 0; ind < L; ++ind)
  {
    *(new_beta + N * L + ind) = *(temp + ind);
  }

  /* basis updating */
  assignPr(new_D, D, L * N_D);
  for (ind = 0; ind < L; ++ind)
  {
    *(new_D + N_D * L + ind) = *(atom + ind);
  }

  assignPr(new_psin, psin, L * N_D);

/* Copyright (C) 2006 Miroslav ANDRLE and Laura REBOLLO-NEIRA

This program is free software; you can redistribute it and/or modify it under the terms 
of the GNU General Public License as published by the Free Software Foundation; either 
version 2 of the License, or (at your option) any later version.

This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
See the GNU General Public License for more details.

You should have received a copy of the GNU General Public License along with this program;
if not, write to the Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor,
Boston, MA  02110-1301, USA. */
}



/* **************************************************************************************************** */
/* Gateway routine */
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
  double *D, *psin, *beta, *atom;
  int k, L, N, N_D;
  double *new_D, *new_psin, *new_beta;
  double tol;



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

  D=mxGetPr(prhs[0]);
  psin=mxGetPr(prhs[1]);
  beta=mxGetPr(prhs[2]);
  atom=mxGetPr(prhs[3]);

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

  /* if nargin<5, tol=1.0000e-7; end */
  if (nrhs > 4)
  {
    tol=mxGetScalar(prhs[4]);
  }
  else
  {
    tol=1.0e-7;
  }

  plhs[0]=mxCreateDoubleMatrix(L, N_D, mxREAL);
  plhs[1]=mxCreateDoubleMatrix(L, N_D, mxREAL);
  plhs[2]=mxCreateDoubleMatrix(L, N, mxREAL);
  new_D=mxGetPr(plhs[0]);
  new_psin=mxGetPr(plhs[1]);
  new_beta=mxGetPr(plhs[2]);

  /* function [D,psin,beta]=bioinsert(D,psin,beta,atom,tol); */
  bioinsert(new_D, new_psin, new_beta, D, psin, beta, atom, tol, L, N, N_D);
}