real SocSystem_Analytical::getCosts(arr& R,arr& r,uint t,const arr& qt){
  uint N=x.N;
  R.resize(N,N); R.setZero();
  r.resize(N);   r.setZero();
  real C=0.;
  
#ifndef USE_TRUNCATION //potentials for collision cost
  arr J(1,qt.N),phiHatQ(1);
  J.setZero();
  phiHatQ.setZero();
  for(uint i=0;i<obstacles.d0;i++){
    real margin = .1;
    real d = (1.-norm(x-obstacles[i])/margin);
    if(d<0) continue;
    phiHatQ(0) += d*d;
    J += ((real)2.*d/margin)*(obstacles[i]-x)/norm(x-obstacles[i]);
  }
  J.reshape(1,J.N);
  arr tJ,target(1);
  target=(real)0.;
  transpose(tJ,J);
  real colprec = (real)5e2;
  C += colprec*sqrDistance(target,phiHatQ);
  R += colprec*tJ*J;
  r += colprec*tJ*(target - phiHatQ + J*qt);
#endif
  
  if(t!=T-1) return C;
  R.setDiag(1.);
  r = x1;
  R *= prec;
  r *= prec;
  C += prec*sqrDistance(x1,x);
  return C;
}

void soc::SocSystem_Ors::getqv0(arr& q_){
   if(!WS->Qlin.N) q_.setBlockVector(WS->q0,WS->v0);
   else{
     arr q,qd;
     getqv0(q,qd);
     q_.setBlockVector(q,qd);
   }
}

 virtual double fs(arr& g, arr& H, const arr& x) {
   double A=.5, f=A*x.N;
   for(uint i=0; i<x.N; i++) f += x(i)*x(i) - A*::cos(10.*x(i));
   if(&g) {
     g.resize(x.N);
     for(uint i=0; i<x.N; i++) g(i) = 2*x(i) + 10.*A*::sin(10.*x(i));
   }
   if(&H) {
     H.resize(x.N,x.N);  H.setZero();
     for(uint i=0; i<x.N; i++) H(i,i) = 2 + 100.*A*::cos(10.*x(i));
   }
   return f;
 }

arr rightminarray(const arr& hist)
{
		arr out(hist.size());
		stack<int> S;
		for(int i=hist.size()-1 ;i>=0; --i)
		{
				while( !S.empty() && hist[S.top()] >= hist[i]) S.pop();
				if(!S.empty())out[i] = S.top();
				else out[i] = hist.size();
				S.push(i);
		}

		return out;
}

arr leftminarray(const arr& hist)
{
		arr out(hist.size());
		stack<int> S;
		for(int i=0;i<hist.size(); ++i)
		{
				while( !S.empty() && hist[S.top()] >= hist[i]) S.pop();
				if(!S.empty())out[i] = S.top();
				else out[i] = -1;
				S.push(i);
		}

		return out;
}

 void getPhi(arr& phiq_i,uint i){ NIY; 
   if(i==1){
     phiq_i.resize(1);
     for(uint i=0;i<obstacles.N;i++){
       phiq_i(0) += norm(x-obstacles[i]);
     }
   }
 }

size_t lookup_by_name(arr &names, const char *name) {
    for (size_t idx = 0; idx < names.size(); ++idx) {
        if (strcmp(name, names[idx]) == 0) {
            return idx;
        }
    }
    return 0;
}

// Prepare weights used by map2alm. weight array must be allocated already:
void PrepRingWeights(int col, arr<double> & weight, int nside) {
  double *tempweight;
  int status;
  long i;

  if (USE_WEIGHTS==1) {
    Announce("Loading Healpix map weights... ");
    tempweight = vector<double>(0, 2*nside-1);
    status = ReadHealpixWeights(col, nside, tempweight);
    if (status==0) for (i=0; i<2*nside; i++) weight[i]=1.0+tempweight[i];
    else { 
      warning("PrepRingWeights: could not load Healpix weights, using 1.0 instead.");
      weight.fill(1);
    }
    free_vector(tempweight, 0, 2*nside-1);
    Announce();
  }
  else weight.fill(1);
}

//Given an array and a value, remove all instances of that value in place and return the new length.
//The order of elements can be changed.It doesn't matter what you leave beyond the new length(超出新长度的元素可以任意).
int Array::removeElement(arr& nums, int val) 
{
	int i, j;
	for (i = j = nums.size() - 1; i >= 0; i--) 
	{
		if (nums[i] == val && i != j--)
			nums[i] = nums[j + 1];
	}
	return j + 1;
}

void getTriNormals(const Mesh& m, arr& Tn) {
  uint i;
  ors::Vector a, b, c;
  Tn.resize(m.T.d0, 3); //tri normals
  for(i=0; i<m.T.d0; i++) {
    a.set(&m.V(m.T(i, 0), 0)); b.set(&m.V(m.T(i, 1), 0)); c.set(&m.V(m.T(i, 2), 0));
    b-=a; c-=a; a=b^c; a.normalize();
    Tn(i, 0)=a.x;  Tn(i, 1)=a.y;  Tn(i, 2)=a.z;
  }
}

int max_area(const arr& hist)
{
		int max_area = INT_MIN;
		arr left  = leftminarray(hist);
		arr right = rightminarray(hist);
		for(int i=0; i < hist.size(); ++i)
		{
				int area = (right[i]-left[i]-1 )*hist[i];
				if( area > max_area) max_area = area;
		}
		return max_area;
}

void generateConditionedRandomProjection(arr& M, uint n, double condition) {
  uint i,j;
  //let M be a ortho-normal matrix (=random rotation matrix)
  M.resize(n,n);
  rndUniform(M,-1.,1.,false);
  //orthogonalize
  for(i=0; i<n; i++) {
    for(j=0; j<i; j++) M[i]()-=scalarProduct(M[i],M[j])*M[j];
    M[i]()/=length(M[i]);
  }
  //we condition each column of M with powers of the condition
  for(i=0; i<n; i++) M[i]() *= pow(condition, double(i) / (2.*double(n - 1)));
}

void sharp_sht::backward(const arr< dcomplex > &in_alm, arr< double > &out_map)
{
    dsharpjob->alm2map(in_alm.begin(), out_map.begin(), false);
}

void sharp_sht::forward(const arr< double > &in_map, arr< dcomplex > &out_alm)
{
    dsharpjob->map2alm(in_map.begin(), out_alm.begin(), false);
}

void ParticleAroundWalls::phi_t(arr& phi, arr& J, uint t, const arr& x_bar){
  uint T=get_T(), n=dim_x(), k=get_k();

  //assert some dimensions
  CHECK(x_bar.d0==k+1,"");
  CHECK(x_bar.d1==n,"");
  CHECK(t<=T,"");

  //-- transition costs: append to phi
  if(k==1)  phi = x_bar[1]-x_bar[0]; //penalize velocity
  if(k==2)  phi = x_bar[2]-2.*x_bar[1]+x_bar[0]; //penalize acceleration
  if(k==3)  phi = x_bar[3]-3.*x_bar[2]+3.*x_bar[1]-x_bar[0]; //penalize jerk

  //-- walls: append to phi
  //Note: here we append to phi ONLY in certain time slices: the dimensionality of phi may very with time slices; see dim_phi(uint t)
  double eps=.1, power=2.;
  if(!hardConstrained){
    //-- wall costs
    for(uint i=0;i<n;i++){ //add barrier costs to each dimension
      if(t==T/4)   phi.append(MT::ineqConstraintCost(i+1.-x_bar(k,i), eps, power));  //middle factor: ``greater than i''
      if(t==T/2)   phi.append(MT::ineqConstraintCost(x_bar(k,i)+i+1., eps, power));  //last factor: ``lower than -i''
      if(t==3*T/4) phi.append(MT::ineqConstraintCost(i+1.-x_bar(k,i), eps, power));  //middle factor: ``greater than i''
      if(t==T)     phi.append(MT::ineqConstraintCost(x_bar(k,i)+i+1., eps, power));  //last factor: ``lower than -i''
    }
  }else{
    //-- wall constraints
    for(uint i=0;i<n;i++){ //add barrier costs to each dimension
      if(t==T/4)   phi.append((i+1.-x_bar(k,i)));  //middle factor: ``greater than i''
      if(t==T/2)   phi.append((x_bar(k,i)+i+1.));  //last factor: ``lower than -i''
      if(t==3*T/4) phi.append((i+1.-x_bar(k,i)));  //middle factor: ``greater than i''
      if(t==T)     phi.append((x_bar(k,i)+i+1.));  //last factor: ``lower than -i''
    }
  }

  uint m=phi.N;
  CHECK(m==dim_phi(t),"");

  if(&J){ //we also need to return the Jacobian
    J.resize(m,k+1,n).setZero();

    //-- transition costs
    for(uint i=0;i<n;i++){
      if(k==1){ J(i,1,i) = 1.;  J(i,0,i) = -1.; }
      if(k==2){ J(i,2,i) = 1.;  J(i,1,i) = -2.;  J(i,0,i) = 1.; }
      if(k==3){ J(i,3,i) = 1.;  J(i,2,i) = -3.;  J(i,1,i) = +3.;  J(i,0,i) = -1.; }
    }

    //-- walls
    if(!hardConstrained){
      for(uint i=0;i<n;i++){
        if(t==T/4)   J(n+i,k,i) = -MT::d_ineqConstraintCost(i+1.-x_bar(k,i), eps, power);
        if(t==T/2)   J(n+i,k,i) =  MT::d_ineqConstraintCost(x_bar(k,i)+i+1., eps, power);
        if(t==3*T/4) J(n+i,k,i) = -MT::d_ineqConstraintCost(i+1.-x_bar(k,i), eps, power);
        if(t==T)     J(n+i,k,i) =  MT::d_ineqConstraintCost(x_bar(k,i)+i+1., eps, power);
      }
    }else{
      for(uint i=0;i<n;i++){
        if(t==T/4)   J(n+i,k,i) = -1.;
        if(t==T/2)   J(n+i,k,i) = +1.;
        if(t==3*T/4) J(n+i,k,i) = -1.;
        if(t==T)     J(n+i,k,i) = +1.;
      }
    }
  }
}

template<typename... Ix> size_t offset_at(const arr& a, Ix&&... idx) { return a.offset_at(idx...); }

template<typename... Ix> size_t index_at(const arr& a, Ix&&... idx) { return a.index_at(idx...); }

template<typename... Ix> arr data(const arr& a, Ix&&... index) {
    return arr(a.nbytes() - a.offset_at(index...), (const uint8_t *) a.data(index...));
}

void SocSystem_Analytical::getProcess(arr& A,arr& a,arr& B){
  uint N=x.N;
  A.setDiag(1.,N);
  B.setDiag(1.,N);
  a.resize(N); a.setZero();
}

bool sRprop::step(arr& w, const arr& grad, uint *singleI) {
    if(!stepSize.N) { //initialize
        stepSize.resize(w.N);
        lastGrad.resize(w.N);
        lastGrad.setZero();
        stepSize = delta0;
    }
    CHECK(grad.N==stepSize.N, "Rprop: gradient dimensionality changed!");
    CHECK(w.N==stepSize.N   , "Rprop: parameter dimensionality changed!");

    uint i=0, I=w.N;
    if(singleI) {
        i=*(singleI);
        I=i+1;
    }
    for(; i<I; i++) {
        if(grad.elem(i) * lastGrad(i) > 0) { //same direction as last time
            if(rMax) dMax=fabs(rMax*w.elem(i));
            stepSize(i) = _mymin(dMax, incr * stepSize(i)); //increase step size
            w.elem(i) += stepSize(i) * -_sgn(grad.elem(i)); //step in right direction
            lastGrad(i) = grad.elem(i);                    //memorize gradient
        } else if(grad.elem(i) * lastGrad(i) < 0) { //change of direction
            stepSize(i) = _mymax(dMin, decr * stepSize(i)); //decrease step size
            w.elem(i) += stepSize(i) * -_sgn(grad.elem(i)); //step in right direction (undo half the step)
            lastGrad(i) = 0;                               //memorize to continue below next time
        } else {                              //after change of direcion
            w.elem(i) += stepSize(i) * -_sgn(grad.elem(i)); //step in right direction
            lastGrad(i) = grad.elem(i);                    //memorize gradient
        }
    }

    return stepSize.max() < incr*dMin;
}