void PZStability::real_imag_idx(arma::uvec & idxr, arma::uvec & idxi) const {
  if(!cplx) {
    ERROR_INFO();
    throw std::runtime_error("Should not call real_imag_idx for purely real calculation!\n");
  }
  
  // Count amount of parameters
  size_t nreal=0, nimag=0;
  if(cancheck) {
    nreal+=oa*va;
    nimag+=oa*va;
    if(!restr) {
      nreal+=ob*vb;
      nimag+=ob*vb;
    }
  }
  if(oocheck) {
    nreal+=oa*(oa-1)/2;
    if(cplx)
      nimag+=oa*(oa+1)/2;
    
    if(!restr) {
      nreal+=ob*(ob-1)/2;
      if(cplx)
	nimag+=ob*(ob+1)/2;
    }
  }

  // Sanity check
  if(nreal+nimag != count_params()) {
    ERROR_INFO();
    throw std::runtime_error("Parameter count is wrong!\n");
  }
  
  // Parameter indices
  idxr.zeros(nreal);
  idxi.zeros(nimag);
  
  // Fill indices.
  size_t ir=0, ii=0;
  
  // Offset
  size_t ioff=0;
  
  if(cancheck) {
    // First are the real parameters
    for(size_t irot=0;irot<oa*va;irot++) {
      idxr(ir++)=irot;
    }
    ioff+=oa*va;
    // followed by the imaginary parameters
    for(size_t irot=0;irot<oa*va;irot++)
      idxi(ii++)=irot+ioff;
    ioff+=oa*va;
    
    if(!restr) {
      // and then again the real parameters
      for(size_t irot=0;irot<ob*vb;irot++)
	idxr(ir++)=irot + ioff;
      ioff+=ob*vb;
      // followed by the imaginary parameters
      for(size_t irot=0;irot<ob*vb;irot++)
	idxi(ii++)=irot + ioff;
      ioff+=ob*vb;
    }
  }
    
  if(oocheck) {
    // First are the real parameters
    for(size_t irot=0;irot<oa*(oa-1)/2;irot++) {
      idxr(ir++)=irot+ioff;
    }
    ioff+=oa*(oa-1)/2;
    // and then the imaginary parameters
    for(size_t irot=0;irot<oa*(oa+1)/2;irot++)
      idxi(ii++)=irot + ioff;
    ioff+=oa*(oa+1)/2;

    if(!restr) {
      // First are the real parameters
      for(size_t irot=0;irot<ob*(ob-1)/2;irot++) {
	idxr(ir++)=irot+ioff;
      }
      ioff+=ob*(ob-1)/2;
      // and then the imaginary parameters
      for(size_t irot=0;irot<ob*(ob+1)/2;irot++)
	idxi(ii++)=irot + ioff;
      ioff+=ob*(ob+1)/2;
    }
  }

  // Sanity check
  arma::uvec idx(nreal+nimag);
  idx.subvec(0,nreal-1)=idxr;
  idx.subvec(nreal,nreal+nimag-1)=idxi;
  idx=arma::sort(idx,"ascending");
  for(size_t i=0;i<idx.n_elem;i++)
    if(idx(i)!=i) {
      std::ostringstream oss;
      oss << "Element " << i << " of compound index is wrong: " << idx(i) << "!\n";
      throw std::runtime_error(oss.str());
    }
}
bool Partition(arma::mat &A, arma::uvec &label, int &nGroups){

	// to do
	int N = A.n_rows;
	arma::uvec parent(N);
	arma::uvec rank;

	for (int i = 0; i < N; i++){
		parent(i) = i;
	}
	rank.zeros(N, 1);
	arma::uvec ori_parent(parent);

	for (int i = 0; i < N; i++){
		// check equal items
		for (int j = 0; j < N; j++){
			if (A(i, j) == 0){
				continue;
			}

			// find root of node i and compress path
			int root_i = Find(parent, i);

			// find root of node j and compress path
			int root_j = Find(parent, j);

			// union both trees
			if (root_j != root_i){
				if (rank(root_j) < rank(root_i)){
					parent(root_j) = root_i;
				}
				else if (rank(root_i) < rank(root_j)){
					parent(root_i) = root_j;
				}
				else{
					parent(root_j) = root_i;
					rank(root_i) = rank(root_i) + 1;
				}
			}
		}
	}

	//parent.print("parent:");
	//rank.print("rank:");

	// label each element
	arma::uvec flag = parent == ori_parent;
	nGroups = arma::sum(flag);
	label.zeros(N,1);

	// matlab: label(flag) = 1:nGroups
	int t = 1;
	for (int i = 0; i < N; i++){
		if (flag(i)){
			label(i) = t++;
		}
	}
	//label.print("label");


	int root_i;
	for (int i = 0; i < N; i++){
		if (parent(i) == i){
			continue;
		}

		// find root of node i
		root_i = Find(parent, i);
		label(i) = label(root_i);
	}

	//label.print("label:");
	//cout << "nGroups:" << nGroups << endl;

	return true;
}