#include "ProcessCorrelations3d.h"

std::unique_ptr<SliceProcessCorrelations> SliceProcessCorrelations3d::construct()
{
	return std::move(std::unique_ptr<SliceProcessCorrelations3d>(new SliceProcessCorrelations3d(3)));
}

SliceProcessCorrelations3d::SliceProcessCorrelations3d(size_t size)
	:SliceProcessCorrelations(size)
{}

void SliceProcessCorrelations3d::initialise(const std::vector<double>& prev_covariances, const std::vector<double>& covariances, const std::array<size_t, 3>& ordering)
{
	assert(prev_covariances.size() == 6);
	assert(covariances.size() == 6);

	////////////////////////////////////////////////////////////////////////////////////////////////////////
	// Lower triangular matrix in raw array -> symmetric full matrix 
	////////////////////////////////////////////////////////////////////////////////////////////////////////
	auto populate_matrix = [](gelib::d_matrix& mat, std::array<double, 6>& mat_data)
	{
		mat[0] = mat_data[0];
		mat[4] = mat_data[2];
		mat[8] = mat_data[5];

		mat[1] = mat[3] = mat_data[1];
		mat[2] = mat[6] = mat_data[3];
		mat[5] = mat[7] = mat_data[4];
	};

	std::array<double, 6> data;

	std::copy(covariances.begin(), covariances.end(), data.begin());
	populate_matrix(m_slice_covariance, data);

	std::copy(prev_covariances.begin(), prev_covariances.end(), data.begin());
	populate_matrix(m_prev_slice_covariance, data);

	std::transform(covariances.begin(), covariances.end(), prev_covariances.begin(), data.begin(), std::minus<double>());
	populate_matrix(m_increment_covariance, data);

	gelib::Gaussian3d g3d;

	// check for obvious errors in the covariance matrix
	if (MFMBase::DEBUG_ON)
		gelib_impl::IO::throw_on_input_error(gelib::Gaussian3d::check_cov_matrix(m_increment_covariance));

	g3d.factor_cov_matrix(m_increment_covariance, ordering[0], ordering[1], ordering[2]);

	std::array<double, 3> M_vals;
	g3d.get_M(M_vals);

	m_alpha = M_vals[0];
	m_beta = M_vals[1];
	m_gamma = M_vals[2];

	m_M[0] = 1.0;
	m_M[4] = 1.0;
	m_M[8] = 1.0;

	m_M[m_M.index(ordering[1], ordering[2])] = m_alpha;
	m_M[m_M.index(ordering[0], ordering[2])] = m_beta;
	m_M[m_M.index(ordering[0], ordering[1])] = m_gamma;

	m_Mt = m_M;
	m_Mt.transpose();
	// TODO need to make get_variance_i function clearer in library
	// TODO pretty sure more general solution is ordering[ordering[i]] for i = 0,1,2
	m_Chat[0] = g3d.get_variance_i(ordering[1]);
	m_Chat[4] = g3d.get_variance_i(ordering[2]);
	m_Chat[8] = g3d.get_variance_i(ordering[0]);

	m_Mk_Ck_Mkt = (m_M * m_slice_covariance) * m_Mt;
	m_Mk_Ckminus1_Mkt = (m_M * m_prev_slice_covariance) * m_Mt;

}

double SliceProcessCorrelations3d::alpha() const
{
	return m_alpha;
}

double SliceProcessCorrelations3d::beta() const
{
	return m_beta;
}

double SliceProcessCorrelations3d::gamma() const
{
	return m_gamma;
}

gelib::d_matrix const& SliceProcessCorrelations3d::get_Mk_Ck_Mkt() const
{
	return m_Mk_Ck_Mkt;
}

gelib::d_matrix const& SliceProcessCorrelations3d::get_Mk_Ckminus1_Mkt() const
{
	return m_Mk_Ckminus1_Mkt;
}


gelib::d_matrix const& SliceProcessCorrelations3d::get_Mk_Ck_Ckminus1_Mkt() const
{
	return get_Chat();
}

