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

}

  int MA27_Refine( Ma27_Data *ma27, double x[], double rhs[], double A[],
                   double tol, int maxitref ) {

    int    n = ma27->n, error, i, j, k, nitref;
    double b_norm, resid_norm;

    PRINT( " MA27 :: Performing iterative refinement...\n" );

    /* Compute initial residual */
    b_norm = cblas_dnrm_infty( n, rhs, 1 );        
    cblas_dcopy( n, rhs, 1, ma27->residual, 1 );
    for( k = 0; k < ma27->nz; k++ ) {
      i = ma27->irn[k];
      j = ma27->jcn[k];
      ma27->residual[i] -= A[k] * x[j];
      if( i != j ) ma27->residual[j] -= A[k] * x[i];
    }
    resid_norm = cblas_dnrm_infty( n, ma27->residual, 1 );

    PRINT( " Norm of residual: %8.2e", resid_norm );
    PRINT( " Tolerance: %8.2e\n", tol*(1+b_norm) );

    /* Perform iterative refinements, if required */
    /* Work with rtmp, and copy result into residual at the end */
    nitref = 0;
    while( nitref < maxitref && resid_norm > tol * (1+b_norm) ) {

      nitref++;

      /* Solve system again with residual as rhs */
      cblas_dcopy( n, ma27->residual, 1, rhs, 1 );
      MA27CD( &n, ma27->factors, &(ma27->la), ma27->iw,
              &(ma27->liw), ma27->w, &(ma27->maxfrt),
              rhs, ma27->iw1, &(ma27->nsteps),
              ma27->icntl, ma27->info );

      error = ma27->info[0];
      if( error ) return error;

      /* Update solution: x <- x + rhs */
      cblas_daxpy( n, 1.0, rhs, 1, x, 1 );
          
      /* Update residual: residual <- residual - A rhs */
      for( k = 0; k < ma27->nz; k++ ) {
        i = ma27->irn[k];
        j = ma27->jcn[k];
        ma27->residual[i] -= A[k] * rhs[j];
        if( i != j )
          ma27->residual[j] -= A[k] * rhs[i];
      }
      resid_norm = cblas_dnrm_infty( n, ma27->residual, 1 );
            
      PRINT("   Ref %-d: Norm of residual: %8.2e\n", nitref, resid_norm);
    }

    PRINT( " done\n" );
    return 0;
  }

static PyObject *Pyma27_ma27( Pyma27Object *self, PyObject *args ) {

    PyArrayObject *a_x, *a_rhs, *a_res;
    double        *x, *rhs;
    int            i, j, k;
    int            error, comp_resid;

    /* We read a right-hand side and a solution */
    if( !PyArg_ParseTuple( args,
                           "O!O!O!i:ma27",
                           &PyArray_Type, &a_rhs,
                           &PyArray_Type, &a_x,
                           &PyArray_Type, &a_res, &comp_resid ) ) return NULL;

    if( a_rhs->descr->type_num != NPY_DOUBLE ) return NULL;
    if( a_x->descr->type_num != NPY_DOUBLE ) return NULL;
    if( a_res->descr->type_num != NPY_DOUBLE ) return NULL;
    if( !a_rhs ) return NULL;                          /* conversion error */
    if( a_rhs->nd != 1 ) return NULL;           /* b must have 1 dimension */
    if( a_rhs->dimensions[0] != self->data->n ) return NULL; /* and size n */
    if( !a_x ) return NULL;
    if( a_x->nd != 1 ) return NULL;
    if( a_x->dimensions[0] != self->data->n ) return NULL;
    if( !a_res ) return NULL;
    if( a_res->nd != 1 ) return NULL;
    if( a_res->dimensions[0] != self->data->n ) return NULL;

    rhs = (double *)a_rhs->data;
    x = (double *)a_x->data;

    /* Copy rhs into x; it will be overwritten by Ma27_Solve() */
    cblas_dcopy( self->data->n, rhs, 1, x, 1 );

    /* Solve */
    error = Ma27_Solve( self->data, x );
    if( error ) {
        fprintf( stderr, " Error return code from Solve: %-d\n", error );
        return NULL;
    }

    /* Compute residual r = rhs - Ax */
    if( comp_resid ) {
        self->data->residual = (double *)a_res->data;
        cblas_dcopy( self->data->n, rhs, 1, self->data->residual, 1 );
        for( k = 0; k < self->data->nz; k++ ) {
            i = self->data->irn[k] - 1;  /* Fortran indexing */
            j = self->data->icn[k] - 1;
            self->data->residual[i] -= self->a[k] * x[j];
            if( i != j )
                self->data->residual[j] -= self->a[k] * x[i];
        }
    }

    Py_INCREF( Py_None );
    return Py_None;
}

EncoderLayer::EncoderLayer(int numIn, int numOut, double *w, double *b, double *c) :
    numIn(numIn), numOut(numOut), y(NULL), h(NULL),
    dy(NULL), dh(NULL), binIn(true), binOut(true)
{
    this->w = new double[numIn*numOut];
    this->dw = new double[numIn*numOut];
    this->b = new double[numOut];
    this->c = new double[numIn];
    cblas_dcopy(numIn*numOut, w, 1, this->w, 1);
    cblas_dcopy(numIn, c, 1, this->c, 1);
    cblas_dcopy(numOut, b, 1, this->b, 1);
}

void diffuse(double *U, double *b, int m, int n, double *du, double *dc, double *dl, double mu) {
  setRHSx(U, b, (R+1), (R+1), mu);
  lapack_dgtsv((R+1), (R+1), dl, dc, du, b, (R+1));

  cblas_dcopy((R+1)*(R+1), b, 1, U, 1);

  setRHSy(U, b, (R+1), (R+1), mu);
  lapack_dgtsv((R+1), (R+1), dl, dc, du, b, (R+1));

  cblas_dcopy((R+1)*(R+1), b, 1, U, 1);

/*   forceBoundaryConditions(U, (R+1), (R+1), 0.0); */
}

void OPT_ALGO::parallel_owlqn(int use_list_len, float* ro_list, float** s_list, float** y_list){
    //define and initial local parameters
    float *local_g = new float[fea_dim];//single thread gradient
    float *local_sub_g = new float[fea_dim];//single thread subgradient
    float *p = new float[fea_dim];//single thread search direction.after two loop
    loss_function_gradient(w, local_g);//calculate gradient of loss by global w)
    loss_function_subgradient(local_g, local_sub_g); 
    //should add code update multithread and all nodes sub_g to global_sub_g
    two_loop(use_list_len, local_sub_g, s_list, y_list, ro_list, p);

    pthread_mutex_lock(&mutex);
    for(int j = 0; j < fea_dim; j++){
        *(global_g + j) += *(p + j);//update global direction of all threads
    }
    pthread_mutex_unlock(&mutex);

    pid_t local_thread_id;
    local_thread_id = getpid();
    if(local_thread_id == main_thread_id){
        for(int j = 0; j < fea_dim; j++){ 
            *(all_nodes_global_g + j) = 0.0;
        }
        for(int j = 0; j < fea_dim; j++){//must be pay attention
            *(global_g + j) /= n_threads;
        }   
        MPI_Allreduce(global_g, all_nodes_global_g, 1, MPI_FLOAT, MPI_SUM, MPI_COMM_WORLD);//all_nodes_global_g store shared sum of every nodes search direction
    }
    pthread_barrier_wait(&barrier);
    //should be synchronous all threads
    line_search(all_nodes_global_g);//use global search direction to search
    //update slist
    if(local_thread_id == main_thread_id){
        cblas_daxpy(fea_dim, -1, (double*)w, 1, (double*)next_w, 1);
        cblas_dcopy(fea_dim, (double*)next_w, 1, (double*)s_list[(m - use_list_len) % m], 1);
    //update ylist
        cblas_daxpy(fea_dim, -1, (double*)global_g, 1, (double*)global_next_g, 1); 
        cblas_dcopy(fea_dim, (double*)global_next_g, 1, (double*)y_list[(m - use_list_len) % m], 1);

        use_list_len++;
        if(use_list_len > m){
            for(int j = 0; j < fea_dim; j++){
                *(*(s_list + abs(m - use_list_len) % m) + j) = 0.0;
                *(*(y_list + abs(m - use_list_len) % m) + j) = 0.0;        
            }
        }
        cblas_dcopy(fea_dim, (double*)next_w, 1, (double*)w, 1);
    }
    pthread_barrier_wait(&barrier);
}

void hoa_vector_perform64_velocity(t_hoa_vector *x, t_object *dsp64, double **ins, long numins, double **outs, long numouts, long sampleframes, long flags, void *userparam)
{
    for(int i = 0; i < numins; i++)
    {
        cblas_dcopy(sampleframes, ins[i], 1, x->f_sig_ins+i, numins);
    }
    for(int i = 0; i < sampleframes; i++)
    {
        x->f_vector->processVelocity(x->f_sig_ins + i * numins, x->f_sig_outs + i * numouts);
    }
    for(int i = 0; i < numouts; i++)
    {
        cblas_dcopy(sampleframes, x->f_sig_outs+i, numouts, outs[i], 1);
    }
}

void hoa_projector_perform64(t_hoa_projector *x, t_object *dsp64, double **ins, long numins, double **outs, long numouts, long sampleframes, long flags, void *userparam)
{
    for(int i = 0; i < numins; i++)
    {
        cblas_dcopy(sampleframes, ins[i], 1, x->f_ins+i, numins);
    }
	for(int i = 0; i < sampleframes; i++)
    {
        x->f_projector->process(x->f_ins + numins * i, x->f_outs + numouts * i);
    }
    for(int i = 0; i < numouts; i++)
    {
        cblas_dcopy(sampleframes, x->f_outs+i, numouts, outs[i], 1);
    }
}

/* compute the slope vector dy for the transient equation
 * dy + cy = p. useful in the transient solver
 */
void slope_fn_block(block_model_t *model, double *y, double *p, double *dy)
{
	/* shortcuts	*/
	int n = model->n_nodes;
	double **c = model->c;

	/* for our equation, dy = p - cy */
	#if (MATHACCEL == MA_INTEL || MATHACCEL == MA_APPLE)
	/* dy = p	*/
	cblas_dcopy(n, p, 1, dy, 1);
	/* dy = dy - c*y = p - c*y */
	cblas_dgemv(CblasRowMajor, CblasNoTrans, n, n, -1, c[0],
				n, y, 1, 1, dy, 1);
	#elif (MATHACCEL == MA_AMD || MATHACCEL == MA_SUN)
	/* dy = p	*/
	dcopy(n, p, 1, dy, 1);
	/* dy = dy - c*y = p - c*y */
	dgemv('T', n, n, -1, c[0], n, y, 1, 1, dy, 1);
	#else
	int i;
	double *t = dvector(n);
	matvectmult(t, c, y, n);
	for (i = 0; i < n; i++)
		dy[i] = p[i]-t[i];
	free_dvector(t);
	#endif
}

// Solve A * x = b, input A and b.
void cblas_backsolver(double *A, double *x, double *b, int N)
{
//	int i;
//	for (i=0; i<N; i++)	x[i] = b[i];
	cblas_dcopy(N, b, 1, x, 1);
	cblas_dtrsv(CblasRowMajor, CblasUpper, CblasNoTrans, CblasNonUnit, N, A, N, x, 1);
}

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;
}

void hoa_map_perform64_in1_in2(t_hoa_map *x, t_object *dsp64, double **ins, long numins, double **outs, long numouts, long sampleframes, long flags, void *userparam)
{
	if (x->f_map->getMute(0))
	{
		for(int i = 0; i < numouts; i++)
			memset(outs[i], 0, sizeof(double)*sampleframes);
		return;
	}
    for(int i = 0; i < sampleframes; i++)
    {
		if(x->f_mode == hoa_sym_polar)
		{
			x->f_map->setRadius(0, ins[1][i]);
			x->f_map->setAzimuth(0, ins[2][i]);
		}
		else if(x->f_mode == hoa_sym_cartesian)
		{
			x->f_map->setAzimuth(0, azimuth(ins[1][i], ins[2][i]));
			x->f_map->setRadius(0, radius(ins[1][i], ins[2][i]));
		}
        x->f_map->process(&ins[0][i], x->f_sig_outs + numouts * i);
    }
    for(int i = 0; i < numouts; i++)
    {
        cblas_dcopy(sampleframes, x->f_sig_outs+i, numouts, outs[i], 1);
    }
}

void THBlas_(copy)(long n, real *x, long incx, real *y, long incy)
{
  if(n == 1)
  {
    incx = 1;
    incy = 1;
  }

#if defined(USE_BLAS) && (defined(TH_REAL_IS_DOUBLE) || defined(TH_REAL_IS_FLOAT))
  if( (n <= INT_MAX) && (incx <= INT_MAX) && (incy <= INT_MAX) )
  {
    int i_n = (int)n;
    int i_incx = (int)incx;
    int i_incy = (int)incy;

#if defined(TH_REAL_IS_DOUBLE)
    cblas_dcopy(i_n, x, i_incx, y, i_incy);
#else
    cblas_scopy(i_n, x, i_incx, y, i_incy);
#endif
    return;
  }
#endif
  {
    long i;
    for(i = 0; i < n; i++)
      y[i*incy] = x[i*incx];
  }
}

void mlcp_pgs_sbm_buildLocalProblem(int rowNumber, const SparseBlockStructuredMatrix* const blmat, LinearComplementarityProblem* local_problem, double* q, double* z)
{

  assert(blmat->blocksize0[rowNumber] > 0);

  /* Position in vector blmat->block of the required diagonal block */
  int diagPos = getDiagonalBlockPos(blmat, rowNumber);
  /* Gets diagonal block = MLocal  */
  local_problem->M->matrix0 = blmat->block[diagPos];
  local_problem->size = blmat->blocksize0[rowNumber];
  int pos = 0;

  if (rowNumber != 0)
  {
    local_problem->size -= blmat->blocksize0[rowNumber - 1];
    pos =  blmat->blocksize0[rowNumber - 1];
  }

  local_problem->M->size0 = local_problem->size;
  local_problem->M->size1 = local_problem->size;

  /* Computes qLocal */
  /* qLocal = q[rowNumber] + sum (rowM.z),
     sum over all non-diagonal blocks, rowM being the current
     row of blocks of M
  */
  cblas_dcopy(local_problem->size, &q[pos], 1, local_problem->q, 1);
  rowProdNoDiagSBM(blmat->blocksize0[blmat->blocknumber0 - 1], local_problem->size, rowNumber, blmat, z, local_problem->q, 0);

}

void THBlas_copy(long size, const real *x, long xStride, real *y, long yStride)
{
  if(size == 1)
  {
    xStride = 1;
    yStride = 1;
  }

#if USE_CBLAS
  if( (size < INT_MAX) && (xStride < INT_MAX) && (yStride < INT_MAX) )
  {
#ifdef USE_DOUBLE
    cblas_dcopy(size, x, xStride, y, yStride);
#else
    cblas_scopy(size, x, xStride, y, yStride);
#endif
    return;
  }
#endif
  {
    long i;
    for(i = 0; i < size; i++)
      y[i*yStride] = x[i*xStride];
  }
}

static void FC3D_compute_F(void* data_opaque, double* reaction, double* velocity)
{
  FrictionContactProblem* problem = ((FC3D_Newton_data*) data_opaque)->problem;
  // velocity <- M*reaction + qfree
  cblas_dcopy(problem->M->size1, problem->q, 1, velocity, 1);
  NM_gemv(1., problem->M, reaction, 1., velocity);
}

void hoa_rotate_perform64_yaw(t_hoa_rotate *x, t_object *dsp64, double **ins, long numins, double **outs, long numouts, long sampleframes, long flags, void *userparam)
{
    for(int i = 0; i < numouts; i++)
    {
        cblas_dcopy(sampleframes, ins[i], 1, x->f_ins+i, numouts);
    }
	for(int i = 0; i < sampleframes; i++)
    {
        x->f_rotate->setYaw(ins[numins-1][i]);
        x->f_rotate->process(x->f_ins + numouts * i, x->f_outs + numouts * i);
    }
    for(int i = 0; i < numouts; i++)
    {
        cblas_dcopy(sampleframes, x->f_outs+i, numouts, outs[i], 1);
    }
}

double maxeig(double *xmat, mwSignedIndex n) 
{
	// xmat is symmetric n x n matrix
	mwSignedIndex incx=1,indmax,maxloop=10000,k=0;
	double alpha, beta, dmax, dmax_temp,dmax_tol;
	double *bufveca=(double *) calloc(n,sizeof(double));
	double *bufvecb=(double *) calloc(n,sizeof(double));

	dmax_tol=.001;
	// do power series to get approximation to max eigenvalue of A+X
	alpha=0.0;cblas_dscal(n,alpha,bufveca,incx);bufveca[0]=1.0; // x_0 = [1,0,0,...] do something better later
	beta=0.0;dmax=1.0;dmax_temp=0.0;
	while ((dabsf(dmax-dmax_temp)>dmax_tol)&&(k<=maxloop)){
		dmax_temp=dmax;
		alpha=1.0;
		cblas_dgemv(CblasColMajor,CblasNoTrans,n,n,alpha,xmat,n,bufveca,incx,beta,bufvecb,incx);
		indmax=idxmax(bufvecb,n);dmax=bufvecb[indmax];
		alpha=1.0/dmax;cblas_dscal(n,alpha,bufvecb,incx);
		cblas_dcopy(n,bufvecb,incx,bufveca,incx);
		k++;
	}
	alpha=1.0;	
	// compute Rayleigh Quotient to approximate max eigenvalue of A+X
	cblas_dgemv(CblasColMajor,CblasNoTrans,n,n,alpha,xmat,n,bufvecb,incx,beta,bufveca,incx);
	dmax=doubdot(bufvecb,bufveca,n);alpha=doubnorm2(bufvecb,n);dmax=dmax/alpha/alpha;
	free(bufveca);free(bufvecb);
	return dmax;
}

void OPT_ALGO::two_loop(int use_list_len, float *local_sub_g, float **s_list, float **y_list, float *ro_list, float *p){
    float *q = new float[fea_dim];//local variable
    float *alpha = new float[m]; 
    cblas_dcopy(fea_dim, (double*)local_sub_g, 1, (double*)q, 1);
    if(use_list_len < m) m = use_list_len; 

    for(int loop = 1; loop <= m; ++loop){
        ro_list[loop - 1] = cblas_ddot(fea_dim, (double*)(&(*y_list)[loop - 1]), 1, (double*)(&(*s_list)[loop - 1]), 1);
        alpha[loop] = cblas_ddot(fea_dim, (double*)(&(*s_list)[loop - 1]), 1, (double*)q, 1)/ro_list[loop - 1];
        cblas_daxpy(fea_dim, -1 * alpha[loop], (double*)(&(*y_list)[loop - 1]), 1, (double*)q, 1);
    }
    delete [] q;
    float *last_y = new float[fea_dim];
    for(int j = 0; j < fea_dim; j++){
        last_y[j] = *((*y_list + m - 1) + j);
    }

    float ydoty = cblas_ddot(fea_dim, (double*)last_y, 1, (double*)last_y, 1);
    float gamma = ro_list[m - 1]/ydoty;
    cblas_sscal(fea_dim, gamma,(float*)p, 1);

    for(int loop = m; loop >=1; --loop){
        float beta = cblas_ddot(fea_dim, (double*)(&(*y_list)[m - loop]), 1, (double*)p, 1)/ro_list[m - loop];
        cblas_daxpy(fea_dim, alpha[loop] - beta, (double*)(&(*s_list)[m - loop]), 1, (double*)p, 1);
    }
    delete [] alpha;
    delete [] last_y;
}

void hoa_wider_perform64_wide(t_hoa_wider *x, t_object *dsp64, double **ins, long numins, double **outs, long numouts, long sampleframes, long flags, void *userparam)
{
    for(int i = 0; i < numouts; i++)
    {
        cblas_dcopy(sampleframes, ins[i], 1, x->f_signals+i, numouts);
    }
	for(int i = 0; i < sampleframes; i++)
    {
        x->f_wider->setWideningValue(ins[numins-1][i]);
        x->f_wider->process(x->f_signals + numouts * i, x->f_signals + numouts * i);
    }
    for(int i = 0; i < numouts; i++)
    {
        cblas_dcopy(sampleframes, x->f_signals+i, numouts, outs[i], 1);
    }
}