void mkl_vector_corr_permutation(vec_p x, vec_p y, 
                               double* result, 
                               unsigned int nPerm) {
    double ret = 0.0;
    assert(x->len == y->len && x->len > 0);
    
    double xmean = 0.0, ymean = 0.0;
    for (int i = 0; i < x->len; i++){
        xmean += x->value[i];
        ymean += y->value[i];
    }
    xmean /= x->len;
    ymean /= x->len;

    /* vec_p temp; */
    /* temp = vector_new (x->len); */

    double xstd = 0.0, ystd = 0.0, xycov = 0.0;
    double tempx, tempy;
    for (int i = 0; i < x-> len; i++) {
        
        x->value[i] -= xmean;
        y->value[i] -= ymean;
    }
    xstd = sqrt(cblas_dnrm2(x->len, x->value, 1));
    ystd = sqrt(cblas_dnrm2(x->len, y->value, 1));

    for (int n = 0; n < nPerm; n++ ) {
        inplace_shuffle(y->value, y->len);
        xycov = 0.0;
        xycov = cblas_ddot(x->len, x->value, 1, y->value, 1);
        result[n] = xycov / sqrt(xstd * ystd);
    }
    return ;
}

int checkTrivialCase_vi(VariationalInequality* problem, double* x, 
                        double* w, SolverOptions* options)
{
  int n = problem->size;
  if (problem->ProjectionOnX)
  {
    problem->ProjectionOnX(problem,x,w);
  }
  else
  {
    cblas_dcopy(problem->size, x, 1, w, 1);
    project_on_set(problem->size, w, problem->set);
  }
  cblas_daxpy(n, -1.0,x, 1, w , 1);
  double nnorm = cblas_dnrm2(n,w,1);
  DEBUG_PRINTF("checkTrivialCase_vi, nnorm = %6.4e\n",nnorm);
  
  if (nnorm > fmin(options->dparam[0], 1e-12))
    return 1;

  problem->F(problem,n,x,w);
  nnorm = cblas_dnrm2(n,w,1);
  DEBUG_PRINTF("checkTrivialCase_vi, nnorm = %6.4e\n",nnorm);

  if (nnorm > fmin(options->dparam[0], 1e-12))
    return 1;

  if (verbose == 1)
    printf("variationalInequality driver, trivial solution F(x) = 0, x in X.\n");
  return 0;
}

int variationalInequality_computeError(
  VariationalInequality* problem,
  double *z , double *w, double tolerance,
  SolverOptions * options, double * error)
{

  assert(problem);
  assert(z);
  assert(w);
  assert(error);
  
  int incx = 1;
  int n = problem->size;

  *error = 0.;
  if (!options->dWork)
  {
    options->dWork = (double*)calloc(2*n,sizeof(double));
  }
  double *ztmp =  options->dWork;
  double *wtmp =  &(options->dWork[n]);

  
  if (!problem->istheNormVIset)
  {
    for (int i=0;i<n;i++)
    {
      ztmp[i]=0.0 ;
    }
    problem->F(problem,n,ztmp,w);
    problem->normVI= cblas_dnrm2(n , w , 1);
    DEBUG_PRINTF("problem->normVI = %12.8e\n", problem->normVI);
    problem->istheNormVIset=1;
  }

  double normq =problem->normVI;
  DEBUG_PRINTF("normq = %12.8e\n", normq);

  problem->F(problem,n,z,w);
  
  cblas_dcopy(n , z , 1 , ztmp, 1);
  cblas_daxpy(n, -1.0, w , 1, ztmp , 1) ;

  problem->ProjectionOnX(problem,ztmp,wtmp);

  cblas_daxpy(n, -1.0, z , 1, wtmp , 1) ;
  *error = cblas_dnrm2(n , wtmp , incx);

  /* Computes error */
  *error = *error / (normq + 1.0);
  if (*error > tolerance)
  {
    if (verbose > 1)
      printf(" Numerics - variationalInequality_compute_error: error = %g > tolerance = %g.\n",
             *error, tolerance);
    return 1;
  }
  else
    return 0;
}

double norm(int N, double *vec){

	// thread variables
	int nthds, tid;

	// compute variables
	int m, stride, start, stop;
	double nrm;

	/* Fork a team of threads giving them their own copies of variables */
	#pragma omp parallel private(nthds, tid) shared(m)
	{
		// compute thread variables
		nthds = omp_get_num_threads();
		tid = omp_get_thread_num();
		
		if(tid == 0){
			m = nthds;
		}

	}

	//printf("m = %d\n",m);

	double pnrms[m]; 

	/* Fork a team of threads giving them their own copies of variables */
	#pragma omp parallel private(nthds, tid, stride, start, stop) shared(N, vec, pnrms)
	{
		// compute thread variables
		nthds = omp_get_num_threads();
		tid = omp_get_thread_num();

		// compute stride
		stride = ceil((long double)N/nthds);

		// compute start and stop
		start = tid*stride;
		stop = (int)fminl((long double)(tid+1)*stride,(long double)N);

		pnrms[tid] = cblas_dnrm2(stop-start,&vec[start],1);
		//printf("pnrms[%d] = %+e\n",tid,pnrms[tid]);
	} 

	nrm = cblas_dnrm2(m,&pnrms[0],1);
	//printf("nrm = %+e\n",nrm);

	return nrm;
}

void grdm_ds(double* A, double* b, double* x, int n, double tol){
  double alpha, *d, *tmp;
  d = (double*) malloc(n * sizeof(double));
  tmp = (double*) malloc(n * sizeof(double));
  
  while(1){
    //printf("x[0] = %f\n", x[0]);
    //d_k = A*x_k - b
    cblas_dcopy(n, b, 1, d, 1);
    cblas_dsymv(CblasRowMajor, CblasUpper, n, 1, A, n, x, 1, -1.0, d, 1);

    //alpha_k = dot(d_k, d_k) / dot(d_k, d_k)_A
    cblas_dsymv(CblasRowMajor, CblasUpper, n, 1, A, n, d, 1, 0.0, tmp, 1);
    alpha = cblas_ddot(n, d, 1, d, 1) / cblas_ddot(n, d, 1, tmp, 1);

    cblas_dcopy(n, x, 1, tmp, 1);

    //x_k+1 = x_k + alpha_k * d_k
    cblas_daxpy(n, -alpha, d, 1, x, 1);
    cblas_daxpy(n, -1.0, x, 1, tmp, 1);

    //convergence check
    if(cblas_dnrm2(n, tmp, 1) < tol) break;
  }

  free(d);
  free(tmp);

}

// x(k+1) = M^(-1) * (b - D * x(k))
void fixpoint_iteration(double *Beta, double *D, double *x, double *b, int N, double tol)
{
	int i, j;

	double *xk, *temp, error;

	xk = (double *) malloc(N*N*sizeof(double));
	temp = (double *) malloc(N*N*sizeof(double));

	for (i=0; i<N*N; i++)
	{
		for (j=0; j<N*N; j++)	xk[j] = x[j];

		dgemm(temp, D, xk, N);
//		if (i == 0)	printf(" Dgemm finish. \n");

		for (j=0; j<N*N; j++)	temp[j] = b[j] - Beta[j]*temp[j];

		fastpoisson(temp, x, N);
//		if(i == 0)	printf(" Fast Poisson finish. \n");

		for (j=0; j<N*N; j++)	temp[j] = x[j] - xk[j];
		error = cblas_dnrm2(N*N, temp, 1);

//		printf(" Step %d finish. \n", i+1);
		if ( error < tol)
		{
			printf("\n Converges at %d step ! \n", i+1);
			break;
		}
	}
}

/* Computes the Frobenius norm of a matrix. */
double nrm2(Mat mA) {
  const int n2 = MatN2(mA);
  const void* a = MatElems(mA);
  const bool dev = MatDev(mA);
  double norm;

  switch (MatElemSize(mA)) {
  case 4:
    if (dev) {
      float norm32;
      cublasSnrm2(g_cublasHandle, n2, a, 1, (float*)&norm32);
      norm = norm32;
    } else {
      norm = cblas_snrm2(n2, a, 1);
    }
    break;

  case 8:
    if (dev) {
      cublasDnrm2(g_cublasHandle, n2, a, 1, (double*)&norm);
    } else {
      norm = cblas_dnrm2(n2, a, 1);
    }
    break;
  }

  return norm;
}

void plotMeritToZsol(double *z)
{
  int incx = 1, incy = 1;
  double q_0, q_tk;
  /*   double merit_k;  */
  /*  double tmin = 1e-12; */
  double tk = 1;
  /*   double m1=0.5; */
  double aux;
  int i = 0;
  int ii;
  if (!sPlotMerit || !sZsol)
    return;
  FILE *fp;

  for (ii = 0; ii < sN; ii++)
    szzaux[ii] = sZsol[ii] - z[ii];



  if (sPlotMerit)
  {
    /*    sPlotMerit=0;*/
    strcpy(fileName, "outputLSZsol");
    (*sFphi)(sN, z, sphi_z, 0);
    q_0 =  cblas_dnrm2(sN, sphi_z , incx);
    q_0 = 0.5 * q_0 * q_0;

    fp = fopen(fileName, "w");

    /*    sPlotMerit=0;*/
    tk = 1;
    aux = -tk;
    for (i = 0; i < 2e3; i++)
    {
      cblas_dcopy(sN, z, incx, sz2, incx);
      cblas_daxpy(sN , aux , szzaux , incx , sz2 , incy);
      (*sFphi)(sN, sz2, sphi_z, 0);
      q_tk =  cblas_dnrm2(sN, sphi_z , incx);
      q_tk = 0.5 * q_tk * q_tk;
      fprintf(fp, "%e %e\n", aux, q_tk);
      aux += tk / 1e3;
    }

    fclose(fp);
  }
}

	double computeDenseEuclideanNorm(const double *vec1, const int elements){
#ifdef USE_INTEL_MKL
		return cblas_dnrm2 (elements, vec1, 1);
#endif
#ifndef USE_INTEL_MKL
		return 0;
#endif
	}

void eblas_dnrm2_sub(size_t iStart, size_t iStop, const double* x, int incx, double* ret, std::mutex* lock)
{	//Compute this thread's contribution:
	double retSub = cblas_dnrm2(iStop-iStart, x+incx*iStart, incx);
	//Accumulate over threads (need sync):
	lock->lock();
	*ret += retSub*retSub;
	lock->unlock();
}

inline void computeColNorms(const MAT * A, double *prob) {
    int n = A->n, j, m = A->m;
    
    memset(prob, 0, n * sizeof(double));
    for (j = 0; j < n; j++) {
        prob[j] = cblas_dnrm2(m, (A->val + j * A->m), 1);
        prob[j] = pow(prob[j], 2);
    }
}

/** Create the Householder symmetric matrix v*v^T
 *
 * This matrix is used to process the rows and columns of the input matrix.
 */
static void create_house_matrix_packed(size_t order, double shift, double *source, size_t incs, double *hhp) {
	double h[order];
	int i;

	/* zero out the destination hhp (it's packed aka triangular, so the size is
	 * non-square) */
	for (i = 0; i < order * (order + 1) / 2; i++) {
		hhp[i] = 0;
	}

	/* create and normalize householder vector h */
	cblas_dcopy(order, source, incs, h, 1);
	h[0] += MYSIGN(h[0]) * cblas_dnrm2(order, h, 1);
	h[0] -= shift;
	cblas_dscal(order, 1.0 / cblas_dnrm2(order, h, 1), h, 1);

	/* hhp = h h^T */
	cblas_dspr(CblasRowMajor, CblasUpper, order, 1.0, h, 1, hhp);
}

int NonMonotomnelineSearch(double *z, double Rk)
{
  int incx = 1, incy = 1;
  double q_0, q_tk;
  /*   double merit_k;  */
  double tmin = 1e-12;
  double tmax = 1000;
  double tk = 1;
  /*   double m1=0.5; */



  (*sFphi)(sN, z, sphi_z, 0);
  q_0 =  cblas_dnrm2(sN, sphi_z , incx);
  q_0 = 0.5 * q_0 * q_0;


  while ((tmax - tmin) > 1e-1)
  {
    tk = (tmax + tmin) / 2;
    /*q_tk = 0.5*|| phi(z+tk*d) ||*/
    cblas_dcopy(sN, z, incx, sz2, incx);
    cblas_daxpy(sN , tk , sdir_descent , incx , sz2 , incy);
    (*sFphi)(sN, sz2, sphi_z, 0);
    q_tk =  cblas_dnrm2(sN, sphi_z , incx);
    q_tk = 0.5 * q_tk * q_tk;
    if (fabs(q_tk - q_0) < Rk)
      tmin = tk;
    else
      tmax = tk;
  }
  printf("NonMonotomnelineSearch, tk = %e\n", tk);
  cblas_dcopy(sN, sz2, incx, z, incx);

  if (tk <= tmin)
  {
    printf("NonMonotomnelineSearch warning, resulting tk < tmin, linesearch stopped.\n");
    return 0;
  }
  return 1;

}

double eblas_dnrm2(int N, const double* x, int incx)
{
	#ifdef MKL_PROVIDES_BLAS
	return cblas_dnrm2(N, x, incx);
	#else
	double ret = 0.;
	std::mutex lock;
	threadLaunch((N<100000) ? 1 : 0, eblas_dnrm2_sub, N, x, incx, &ret, &lock);
	return sqrt(ret);
	#endif
}

bool verify(const double result, const double *data, const size_t size)
{
    bool status = false;
    // double cblas_dnrm2(const int N,  const double *X,  const int incX);
    double stdResult = cblas_dnrm2(size, data, 1);
    status = (abs(stdResult - result) < 1e-16);

#ifdef DEBUG
    printf("The verification result is %d\n", status);
#endif
    
    return status;
}

int soclcp_compute_error_v(SecondOrderConeLinearComplementarityProblem* problem, double *z , double *w, double tolerance, SolverOptions *options, double * error)
{
  /* Checks inputs */
  if(problem == NULL || z == NULL || w == NULL)
    numericsError("soclcp_compute_error", "null input for problem and/or z and/or w");

  /* Computes w = Mz + q */
  int incx = 1, incy = 1;
  int nc = problem->nc;
  int n = problem->n;
  double *mu = problem->mu;

  double invmu = 0.0;
  cblas_dcopy(n , problem->q , incx , z , incy); // z <-q

  // Compute the current reaction
  prodNumericsMatrix(n, n, 1.0, problem->M, w, 1.0, z);

  *error = 0.;
  double rho = 1.0;
  for(int ic = 0 ; ic < nc ; ic++)
  {
    int dim = problem->coneIndex[ic+1]-problem->coneIndex[ic];
    double * worktmp = (double *)malloc(dim*sizeof(double)) ;
    int nic = problem->coneIndex[ic];
    for (int i=0; i < dim; i++)
    {
      worktmp[i] = w[nic+i] - rho * z[nic+i];
    }
    invmu = 1.0 / mu[ic];
    projectionOnSecondOrderCone(worktmp, invmu, dim);
    for (int i=0; i < dim; i++)
    {
      worktmp[i] = w[nic+i] - worktmp[i];
      *error +=  worktmp[i] * worktmp[i];
    }
    free(worktmp);
  }
  *error = sqrt(*error);

  /* Computes error */
  double normq = cblas_dnrm2(n , problem->q , incx);
  *error = *error / (normq + 1.0);
  if(*error > tolerance)
  {
    /*      if (verbose > 0) printf(" Numerics - soclcp_compute_error_velocity failed: error = %g > tolerance = %g.\n",*error, tolerance); */
    return 1;
  }
  else
    return 0;
}

/*
 *check M z + q =0
 *
 *
 */
double LinearSystem_computeError(LinearSystemProblem* problem, double *z)
{
  double * pM = problem->M->matrix0;
  double * pQ = problem->q;
  double error = 10;
  int n = problem->size;
  double * res = (double*)malloc(n * sizeof(double));
  memcpy(res, pQ, n * sizeof(double));
  cblas_dgemv(CblasColMajor,CblasNoTrans, n, n, 1.0, pM, n, z, 1, 1.0, res, 1);
  error = cblas_dnrm2(n, res, 1);
  free(res);
  return error;

}

void Orthogonalize(OrthoContext* c, double* p, int numBases, double* orthonormalBases)
{
	memcpy(c->Pv->Data, p, c->Pv->Count * sizeof(double));
	memcpy(c->Bases->Data, orthonormalBases, numBases * c->Pv->Count * sizeof(double));
	c->Bases->RowCount = numBases;
	c->Dp->Count = numBases;

	int basisLen = c->Pv->Count;
	GEMV(1, c->Bases, c->Pv, 0, c->Dp);
	for (int i = 0, offset = 0; i < numBases; i++, offset += basisLen)
		AXPY2(-1 * c->Dp->Data[i], c->Bases->Data + offset, basisLen, c->Pv->Data);
	double mag = cblas_dnrm2(basisLen, c->Pv->Data, 1);
	cblas_dscal(basisLen, 1.0 / mag, c->Pv->Data, 1);
	memcpy(p, c->Pv->Data, basisLen * sizeof(double));
}

void linesearch_Armijo(int n, double *z, double* dir, double psi_k,
                       double descentCondition, NewtonFunctionPtr* phi)
{
    double * phiVector = (double*)malloc(n * sizeof(*phiVector));
    if (phiVector == NULL)
    {
        fprintf(stderr, "NonSmoothNewton::linesearch_Armijo, memory allocation failed for phiVector\n");
        exit(EXIT_FAILURE);
    }

    /* IN :
       psi_k (merit function for current iteration)
       jacobian_psi_k (jacobian of the merit function)
       dk: descent direction

       OUT: tk, z
    */

    double sigma = 1e-4;
    double tk = 1;
    int incx = 1, incy = 1;
    double merit, merit_k;
    double tmin = 1e-12;

    /* z1 = z0 + dir */
    cblas_daxpy(n , 1.0 , dir , incx , z , incy);

    while (tk > tmin)
    {
        /* Computes merit function = 1/2*norm(phi(z_{k+1}))^2 */
        (*phi)(n, z, phiVector, 0);
        merit =  cblas_dnrm2(n, phiVector , incx);
        merit = 0.5 * merit * merit;
        merit_k = psi_k + sigma * tk * descentCondition;
        if (merit < merit_k) break;
        tk = tk * 0.5;
        /* Computes z_k+1 = z0 + tk.dir
        warning: (-tk) because we need to start from z0 at each step while z_k+1 is saved in place of z_k ...*/
        cblas_daxpy(n , -tk , dir , incx , z , incy);
    }
    free(phiVector);
    if (tk <= tmin)
        if (verbose > 0)
            printf("NonSmoothNewton::linesearch_Armijo warning, resulting tk < tmin, linesearch stopped.\n");

}

VALUE rb_blas_xnrm2(int argc, VALUE *argv, VALUE self)
{
  Matrix *dx;
  int incx;
  int incy;
  int n;
  //char error_msg[64];
  VALUE n_value,  incx_value;
  
  rb_scan_args(argc, argv, "02",   &incx_value, &n_value);
  
  Data_Get_Struct(self, Matrix, dx);

  if(incx_value == Qnil)
    incx = 1;
  else
    incx = NUM2INT(incx_value);
  
  if(n_value == Qnil)
    n = dx->nrows;
  else
    n = NUM2INT(n_value);

  if(dx == NULL || dx->ncols != 1)
  { //sprintf(error_msg, "Self is not a Vector");
    rb_raise(rb_eRuntimeError, "Self is not a Vector");
  }
  
  switch(dx->data_type)
  {
  case Single_t: //s
    return rb_float_new(cblas_snrm2(n , (float *)dx->data, incx)); 
  case Double_t: //d
    return rb_float_new(cblas_dnrm2(n , (double *)dx->data, incx)); 
  case Complex_t: //c
    return rb_float_new(cblas_scnrm2(n , dx->data, incx)); 
  case Double_Complex_t: //z
    return rb_float_new(cblas_dznrm2(n , dx->data, incx)); 
  default:
    //sprintf(error_msg, "Invalid data_type (%d) in Matrix", dx->data_type);
    rb_raise(rb_eRuntimeError, "Invalid data_type (%d) in Matrix", dx->data_type);
    return Qnil; //Never reaches here.
  }
}