void CFactorGraphModel::w_to_fparams(SGVector<float64_t> w)
{
	// if nothing changed
	if (m_w_cache.equals(w))
		return;

	if (m_verbose)
		SG_SPRINT("****** update m_w_cache!\n");

	ASSERT(w.size() == m_w_cache.size());
	m_w_cache = w.clone();

	int32_t offset = 0;
	for (int32_t fi = 0; fi < m_factor_types->get_num_elements(); ++fi)
	{
		CFactorType* ftype = dynamic_cast<CFactorType*>(m_factor_types->get_element(fi));
		int32_t w_dim = ftype->get_w_dim();
		offset += w_dim;
		SGVector<float64_t> fw(w_dim);
		SGVector<int32_t> fw_map = get_params_mapping(ftype->get_type_id());

		for (int32_t wi = 0; wi < w_dim; wi++)
			fw[wi] = m_w_cache[fw_map[wi]];

		ftype->set_w(fw);
		SG_UNREF(ftype);
	}

	ASSERT(offset == m_w_cache.size());
}

float64_t CMahalanobisDistance::compute(int32_t idx_a, int32_t idx_b)
{
	int32_t blen;
	bool bfree;
	float64_t* bvec = ((CSimpleFeatures<float64_t>*) rhs)->
		get_feature_vector(idx_b, blen, bfree);

	ASSERT(blen == mean.vlen);

	SGVector<float64_t> diff(bvec, blen);
	for (int32_t i = 0 ; i<blen ; i++)
		diff[i] -= mean[i];

	SGVector<float64_t> v = diff.clone();
	cblas_dgemv(CblasColMajor, CblasNoTrans,
		icov.num_rows, icov.num_cols, 1.0, icov.matrix, 
		diff.vlen, diff.vector, 1, 0.0, v.vector, 1);

	float64_t result = cblas_ddot(v.vlen, v.vector, 1, diff.vector, 1);

	((CSimpleFeatures<float64_t>*) lhs)->free_feature_vector(bvec, idx_b, bfree);
	v.destroy_vector();

	if (disable_sqrt)
		return result;
	else
		return CMath::sqrt(result);
}

int32_t CDisjointSet::get_unique_labeling(SGVector<int32_t> out_labels)
{
	REQUIRE(m_num_elements > 0, "%s::get_unique_labeling(): m_num_elements <= 0.\n", get_name());

	if (out_labels.size() != m_num_elements)
		out_labels.resize_vector(m_num_elements);

	SGVector<int32_t> roots(m_num_elements);
	SGVector<int32_t> flags(m_num_elements);
	SGVector<int32_t>::fill_vector(flags.vector, flags.vlen, -1);
	int32_t unilabel = 0;

	for (int32_t i = 0; i < m_num_elements; i++)
	{
		roots[i] = find_set(i);
		// if roots[i] never be found
		if (flags[roots[i]] < 0)
			flags[roots[i]] = unilabel++;
	}

	for (int32_t i = 0; i < m_num_elements; i++)
		out_labels[i] = flags[roots[i]];

	return unilabel;
}

SGVector<float64_t> CFactorGraphModel::fparams_to_w()
{
	REQUIRE(m_factor_types != NULL, "%s::fparams_to_w(): no factor types!\n", get_name());

	if (m_w_cache.size() != get_dim())
		m_w_cache.resize_vector(get_dim());

	int32_t offset = 0;
	for (int32_t fi = 0; fi < m_factor_types->get_num_elements(); ++fi)
	{
		CFactorType* ftype = dynamic_cast<CFactorType*>(m_factor_types->get_element(fi));
		int32_t w_dim = ftype->get_w_dim();
		offset += w_dim;
		SGVector<float64_t> fw = ftype->get_w();
		SGVector<int32_t> fw_map = get_params_mapping(ftype->get_type_id());

		ASSERT(fw_map.size() == fw.size());

		for (int32_t wi = 0; wi < w_dim; wi++)
			m_w_cache[fw_map[wi]] = fw[wi];

		SG_UNREF(ftype);
	}

	ASSERT(offset == m_w_cache.size());

	return m_w_cache;
}

SGVector<float64_t> CLogitLikelihood::get_log_zeroth_moments(
		SGVector<float64_t> mu, SGVector<float64_t> s2, const CLabels* lab) const
{
	SGVector<float64_t> y;

	if (lab)
	{
		REQUIRE((mu.vlen==s2.vlen) && (mu.vlen==lab->get_num_labels()),
				"Length of the vector of means (%d), length of the vector of "
				"variances (%d) and number of labels (%d) should be the same\n",
				mu.vlen, s2.vlen, lab->get_num_labels())
		REQUIRE(lab->get_label_type()==LT_BINARY,
				"Labels must be type of CBinaryLabels\n")

		y=((CBinaryLabels*)lab)->get_labels();
	}
	else
	{
		REQUIRE(mu.vlen==s2.vlen, "Length of the vector of means (%d) and "
				"length of the vector of variances (%d) should be the same\n",
				mu.vlen, s2.vlen)

		y=SGVector<float64_t>(mu.vlen);
		y.set_const(1.0);
	}

	// create an object of normal pdf function
	CNormalPDF* f=new CNormalPDF();

	// create an object of sigmoid function
	CSigmoidFunction* g=new CSigmoidFunction();

	// create an object of product of sigmoid and normal pdf functions
	CProductFunction* h=new CProductFunction(f, g);
	SG_REF(h);

	// compute probabilities using numerical integration
	SGVector<float64_t> r(mu.vlen);

	for (index_t i=0; i<mu.vlen; i++)
	{
		// set normal pdf parameters
		f->set_mu(mu[i]);
		f->set_sigma(CMath::sqrt(s2[i]));

		// set sigmoid parameters
		g->set_a(y[i]);

		// evaluate integral on (-inf, inf)
		r[i]=CIntegration::integrate_quadgk(h, -CMath::INFTY, mu[i])+
			CIntegration::integrate_quadgk(h, mu[i], CMath::INFTY);
	}

	SG_UNREF(h);

	r.log();

	return r;
}

SGVector<float64_t> CFactor::get_energies() const
{
	if (is_data_dependent() == false && m_energies.size() == 0)
	{
		const SGVector<float64_t> ft_energies = m_factor_type->get_w();
		ASSERT(ft_energies.size() == m_factor_type->get_num_assignments());
		return ft_energies;
	}
	return m_energies;
}

void CFactorGraph::loss_augmentation(SGVector<int32_t> states_gt, SGVector<float64_t> loss)
{
	if (loss.size() == 0)
	{
		loss.resize_vector(states_gt.size());
		SGVector<float64_t>::fill_vector(loss.vector, loss.vlen, 1.0 / states_gt.size());
	}

	int32_t num_vars = states_gt.size();
	ASSERT(num_vars == loss.size());

	SGVector<int32_t> var_flags(num_vars);
	var_flags.zero();

	// augment loss to incorrect states in the first factor containing the variable
	// since += L_i for each variable if it takes wrong state ever
	// TODO: augment unary factors
	for (int32_t fi = 0; fi < m_factors->get_num_elements(); ++fi)
	{
		CFactor* fac = dynamic_cast<CFactor*>(m_factors->get_element(fi));
		SGVector<int32_t> vars = fac->get_variables();
		for (int32_t vi = 0; vi < vars.size(); vi++)
		{
			int32_t vv = vars[vi];
			ASSERT(vv < num_vars);
			if (var_flags[vv])
				continue;

			SGVector<float64_t> energies = fac->get_energies();
			for (int32_t ei = 0; ei < energies.size(); ei++)
			{
				CTableFactorType* ftype = fac->get_factor_type();
				int32_t vstate = ftype->state_from_index(ei, vi);
				SG_UNREF(ftype);

				if (states_gt[vv] == vstate)
					continue;

				// -delta(y_n, y_i_n)
				fac->set_energy(ei, energies[ei] - loss[vv]);
			}

			var_flags[vv] = 1;
		}

		SG_UNREF(fac);
	}

	// make sure all variables have been checked
	int32_t min_var = SGVector<int32_t>::min(var_flags.vector, var_flags.vlen);
	ASSERT(min_var == 1);
}

SGVector<float64_t> CExactInferenceMethod::get_diagonal_vector()
{
	check_members();

	float64_t m_sigma =
			dynamic_cast<CGaussianLikelihood*>(m_model)->get_sigma();

	SGVector<float64_t> result =
			SGVector<float64_t>(m_features->get_num_vectors());

	result.fill_vector(result.vector, m_features->get_num_vectors(),
			1.0/m_sigma);

	return result;
}

void CTwoStateModel::reshape_emission_params(SGVector< float64_t >& emission_weights,
		SGVector< float64_t > w, int32_t num_feats, int32_t num_obs)
{
	emission_weights.zero();

	// Legend for state indices:
	// 0 -> start state
	// 1 -> stop state
	// 2 -> negative state (label == 0)
	// 3 -> positive state (label == 1)
	//
	// start and stop states have no emission scores

	index_t em_idx, w_idx = m_num_transmission_params;
	for ( int32_t s = 2 ; s < m_num_states ; ++s )
	{
		for ( int32_t f = 0 ; f < num_feats ; ++f )
		{
			for ( int32_t o = 0 ; o < num_obs ; ++o )
			{
				em_idx = s*num_feats*num_obs + f*num_obs + o;
				emission_weights[em_idx] = w[w_idx++];
			}
		}
	}
}

void CFactor::set_energies(SGVector<float64_t> ft_energies)
{
	REQUIRE(m_factor_type->get_num_assignments() == ft_energies.size(),
		"%s::set_energies(): ft_energies is not a valid energy table!\n", get_name());

	m_energies = ft_energies;
}

void gen_rand_data(SGVector<float64_t> lab, SGMatrix<float64_t> feat,
		float64_t dist)
{
	index_t dims=feat.num_rows;
	index_t num=lab.vlen;

	for (int32_t i=0; i<num; i++)
	{
		if (i<num/2)
		{
			lab[i]=-1.0;

			for (int32_t j=0; j<dims; j++)
				feat(j, i)=CMath::random(0.0, 1.0)+dist;
		}
		else
		{
			lab[i]=1.0;

			for (int32_t j=0; j<dims; j++)
				feat(j, i)=CMath::random(0.0, 1.0)-dist;
		}
	}
	lab.display_vector("lab");
	feat.display_matrix("feat");
}

CDualLibQPBMSOSVM::CDualLibQPBMSOSVM(
    CStructuredModel*	model,
    CStructuredLabels*	labs,
    float64_t	_lambda,
    SGVector< float64_t >	W)
    : CLinearStructuredOutputMachine(model, labs)
{
    init();
    set_lambda(_lambda);

    // get dimension of w
    int32_t nDim=this->m_model->get_dim();

    // Check for initial solution
    if (W.vlen==0)
    {
        set_w(SGVector< float64_t >(nDim));
        get_w().zero();
    }
    else
    {
        ASSERT(W.size() == nDim);
        set_w(W);
    }
}

SGVector< float64_t > CFactorGraphModel::get_joint_feature_vector(int32_t feat_idx, CStructuredData* y)
{
	// factor graph instance
	CFactorGraphFeatures* mf = CFactorGraphFeatures::obtain_from_generic(m_features);
	CFactorGraph* fg = mf->get_sample(feat_idx);

	// ground truth states
	CFactorGraphObservation* fg_states = CFactorGraphObservation::obtain_from_generic(y);
	SGVector<int32_t> states = fg_states->get_data();

	// initialize psi
	SGVector<float64_t> psi(get_dim());
	psi.zero();

	// construct unnormalized psi
	CDynamicObjectArray* facs = fg->get_factors();
	for (int32_t fi = 0; fi < facs->get_num_elements(); ++fi)
	{
		CFactor* fac = dynamic_cast<CFactor*>(facs->get_element(fi));
		CTableFactorType* ftype = fac->get_factor_type();
		int32_t id = ftype->get_type_id();
		SGVector<int32_t> w_map = get_params_mapping(id);

		ASSERT(w_map.size() == ftype->get_w_dim());

		SGVector<float64_t> dat = fac->get_data();
		int32_t dat_size = dat.size();

		ASSERT(w_map.size() == dat_size * ftype->get_num_assignments());

		int32_t ei = ftype->index_from_universe_assignment(states, fac->get_variables());
		for (int32_t di = 0; di < dat_size; di++)
			psi[w_map[ei*dat_size + di]] += dat[di];

		SG_UNREF(ftype);
		SG_UNREF(fac);
	}

	// negation (-E(x,y) = <w,phi(x,y)>)
	psi.scale(-1.0);

	SG_UNREF(facs);
	SG_UNREF(fg);

	return psi;
}

SGVector<float64_t> CProbitLikelihood::get_log_zeroth_moments(
		SGVector<float64_t> mu, SGVector<float64_t> s2, const CLabels* lab) const
{
	SGVector<float64_t> y;

	if (lab)
	{
		REQUIRE((mu.vlen==s2.vlen) && (mu.vlen==lab->get_num_labels()),
				"Length of the vector of means (%d), length of the vector of "
				"variances (%d) and number of labels (%d) should be the same\n",
				mu.vlen, s2.vlen, lab->get_num_labels())
		REQUIRE(lab->get_label_type()==LT_BINARY,
				"Labels must be type of CBinaryLabels\n")

		y=((CBinaryLabels*)lab)->get_labels();
	}
	else
	{
		REQUIRE(mu.vlen==s2.vlen, "Length of the vector of means (%d) and "
				"length of the vector of variances (%d) should be the same\n",
				mu.vlen, s2.vlen)

		y=SGVector<float64_t>(mu.vlen);
		y.set_const(1.0);
	}

	Map<VectorXd> eigen_y(y.vector, y.vlen);
	Map<VectorXd> eigen_mu(mu.vector, mu.vlen);
	Map<VectorXd> eigen_s2(s2.vector, s2.vlen);

	SGVector<float64_t> r(y.vlen);
	Map<VectorXd> eigen_r(r.vector, r.vlen);

	// compute: lp=log(normal_cdf((mu.*y)./sqrt(1+sigma^2)))
	eigen_r=eigen_mu.array()*eigen_y.array()/((1.0+eigen_s2.array()).sqrt());

	for (index_t i=0; i<eigen_r.size(); i++)
		eigen_r[i]=CStatistics::lnormal_cdf(eigen_r[i]);

	return r;
}

float64_t CMahalanobisDistance::compute(int32_t idx_a, int32_t idx_b)
{

	SGVector<float64_t> bvec = ((CDenseFeatures<float64_t>*) rhs)->
		get_feature_vector(idx_b);

	SGVector<float64_t> diff;
	SGVector<float64_t> avec;

	if (use_mean)
		diff = mean.clone();
	else
	{
		avec = ((CDenseFeatures<float64_t>*) lhs)->get_feature_vector(idx_a);
		diff=avec.clone();
	}

	ASSERT(diff.vlen == bvec.vlen);

	for (int32_t i=0; i < diff.vlen; i++)
		diff[i] = bvec.vector[i] - diff[i];

	SGVector<float64_t> v = diff.clone();
	cblas_dgemv(CblasColMajor, CblasNoTrans,
		icov.num_rows, icov.num_cols, 1.0, icov.matrix,
		diff.vlen, diff.vector, 1, 0.0, v.vector, 1);

	float64_t result = cblas_ddot(v.vlen, v.vector, 1, diff.vector, 1);

	if (!use_mean)
		((CDenseFeatures<float64_t>*) lhs)->free_feature_vector(avec, idx_a);

	((CDenseFeatures<float64_t>*) rhs)->free_feature_vector(bvec, idx_b);

	if (disable_sqrt)
		return result;
	else
		return CMath::sqrt(result);
}

float64_t CFactorGraph::evaluate_energy(const SGVector<int32_t> state) const
{
	ASSERT(state.size() == m_cardinalities.size());

	float64_t energy = 0.0;
	for (int32_t fi = 0; fi < m_factors->get_num_elements(); ++fi)
	{
		CFactor* fac = dynamic_cast<CFactor*>(m_factors->get_element(fi));
		energy += fac->evaluate_energy(state);
		SG_UNREF(fac);
	}
	return energy;
}

void SGNDArray<T>::expand(SGNDArray &big_array, SGVector<index_t>& axes)
{
	// TODO: A nice implementation would be a function like repmat in matlab
	REQUIRE(axes.size() <= 2,
			"Provided axes size (%d) must be smaller than 2.\n", axes.size());
	REQUIRE(num_dims <= 2,
			"Number of dimensions (%d) must be smaller than 2. Only 1-d and 2-d array can be expanded currently.\n", num_dims);

	// Initialize indices in big array to zeros
	SGVector<index_t> inds_big(big_array.num_dims);
	inds_big.zero();

	// Replicate the small array to the big one.
	// Go over the big one by one and take the corresponding value
	T* data_big = &big_array.array[0];
	for (int32_t vi = 0; vi < big_array.len_array; vi++)
	{
		int32_t y = 0;

		if (axes.size() == 1)
		{
			y = inds_big[axes[0]];
		}
		else if (axes.size() == 2)
		{
			int32_t ind1 = axes[0];
			int32_t ind2 = axes[1];
			y = inds_big[ind1] * dims[1] + inds_big[ind2];
		}

		*data_big = array[y];
		data_big++;

		// Move to the next index
		big_array.next_index(inds_big);
	}
}

void SGNDArray<T>::next_index(SGVector<index_t>& curr_index) const
{
	REQUIRE(curr_index.size() == num_dims,
			"The provided number of dimensions (%d) does not match the internal number of dimensions (%d).\n", curr_index.size(), num_dims);

	for (int32_t i = num_dims - 1; i >= 0; i--)
	{
		curr_index[i]++;

		if (curr_index[i] < dims[i])
			break;

		curr_index[i] = 0;
	}
}

void CFactorGraph::connect_components()
{
	if (m_dset->get_connected())
		return;

	// need to be reset once factor graph is updated
	m_dset->make_sets();
	bool flag = false;

	for (int32_t fi = 0; fi < m_factors->get_num_elements(); ++fi)
	{
		CFactor* fac = dynamic_cast<CFactor*>(m_factors->get_element(fi));
		SGVector<int32_t> vars = fac->get_variables();

		int32_t r0 = m_dset->find_set(vars[0]);
		for (int32_t vi = 1; vi < vars.size(); vi++)
		{
			// for two nodes in a factor, should be an edge between them
			// but this time link() isn't performed, if they are linked already
			// means there is another path connected them, so cycle detected
			int32_t ri = m_dset->find_set(vars[vi]);	

			if (r0 == ri)
			{
				flag = true;	
				continue;
			}

			r0 = m_dset->link_set(r0, ri);
		}

		SG_UNREF(fac);
	}
	m_has_cycle = flag;
	m_dset->set_connected(true);
}

template<class T> SGNDArray<T>::SGNDArray(const SGVector<index_t> dimensions, bool ref_counting) :
	SGReferencedData(ref_counting)
{
	num_dims = dimensions.size();
	dims = SG_MALLOC(index_t, num_dims);

	len_array = 1;
	for (int32_t i=0; i<num_dims; i++)
	{
		dims[i] = dimensions[i];
		len_array *= dims[i];
	}

	REQUIRE(len_array>0, "Length of array (%d) must be greater than 0\n", len_array);
	array = SG_MALLOC(T, len_array);
}