// m=m-m*v*v'
void sub_m_v_vT(mat &m, const vec &v)
{
  vec v2(m.rows());
  double tmp, *v2p;
  const double *vp;
  int i, j;

  it_assert(v.size() == m.cols(), "sub_m_v_vT()");

  v2p = v2._data();
  for (i = 0; i < m.rows(); i++) {
    tmp = 0.0;
    vp = v._data();
    for (j = 0; j < m.cols(); j++)
      tmp += *(vp++) * m._elem(i, j);
    *(v2p++) = tmp;
  }

  v2p = v2._data();
  for (i = 0; i < m.rows(); i++) {
    vp = v._data();
    for (j = 0; j < m.cols(); j++)
      m._elem(i, j) -= *v2p * *(vp++);
    v2p++;
  }
}

// fonction retournant le produit d'une matrice et d'un vecteur
vec Calculs::produit(mat MM, vec V)
{
       VV.resize(MM.size());
       for(kk=0;kk<MM.size();kk++)
       {
               som = 0;
               for(ll=0;ll<V.size();ll++)
               {
                       som = som+MM[kk][ll]*V[ll];
               }
               VV[kk] = som;
       }
       return VV;
}

inline void update_WtA(mat & WtA, const mat & W, const mat & W1, const mat & H2, const mat & A)
{
	// compute WtA = (W[:, 0:k-1], W1)^T (A - W[, k:end] H2^T)
	if (H2.empty())
		update_WtA(WtA, W, W1, A);
	else
	{
		int k = W.n_cols - H2.n_cols;
		//std::cout << "1.3" << std::endl;
		//A.print("A = ");
		//(W.cols(k, W.n_cols-1) * H2.t()).print("W[, k:] = ");
		update_WtA(WtA, W.cols(0, k-1), W1, A - W.cols(k, W.n_cols-1) * H2.t());
	}
}

//! get gravity and body acc. components of the window
//! @param[in] &window		reference to the window
//! @param[out] &gravity	reference to the gravity comp. extracted from the window
//! @param[out] &body		reference to the body acc. comp. extracted from the window
void Classifier::analyzeWindow(mat &window, mat &gravity, mat &body)
{
	// perform median filtering to reduce the noise
	int n = 3;
	mat clean_window = window.t();
	medianFilter(clean_window, n);
	clean_window = clean_window.t();

	// discriminate between gravity and body acc. components
	mat tempgr = clean_window.t();
	gravity = ChebyshevFilter(tempgr);
	gravity = gravity.t();
	body = clean_window - gravity;
}

// convertion
void mat2Blob(mat& mA, shared_ptr<Blob>& out, int c, int h, int w) {
    int n = mA.n_rows;
    assert(mA.n_cols == c*h*w);

    mA = mA.t();
    if (out) {
        out.reset();
    }
    out.reset(new Blob(n, c, h, w));
    for (int i = 0; i < n; ++i) {
        (*out)[i] = cube(mA.colptr(i), h, w, c);
    }
    return;
}

double Jastrow::getRatio(const int &particleNum, const mat &rNew, const mat &rOld){
    double argument = 0;
    double fNew = 0;
    double fOld = 0;
    double r12 = 0;
    for (int i = 0; i < particleNum; i++){
        r12Vec = rNew.row(particleNum) - rNew.row(i);
        r12 = sqrt(r12Vec(0)*r12Vec(0) + r12Vec(1)*r12Vec(1) + r12Vec(2)*r12Vec(2));
        fNew = f(r12, particleNum, i);

        r12Vec = rOld.row(particleNum) - rOld.row(i);
        r12 = sqrt(r12Vec(0)*r12Vec(0) + r12Vec(1)*r12Vec(1) + r12Vec(2)*r12Vec(2));
        fOld = f(r12, particleNum, i);

        argument += fNew - fOld;
    }
    for (int i = particleNum + 1; i < nParticles; i++){
        r12Vec = rNew.row(particleNum) - rNew.row(i);
        r12 = sqrt(r12Vec(0)*r12Vec(0) + r12Vec(1)*r12Vec(1) + r12Vec(2)*r12Vec(2));
        fNew = f(r12, particleNum, i);

        r12Vec = rOld.row(particleNum) - rOld.row(i);
        r12 = sqrt(r12Vec(0)*r12Vec(0) + r12Vec(1)*r12Vec(1) + r12Vec(2)*r12Vec(2));
        fOld = f(r12, particleNum, i);

        argument += fNew - fOld;
    }
    return exp(argument);
}

//used for both spheric and hybrid multilateration
static
bool generate_meas(vec &meas, const bvec &method, const mat &bs_pos, const vec &ms_pos)
{
  unsigned int method_len = length(method);
  unsigned int nb_bs = bs_pos.cols();
  bool column = true;
  if(3 == nb_bs) {
    nb_bs = bs_pos.rows();
    column = false;
  }
  if((nb_bs < method_len) || (3 != length(ms_pos))) {
    return false;
  }
  meas.set_size(method_len);
  vec pos(3);
  vec pos_ref(3);
  pos_ref = column ? bs_pos.get_col(0) : bs_pos.get_row(0);
  for(unsigned int k = 0; k < method_len; ++k) {
    if(bin(1) == method[k]) {  /* hyperbolic */
      pos = column ? bs_pos.get_col(k + 1) : bs_pos.get_row(k + 1);
      meas[k] = get_dist(pos, ms_pos) - get_dist(pos_ref, ms_pos);
    }
    else { /* spherical */
      pos = column ? bs_pos.get_col(k) : bs_pos.get_row(k);
      meas[k] = get_dist(pos, ms_pos);
    }
  }
  return true;
}

mat bb_shift_relative(mat bb,pair<double,double>shift)
{
	bb.row(1) = bb.row(1)+(bb.row(3)-bb.row(1)+1)*shift.first;
	bb.row(2) = bb.row(2)+(bb.row(4)-bb.row(2)+1)*shift.second;
	bb.row(3) = bb.row(3)+(bb.row(3)-bb.row(1)+1)*shift.first;
	bb.row(4) = bb.row(4)+(bb.row(4)-bb.row(2)+1)*shift.second;
}

void forward_substitution(const mat &L, int p, const vec &b, vec &x)
{
    assert( L.rows() == L.cols() && L.cols() == b.size() && b.size() == x.size() && p <= L.rows()/2 );
    int n = L.rows(), i, j;

    x=b;
  
    for (j=0;j<n;j++) {
	x(j)/=L(j,j);
	for (i=j+1;i<MIN(j+p+1,n);i++) {
	    x(i)-=L(i,j)*x(j);
	}
    }
}

mat backslash(const mat &A, const mat &B)
{
  mat ls_solve(const mat &A, const mat &b);
  mat ls_solve_od(const mat &A, const mat &b);
    int m=A.rows(), n=A.cols();

    if (m == n)
	return ls_solve(A,B);
    else if (m > n)
	return ls_solve_od(A,B);
    
    //it_error("Cannot solve under-determined linear equation systems!");
    return vec(0);
}

//Beregning av enkeltderiverte til orbitalene
rowvec3 Wavefunction::Gradient(const mat &r, const int number_particle, const int &Orbital_number, double alpha, int atom_nr)
{
    double Sum_I_Eksponenten = 0;
    rowvec3 Gradienten;
    Gradienten(0)=0; Gradienten(1)=0; Gradienten(2)=0;

    for (int k = 0; k < dimension; k++)
    {
        Sum_I_Eksponenten += (r(number_particle, k) - R(atom_nr, k)) * (r(number_particle, k) - R(atom_nr, k));
    }

    Sum_I_Eksponenten = sqrt(Sum_I_Eksponenten);

    if (Orbital_number == 0)
    {
        Gradienten = -(alpha * (r.row(number_particle)-R.row(atom_nr)) / Sum_I_Eksponenten) * exp(-alpha*Sum_I_Eksponenten);
    }

    if (Orbital_number == 1)
    {
        Gradienten = (alpha*(r.row(number_particle) - R.row(atom_nr))*(alpha*Sum_I_Eksponenten-4) / (4*Sum_I_Eksponenten)) * exp(-alpha*Sum_I_Eksponenten/2);
    }

    if (Orbital_number == 2)
    {
        Gradienten(0) = -alpha*(r(number_particle, 0)-R(atom_nr, 0)) * (r(number_particle ,0)-R(atom_nr, 0)) + 2*Sum_I_Eksponenten;
        Gradienten(1) = -alpha*(r(number_particle, 0)-R(atom_nr,0)) * (r(number_particle ,1)-R(atom_nr,1));
        Gradienten(2) = -alpha*(r(number_particle, 0)-R(atom_nr,0)) * (r(number_particle ,2)-R(atom_nr,2));
        Gradienten = Gradienten * exp(-alpha*Sum_I_Eksponenten / 2) / (2*Sum_I_Eksponenten);
    }

    if (Orbital_number == 3)
    {
        Gradienten(0) = -alpha*(r(number_particle, 1)-R(atom_nr,1)) * (r(number_particle ,0)-R(atom_nr, 0));
        Gradienten(1) = -alpha*(r(number_particle, 1)-R(atom_nr,1)) * (r(number_particle ,1)-R(atom_nr,1)) + 2*Sum_I_Eksponenten;
        Gradienten(2) = -alpha*(r(number_particle, 1)-R(atom_nr,1)) * (r(number_particle ,2)-R(atom_nr,2));
        Gradienten = Gradienten * exp(-alpha*Sum_I_Eksponenten / 2) / (2*Sum_I_Eksponenten);
    }

    if (Orbital_number == 4)
    {
        Gradienten(0) = -alpha*(r(number_particle, 2)-R(atom_nr,2)) * (r(number_particle ,0)-R(atom_nr, 0));
        Gradienten(1) = -alpha*(r(number_particle, 2)-R(atom_nr,2)) * (r(number_particle ,1)-R(atom_nr,1));
        Gradienten(2) = -alpha*(r(number_particle, 1)-R(atom_nr,1)) * (r(number_particle ,2)-R(atom_nr,2)) + 2*Sum_I_Eksponenten;
        Gradienten = Gradienten * exp(-alpha*Sum_I_Eksponenten / 2) / (2*Sum_I_Eksponenten);
    }

    return Gradienten;
}

void randselect(mat X, mat y, int n, mat &outX, mat &outy) {
  std::vector<int> indeces;
	for (int i = 0; i < (int)X.n_rows; i++) {
		indeces.push_back(i);
	}
	random_shuffle(indeces.begin(), indeces.end());
	mat _X_ = X;
	mat _y_ = y;
	outX = mat(n, X.n_cols);
	outy = mat(n, 1);
	for (int i = 0; i < n; i++) {
		outX.row(i) = _X_.row(indeces[i]);
		outy.row(i) = _y_.row(indeces[i]);
	}
}

mat mul(mat &A, mat &B) //A: m*n, B: n*p
{
    mat C(A.size(), vec(B[0].size())); //C: m:p
    for (int i = 0; i < A.size(); i++)
    {
        for (int k = 0; k < B.size(); k++)
        {
            for (int j = 0; j < B[0].size(); j++)
            {
                C[i][j] = (C[i][j] + A[i][k] * B[k][j]) % M;
            }
        }
    }
    return C;
}

mat pow(mat A, ll n)
{
	mat B(A.size(),vec(A.size()));
	for(int i=0;i<A.size();i++)
	{
		B[i][i]=1ll;
	}
	while(n>0)
	{
		if(n&1)B = mul(B,A);
		A = mul(A,A);
		n >>= 1;
	}
	return B;
}

mat mul(mat &A,mat &B)
{
	mat C(A.size(),vec(B[0].size()));
	for(int i=0;i<A.size();i++)
	{
		for(int k=0;k<B.size();k++)
		{
			for(int j=0;j<B[0].size();j++)
			{
				C[i][j]=(C[i][j]+A[i][k]*B[k][j])%MOD;
			}
		}
	}
	return C;
}

mat pow(mat A, LL n)
{
     int i, j;
     mat B(A.size(), vec(A.size()));
     for(i = 0; i < A.size(); i++)
          B[i][i] = 1;
     while(n > 0)
     {
          if(n&1)
               B = mul(B, A);
          A = mul(A, A);
          n = n >> 1;
     }
     return B;
}

mat FMRLasso::GenoStd(mat& geno)
{
    assert(geno.n_rows == mN);
    assert(geno.n_cols == mP);
    
    double q;
    for ( unsigned int j = 0; j < mP; j++)
    {
        q = sum(geno.col(j)) / (2.0 * mN); // BE CAREFUL FOR A VECTOR 0 INDEX
        //cout << q << endl;
        geno.col(j) = (geno.col(j) - 2.0 * q) / sqrt(2.0 * q * (1.0 - q));
    }
    
    return geno;
}

void ConstantCF::covarianceGradient(mat& grad, const int parameterNumber, const mat& X) const
{
	assert(parameterNumber == 0);

	Transform* t = getTransform(parameterNumber);
	double gradientTransform = t->gradientTransform(getParameter(parameterNumber));

	switch(parameterNumber)
	{
		case 0 :
		{
		    grad = gradientTransform * ones(X.rows(), X.rows());
		}
	}
}

void tlin::traduceD(const mat &m, SuperMatrix *&A)
{
	int rows = m.rows(), cols = m.cols();

	if (!A)
		allocD(A, rows, cols);

	int lda;
	double *Avalues = 0;
	readDN(A, lda, Avalues);

	assert(A->nrow == rows && A->ncol == cols && lda == rows);

	memcpy(Avalues, m.values(), rows * cols * sizeof(double));
}

void SeqOls::useObservations(mat X, mat Y  // A n x p matrix with n responses,
                             // one response per row
                            ) {
    if (X.size() != Y.size()) {
        cout << " The predictor/response matrices should have the same size";
        cout << endl;
    }

    n = X.n_rows;
    p = X.n_cols;
    X = join_rows(X, ones(n, 1));
    P = inv((X.t()) * X);
    B = P * (X.t() * Y);
    G = inv(B.rows(0, p - 1));
}