int InvertMatrix ( double * A, int n){
        int INFO;
        int N;
        static int * IPIV=NULL;
        static int LWORK=0;
        static double * WORK=NULL;
        static int last_n=0;

        N = n;

        if ( n>last_n){
                if (NULL==IPIV){
                        WORK = malloc(sizeof(double));
                } else {
                        free(IPIV);
                }
                LWORK = -1;
                dgetri_ (&N,A,&N,IPIV,WORK,&LWORK,&INFO);
                LWORK=(int)WORK[0];
                free(WORK);
                WORK = malloc(LWORK*sizeof(double));
                IPIV = malloc(n*sizeof(int));
                last_n = n;
        }

        dgetrf_ (&N,&N,A,&N,IPIV,&INFO);
        if ( INFO==0){
                dgetri_ (&N,A,&N,IPIV,WORK,&LWORK,&INFO);
        }

        if (INFO!=0)
                return -1;
        return 0;
}

/* Currently only used as 'support' function for the matrix_det function. */
static void matrix_dgetrf__( matrix_type * A, int * ipiv, int * info) {
  int lda       = matrix_get_column_stride( A );
  int m         = matrix_get_rows( A );
  int n         = matrix_get_columns( A );  

  dgetrf_( &m , &n , matrix_get_data( A ) , &lda , ipiv , info);
}

		//--- Calculation of determinamt ---
		double det(const dmatrix &_a)
		{
				assert( _a.cols() == _a.rows() );

				typedef dmatrix mlapack;
				mlapack a = _a;	// <-

				int info;
				int n = (int)a.cols();
				int lda = n;
				std::vector<int> ipiv(n);

#ifdef USE_CLAPACK_INTERFACE
				info = clapack_dgetrf(CblasColMajor,
									  n, n, &(a(0,0)), lda, &(ipiv[0]));
#else
				dgetrf_(&n, &n, &a(0,0), &lda, &(ipiv[0]), &info);
#endif

				double det=1.0;
	
				for(int i=0; i < n-1; i++)
						if(ipiv[i] != i+1)  det = -det;
				
				for(int i=0; i < n; i++)  det *= a(i,i);

				assert(info == 0);
				
				return det;
		}

/* on output, A is replaced by A^{-1} */
int
lapack_inverse(gsl_matrix *A)
{
  int s = 0;
  int M = A->size1;
  int N = A->size2;
  int lda = N;
  int *ipiv;
  int lwork;
  double *work;
  double q[1];

  ipiv = malloc(N * sizeof(int));

  dgetrf_(&M, &N, A->data, &lda, ipiv, &s);
  if (s != 0)
    {
      fprintf(stderr, "lapack_inverse: error: %d\n", s);
      return s;
    }

  lwork = -1;
  dgetri_(&N, A->data, &lda, ipiv, q, &lwork, &s);

  lwork = (int) q[0];
  work = malloc(lwork * sizeof(double));

  /* compute inverse */
  dgetri_(&N, A->data, &lda, ipiv, work, &lwork, &s);

  free(ipiv);
  free(work);

  return s;
}

long benchmark(int size) {
    int m = sqrt(size);
	long requestStart, requestEnd;

    // random matrices are full rank (and can always be inverted if square)
    // http://www.sciencedirect.com/science/article/pii/S0096300306009040
    double* a = random_array(m * m);
    int bSize = m * m;
    double* b = calloc(bSize, sizeof(double));
    int* p = calloc(m, sizeof(int));
    int info = 0;

	requestStart = currentTimeNanos();

    // calling raw fortran because OS X doesn't have LAPACKE
    dgetrf_( &m, &m, a, &m, p, &info );
    dgetri_( &m, a, &m, p, b, &bSize, &info );

	requestEnd = currentTimeNanos();

    free(a);
    free(b);
    free(p);

    return (requestEnd - requestStart);
  }

void Peer::ros2(double * y, double& tstart, double tend, IContinuous *continuousSystem, ITime *timeSystem) {
    double *T=new double[_dimSys*_dimSys];
    double *D=new double[_dimSys];
    double *k1=new double[_dimSys];
    double *k2=new double[_dimSys];
    long int *P=new long int[_dimSys];
    long int info;
    long int dim=1;
    double t=tstart;
    const double gamma=1.-sqrt(2.)/2.;
    char trans='N';
    double hu=(tend-tstart)/10.;
    for(int count=0; count<10; ++count) {
        evalJ(t,y,T,continuousSystem, timeSystem,-hu*gamma);
        for(int i=0; i<_dimSys;++ i) T[i*_dimSys+i]+=1.;
        dgetrf_(&_dimSys, &_dimSys, T, &_dimSys, P, &info);
        evalF(t,y,k1,continuousSystem, timeSystem);
        evalD(t,y,D,continuousSystem, timeSystem);
        for(int i=0; i<_dimSys;++ i) k1[i]+=gamma*hu*D[i];
        dgetrs_(&trans, &_dimSys, &dim, T, &_dimSys, P, k1, &_dimSys, &info);
        for(int i=0; i<_dimSys;++ i) y[i]+=hu*k1[i];
        evalF(t,y,k2,continuousSystem, timeSystem);
        for(int i=0; i<_dimSys;++ i)  k2[i]+= hu*gamma*D[i]-2.*k1[i];
        dgetrs_(&trans, &_dimSys, &dim, T, &_dimSys, P, k2, &_dimSys, &info);
        for(int i=0; i<_dimSys;++ i) y[i]+=0.5*hu*(k1[i]+k2[i]);
    }
}

/*  returns determinant
	Matrix m wil NOT change!
*/
double utr_mat_det(const double *m, int n,char store, double * det) 
{
  int aux=n,i=0;
  int *  pivots = malloc(n*sizeof(int));
  double * M=malloc(n*n*sizeof(double));
  
  *det=1.0;
  
  if(M!=NULL && pivots != NULL) {
	if(store == 'R' || store == 'r') {
	  int j=0;
	  for(i=0;i<n;++i) {
	for(j=0;j<n;++j) {
	  M[n*j+i]=m[n*i+j];
	}
	  }
	}
	else {	// 'c' or 'C' column store schema 
	  memcpy(M,m,n*n*sizeof(double));
	}
	dgetrf_(&n,&n,M,&n,pivots,&aux);
	for(i=0; i < n; ++i) {
	  *det *= M[n*i+i] * (pivots[i]!=(i+1)? -1.0 : 1.0);
	}
	free(M);
	free(pivots);
  }
  return(*det);
}

  void LapackLuDense::prepare() {
    double time_start=0;
    if (CasadiOptions::profiling && CasadiOptions::profilingBinary) {
      time_start = getRealTime(); // Start timer
      profileWriteEntry(CasadiOptions::profilingLog, this);
    }
    prepared_ = false;

    // Get the elements of the matrix, dense format
    input(0).get(mat_);

    if (equilibriate_) {
      // Calculate the col and row scaling factors
      double colcnd, rowcnd; // ratio of the smallest to the largest col/row scaling factor
      double amax; // absolute value of the largest matrix element
      int info = -100;
      dgeequ_(&ncol_, &nrow_, getPtr(mat_), &ncol_, getPtr(r_),
              getPtr(c_), &colcnd, &rowcnd, &amax, &info);
      if (info < 0)
          throw CasadiException("LapackQrDense::prepare: "
                                "dgeequ_ failed to calculate the scaling factors");
      if (info>0) {
        stringstream ss;
        ss << "LapackLuDense::prepare: ";
        if (info<=ncol_)  ss << (info-1) << "-th row (zero-based) is exactly zero";
        else             ss << (info-1-ncol_) << "-th col (zero-based) is exactly zero";

        userOut() << "Warning: " << ss.str() << endl;



        if (allow_equilibration_failure_)  userOut() << "Warning: " << ss.str() << endl;
        else                              casadi_error(ss.str());
      }

      // Equilibrate the matrix if scaling was successful
      if (info!=0)
        dlaqge_(&ncol_, &nrow_, getPtr(mat_), &ncol_, getPtr(r_), getPtr(c_),
                &colcnd, &rowcnd, &amax, &equed_);
      else
        equed_ = 'N';
    }

    // Factorize the matrix
    int info = -100;
    dgetrf_(&ncol_, &ncol_, getPtr(mat_), &ncol_, getPtr(ipiv_), &info);
    if (info != 0) throw CasadiException("LapackLuDense::prepare: "
                                         "dgetrf_ failed to factorize the Jacobian");

    // Success if reached this point
    prepared_ = true;

    if (CasadiOptions::profiling && CasadiOptions::profilingBinary) {
      double time_stop = getRealTime(); // Stop timer
      profileWriteTime(CasadiOptions::profilingLog, this, 0, time_stop-time_start,
                       time_stop-time_start);
      profileWriteExit(CasadiOptions::profilingLog, this, time_stop-time_start);
    }
  }

/* Main function to be called */
void errbars (int numparams, double *p, struct kslice *ks, double **covar)
{
	int ii, ij, ik;
	lapack_int n, lda, info, worksize;
	char c;
	double *fish, *work, *rscale, *cscale;
	double row, col, amax;
	int *piv;

	c = 'U';
	fish = malloc(numparams*numparams*sizeof(double));
	piv = malloc(numparams*numparams*sizeof(int));
	rscale = malloc(numparams*sizeof(double));
	cscale = malloc(numparams*sizeof(double));

	/* compute fisher information matrix */
	fisher(numparams, p, ks, fish);

	n = numparams;
	lda = numparams;
	dgeequ_(&n, &n, fish, &lda, rscale, cscale, &row, &col, &amax, &info);
/*
	for (ii=0; ii<numparams; ii++)
		for (ij=0; ij<numparams; ij++)
			fish[ii*numparams+ij] *= rscale[ii]*cscale[ij];
*/

	n = numparams;
	lda = numparams;
	dgetrf_(&n, &n, fish, &lda, piv, &info);
//	printf("\tLU decomp status: %d\n", info);

	worksize = 32*n;
	work = malloc(worksize*sizeof(double));
	dgetri_(&n, fish, &lda, piv, work, &worksize, &info);
//	printf("\tInversion status: %d\n", info);
/*
	for (ii=0; ii<numparams; ii++)
		for (ij=0; ij<numparams; ij++)
			fish[ii*numparams+ij] *= rscale[ij]*cscale[ii];
*/
	/* compute inverse of fisher information matrix */
/*
	for (ii=0; ii<numparams; ii++)
	{
		for (ij=0; ij<numparams; ij++)
			printf("%d\t%d\t%e\n", ii, ij, (fish[ii*numparams+ij]));
		printf("\n");
	}
*/
	/* return */
	*covar = fish;

	/* free local memory */
	free(work);
	free(piv);
	free(rscale);
	free(cscale);
}

static long
dgetrf(long M, long N, double *A, long LDA, long *IPIV)
{
  extern void dgetrf_(const long *M,const long *N, double *A,const long *LDA,
   long *IPIV, long *infop);
  long info;
  dgetrf_(&M, &N, A, &LDA, IPIV, &info);
  return info;
}

  void QuasiNewton<double>::symmNonHerDiag(int NTrial, ostream &output){
    char JOBVL = 'N';
    char JOBVR = 'V';
    int TwoNTrial = 2*NTrial;
    int *IPIV = new int[TwoNTrial];
    int INFO;

    RealCMMap  SSuper(this->SSuperMem, TwoNTrial,TwoNTrial);
    RealCMMap  ASuper(this->ASuperMem, TwoNTrial,TwoNTrial);
    RealCMMap    SCPY(this->SCPYMem,   TwoNTrial,TwoNTrial);
    RealCMMap NHrProd(this->NHrProdMem,TwoNTrial,TwoNTrial);

    SCPY = SSuper; // Copy of original matrix to use for re-orthogonalization

    // Invert the metric (maybe not needed?)
    dgetrf_(&TwoNTrial,&TwoNTrial,this->SSuperMem,&TwoNTrial,IPIV,&INFO);
    dgetri_(&TwoNTrial,this->SSuperMem,&TwoNTrial,IPIV,this->WORK,&this->LWORK,&INFO);
    delete [] IPIV;

    NHrProd = SSuper * ASuper;
  //cout << endl << "PROD" << endl << NHrProd << endl;

    dgeev_(&JOBVL,&JOBVR,&TwoNTrial,NHrProd.data(),&TwoNTrial,this->ERMem,this->EIMem,
           this->SSuperMem,&TwoNTrial,this->SSuperMem,&TwoNTrial,this->WORK,&this->LWORK,
           &INFO);
    // Sort eigensystem using Bubble Sort
    RealVecMap ER(this->ERMem,TwoNTrial);
    RealVecMap EI(this->EIMem,TwoNTrial);
    RealCMMap  VR(this->SSuperMem,TwoNTrial,TwoNTrial);
//  cout << endl << ER << endl;
    this->eigSrt(VR,ER);
//  cout << endl << ER << endl;
  
    // Grab the "positive paired" roots (throw away other element of the pair)
    this->ERMem += NTrial;
    new (&ER    ) RealVecMap(this->ERMem,NTrial);
    new (&SSuper) RealCMMap(this->SSuperMem+2*NTrial*NTrial,2*NTrial,NTrial);

    /*
     * Re-orthogonalize the eigenvectors with respect to the metric S(R)
     * because DSYGV orthogonalzies the vectors with respect to E(R)
     * because we solve the opposite problem.
     *
     * Gramm-Schmidt
     */
    this->metBiOrth(SSuper,SCPY);

    // Separate the eigenvectors into gerade and ungerade parts
    RealCMMap XTSigmaR(this->XTSigmaRMem,NTrial,NTrial);
    RealCMMap XTSigmaL(this->XTSigmaLMem,NTrial,NTrial);
    XTSigmaR = SSuper.block(0,     0,NTrial,NTrial);
    XTSigmaL = SSuper.block(NTrial,0,NTrial,NTrial);
  //cout << endl << "ER" << endl << ER << endl << endl;
  //cout << endl << "CR" << endl << XTSigmaR << endl << endl;
  //cout << endl << "CR" << endl << XTSigmaL << endl << endl;
//  CErr();
  }

	DLLEXPORT MKL_INT d_lu_factor(MKL_INT m, double a[], MKL_INT ipiv[])
	{
		MKL_INT info = 0;
		dgetrf_(&m,&m,a,&m,ipiv,&info);
		for(MKL_INT i = 0; i < m; ++i ){
			ipiv[i] -= 1;
		}
		return info;
	}

    //! build and factorize "real" matrix
    //! \param fac1 : we add fac1*I to the Jacobian.
    //! \param Jac the jacobian.
    inline void decomr(double fac1,MatrixReal& Jac)
    {
      E1.equal_minus(Jac);
      E1.addDiag(fac1);
      int nn=n,info;
      dgetrf_(&nn,&nn,&E1,&nn,&(ipivr[0]),&info);
      if(info!=0)
	throw OdesException("odes::Matrices::decomr dgetrf,info=",info);
    }

	DLLEXPORT int d_lu_factor(int m, double a[], int ipiv[])
	{
		int info = 0;
		dgetrf_(&m,&m,a,&m,ipiv,&info);
		for(int i = 0; i < m; ++i ){
			ipiv[i] -= 1;
		}
		return info;
	}

/*******************************************************************
 Subroutine to compute the Determinant of Matrix
 by using CLAPACK subroutine - dgetrf_() ( PLU decomposition:
 where P is a permutation matrix, L is lower triangular with unit
 diagonal elements (lower trapezoidal if m > n), and U is upper
 triangular (upper trapezoidal if m < n))

   matrix *A:     the pointer to the matrix   
 
   return value: the determinant of matrix
*******************************************************************/
double det(matrix *A)
{
    integer m, n, lda, info; 
	int i, j, size;
    double *AT;
	integer *ipiv;
	double detU=1;
	int num_permut=0;

	m = A->m;
    n = A->n;

	if (m != n) {
		printf(" Warning: det() is failed since the matrix is not square matrix. \n");
		return 0;
	}

    lda = m;
    size = m*n;

    AT = new double[size];
    ipiv = new integer[n];

    // to call a Fortran routine from C we have to transform the matrix
    for (i=0; i<m; i++) {				
        for (j=0; j<n; j++) {
            AT[n*i+j] = *(A->pr+m*j+i);
        }		
    }
   
	dgetrf_(&m, &n, AT, &lda, ipiv, &info);
 
	if (info < 0) {
		printf(" Warning: det() is failed. \n");
	}
	
	// the determinant of U
	for (i=0; i<n; i++) {
		detU *= AT[n*i+i];
	}

	// the determinant of P is either +1 or -1
	// depending of whether the number of row permutations is even or odd.
	for (i=0; i<n; i++) {
		if (ipiv[i] != i+1) {
			num_permut++;
		}
	}

	if (num_permut%2 == 0) {
		return detU;
	} else {
		return -detU;
	}

}

int32_t invert_matrix(__CLPK_integer dim, double* matrix, MATRIX_INVERT_BUF1_TYPE* int_1d_buf, double* dbl_2d_buf) {
    // dgetrf_/dgetri_ is more efficient than dpotrf_/dpotri_ on OS X.
    __CLPK_integer lwork = dim * dim;
    __CLPK_integer info;
    dgetrf_(&dim, &dim, matrix, &dim, int_1d_buf, &info);
    dgetri_(&dim, matrix, &dim, int_1d_buf, dbl_2d_buf, &lwork, &info);
    if (info) {
        return 1;
    }
    return 0;
}

 LU::LU(const Matrix &mat)
   : dcmp(mat),
     pivots(std::min(mat.nrow(), mat.ncol())),
     sing_(false)
 {
   int m = mat.nrow();
   int n = mat.ncol();
   int info;
   dgetrf_(&m, &n, dcmp.data(), &m, &pivots[0], &info);
   if(info!=0) sing_ = true;
 }

template <typename fptype> static inline int
lapack_LU(fptype* a, size_t a_step, int m, fptype* b, size_t b_step, int n, int* info)
{
    int lda = a_step / sizeof(fptype), sign = 0;
    int* piv = new int[m];

    transpose_square_inplace(a, lda, m);

    if(b)
    {
        if(n == 1 && b_step == sizeof(fptype))
        {
            if(typeid(fptype) == typeid(float))
                sgesv_(&m, &n, (float*)a, &lda, piv, (float*)b, &m, info);
            else if(typeid(fptype) == typeid(double))
                dgesv_(&m, &n, (double*)a, &lda, piv, (double*)b, &m, info);
        }
        else
        {
            int ldb = b_step / sizeof(fptype);
            fptype* tmpB = new fptype[m*n];

            transpose(b, ldb, tmpB, m, m, n);

            if(typeid(fptype) == typeid(float))
                sgesv_(&m, &n, (float*)a, &lda, piv, (float*)tmpB, &m, info);
            else if(typeid(fptype) == typeid(double))
                dgesv_(&m, &n, (double*)a, &lda, piv, (double*)tmpB, &m, info);

            transpose(tmpB, m, b, ldb, n, m);
            delete[] tmpB;
        }
    }
    else
    {
        if(typeid(fptype) == typeid(float))
            sgetrf_(&m, &m, (float*)a, &lda, piv, info);
        else if(typeid(fptype) == typeid(double))
            dgetrf_(&m, &m, (double*)a, &lda, piv, info);
    }

    if(*info == 0)
    {
        for(int i = 0; i < m; i++)
            sign ^= piv[i] != i + 1;
        *info = sign ? -1 : 1;
    }
    else
        *info = 0; //in opencv LU function zero means error

    delete[] piv;
    return CV_HAL_ERROR_OK;
}

/* LU decomposition */
void THLapack_(getrf)(int m, int n, real *a, int lda, int *ipiv, int *info)
{
#ifdef  USE_LAPACK
#if defined(TH_REAL_IS_DOUBLE)
  dgetrf_(&m, &n, a, &lda, ipiv, info);
#else
  sgetrf_(&m, &n, a, &lda, ipiv, info);
#endif
#else
  THError("getrf : Lapack library not found in compile time\n");
#endif
}

/* solve linear equation -------------------------------------------------------
* solve linear equation (X=A\Y or X=A'\Y)
* args   : char   *tr       I   transpose flag ("N":normal,"T":transpose)
*          double *A        I   input matrix A (n x n)
*          double *Y        I   input matrix Y (n x m)
*          int    n,m       I   size of matrix A,Y
*          double *X        O   X=A\Y or X=A'\Y (n x m)
* return : status (0:ok,0>:error)
* notes  : matirix stored by column-major order (fortran convention)
*          X can be same as Y
*-----------------------------------------------------------------------------*/
static int solve(const char *tr, const double *A, const double *Y, integer n,
                 integer m, double *X)
{
    double B[n*n];
    integer info;
    integer ipiv[n];

    memcpy(B, A, sizeof(double)*n*n);
    memcpy(X, Y, sizeof(double)*n*m);
    dgetrf_(&n,&n,B,&n,ipiv,&info);
    if (!info) dgetrs_((char *)tr,&n,&m,B,&n,ipiv,X,&n,&info);
    return info;
}