#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 [Q,beta]=biodictins(D,Q,beta,k); */
void biodictins(double *new_Q, double *new_beta, double *D, double *Q, double *beta, int k, int L, int N, int N_D)
{
/* BIODICTINS enlarges a basis selected from a given dictionary  
  by  one vector
  
 It appropriately modifies the biorthogonal functions and unselected dictionary atoms 
 subtracted by their components in the selected basis. It also updates the set of 
 orthonormal functions spanning the same space as chosen atoms.
  
 Usage: [Q,beta]=biodictins(D,Q,beta,k);
  
 Inputs:
  D    dictionary, rearranged in such way that 
       D(:,1:k-1) are the selected atoms 
       D(:,k) is the atom to add into the basis  
  Q    Q(:,1:k-1)= orthonormal basis to D(:,1:k-1)
       Q(:,k:end)=D(:,k:end) without component from D(:,1:k-1)   
  beta biorthogonal functions to D(:,1:k-1)
  k    k-th atom of D to incorporate
       it says that first k-1 atoms are already in basis
  
 Outputs: 
  Q    Q(:,1:k)= orthonormal basis to D(:,1:k)
       Q(:,k+1:end)=D(:,k+1:end) without component from D(:,1:k)
  beta updated biorthogonal functions to D(:,1:k)
  
 References:
    L. Rebollo-Neira, "Recursive bi-orthogonalisation approach and orthogonal projectors",
      math-ph/0209026 (2002).  
  
 See also NBIODICTINS, BIOINSERT, NBIOINSERT.

 See http://www.ncrg.aston.ac.uk/Projects/BiOrthog/ for more details */
  
  int p;
  double *Qk = malloc(L * sizeof(double));
  double *nork;
  double *trQk = malloc(L * sizeof(double));
  int ind;

  extractCol(Q, k, k, Qk, 1, 1, L, N_D, 1);
  transpose(trQk, Qk, L, 1);

  /* re-orthogonalization of Q(:,k) w.r.t Q(:,1),..., Q(:,k-1) */
  for (p = 0; p < k; ++p)
  {
    double *Qp = malloc(L * sizeof(double));
	double *trQp = malloc(L * sizeof(double));
	double *dummyprod;
	double *dummyprod2 = malloc(L * sizeof(double));
	double *dummysub = malloc(L * sizeof(double));
	int ind;

    /* Q(:,k)=Q(:,k)-(Q(:,p)'*Q(:,k))*Q(:,p); */
	extractCol(Q, p, p, Qp, 1, 1, L, N_D, 1);
    transpose(trQp, Qp, L, 1);
    prodMatrices(dummyprod, trQp, 1, L, Qk, L, 1);
	prodMatrices(dummyprod2, Qp, L, 1, dummyprod, 1, 1);
    subMatrices(dummysub, Qk, L, 1, dummyprod2, L, 1);

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

	free(Qp);
	free(trQp);
	free(dummyprod2);
	free(dummysub);
  }

  /* nork=norm(Q(:,k)); */
  normVec(Qk, L, nork);
  for (ind=0; ind < L; ++ind)
  {
    /* Q(:,k)=Q(:,k)/nork; %normalization of Q(:,k) */
    *(Q + k * L + ind) = *(Q + k * L + ind) / *nork;
  }

  /* updating of biorthogonal functions */
  if (k > 1)
  {
    double *Dk = malloc(L * sizeof(double));
	double *trDk = 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));
	int ind;

    /* beta=beta-Q(:,k)*(D(:,k)'*beta)/nork; */
	extractCol(D, k, k, Dk, 1, 1, L, N_D, 1);
    transpose(trDk, Dk, L, 1);
    prodMatrices(dummyprod, Qk, L, 1, Dk, 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) / *nork;
	}

	free(Dk);
	free(trDk);
	free(dummyprod2);
	free(dummysub);
  }

  /* beta(:,k)=Q(:,k)/nork;   % k-th biorthogonal function */
  for (ind = 0; ind < L; ++ind)
  {
    *(beta + k * L + ind) = *(Q + k * L + ind) / *nork;
  }
  
  /* orthogonalization of Q(:,k+1:n) wrt Q(:,k) */
  for (p = 0; p < N - (k + 1); ++p)
  {
    /* orthogonalization of Q(:,p) wrt Q(:,k) */
    double *Qp = malloc(L * sizeof(double));
	double *dummyprod;
	double *dummyprod2 = malloc(L * sizeof(double));
	double *dummysub = malloc(L * sizeof(double));
	int ind;

    /* Q(:,p)=Q(:,p)-(Q(:,k)'*Q(:,p))*Q(:,k); */
    /* Q(:,k)=Q(:,k)-(Q(:,p)'*Q(:,k))*Q(:,p); */
	extractCol(Q, p, p, Qp, 1, 1, L, N_D, 1);
    prodMatrices(dummyprod, trQk, 1, L, Qp, L, 1);
	prodMatrices(dummyprod2, Qk, L, 1, dummyprod, 1, 1);
    subMatrices(dummysub, Qp, L, 1, dummyprod2, L, 1);
    
    for (ind=0; ind < L; ++ind)
    {
      *(Q + p * L + ind) = *(dummysub + ind);
    }

	free(Qp);
	free(dummyprod2);
	free(dummysub);
  }

  assignPr(new_Q, Q, L * N_D);
  assignPr(new_beta, beta, L * N);

/* 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, *Q, *beta;
  int k, L, N, N_D;
  double *new_Q, *new_beta;



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

  D=mxGetPr(prhs[0]);
  Q=mxGetPr(prhs[1]);
  beta=mxGetPr(prhs[2]);
  k=mxGetScalar(prhs[3]);

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

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

  /* [Q,beta]=biodictins(D,Q,beta,k); */
  biodictins(new_Q, new_beta, D, Q, beta, k, L, N, N_D);
}