/*
 * To change this license header, choose License Headers in Project Properties.
 * To change this template file, choose Tools | Templates
 * and open the template in the editor.
 */

#include "Rbm.hpp"

void mylog(const char* format, ...);
#define printf mylog

#define EPSILON_SIGMA	0.001

Rbm::Rbm(Weights &weights, const MatrixXd &batch)
: m_w(weights)
, m_batch(batch)
, m_variableSigma(weights.getNumVisible())
, m_progress(0)
{
	setMiniBatchSize(batch.rows());
	Noise_Init(&m_noise, 0x32727155);
	m_variableSigma.fill(m_params.m_constantSigma);
	updateHiddenBatch();
}

Rbm::~Rbm()
{
	Noise_Free(&m_noise);
}

void Rbm::noiseGaussian(MatrixXd &dst)
{
	for (size_t i=0; i < dst.rows(); i++)
	{
		for (size_t j=0; j < dst.cols(); j++)
		{
			dst(i, j) = Noise_Gaussian(&m_noise);
		}
	}
}

void Rbm::noiseUniform(MatrixXd &dst)
{
	for (size_t i=0; i < dst.rows(); i++)
	{
		for (size_t j=0; j < dst.cols(); j++)
		{
			dst(i, j) = Noise_Uniform(&m_noise);
		}
	}
}

void Rbm::sampleGaussian(MatrixXd &dst, MatrixXd const &src, const MatrixXd &sigma)
{
	MatrixXd n(src.rows(), src.cols());

	noiseGaussian(n);
	dst = sigma.array()*n.array() + src.array();
}

void Rbm::sampleGaussian(MatrixXd &srcDst, const MatrixXd &sigma)
{
	MatrixXd n(srcDst.rows(), srcDst.cols());

	noiseGaussian(n);
	srcDst.array() += sigma.array()*n.array();
}

void Rbm::sampleGaussian(MatrixXd &srcDst, const double &sigma)
{
	sampleGaussian(srcDst, sigma*MatrixXd::Ones(srcDst.rows(), srcDst.cols()));
}

void Rbm::sample(MatrixXd &srcDst)
{
	sample(srcDst, srcDst);
}

void Rbm::sample(MatrixXd &dst, MatrixXd const &src)
{
	MatrixXd n(src.rows(), src.cols());

	noiseUniform(n);

	dst = (src.array() > n.array()).cast<double>();
	
}

void Rbm::probsLogistic(MatrixXd &src)
{
	src = (1 + (-src.array()).exp()).array().cwiseInverse();
}

void Rbm::probsLogistic(RowVectorXd &src)
{
	src = (1 + (-src.array()).exp()).array().cwiseInverse();
}

void Rbm::probsLogistic(MatrixXd &src, const MatrixXd &sigma)
{
	src.array() /= (sigma.array() + EPSILON_SIGMA);
	probsLogistic(src);
}

void Rbm::probsLogistic(RowVectorXd &src, const RowVectorXd &sigma)
{
	src.array() /= (sigma.array() + EPSILON_SIGMA);
	probsLogistic(src);
}

void Rbm::probsGaussian(MatrixXd &src, const MatrixXd &sigma)
{
	src.array() = 1 - src.array();
	src.array() *= src.array();
	src.array() *= -0.5;

	MatrixXd var = sigma;
	var.array() += EPSILON_SIGMA;
	var.array() *= var.array();

	src.array() /= var.array();
	src.array() = src.array().exp();

	MatrixXd k = var;

	k.array() *= 2*3.14159265359;
	k.array() = k.array().sqrt();
	k.array() = 1.0/k.array();

	src.array() *= k.array();
}

void Rbm::probsGaussian(RowVectorXd &src, const RowVectorXd &sigma)
{
	src.array() = 1 - src.array();
	src.array() *= src.array();
	src.array() *= -0.5;

	RowVectorXd var = sigma;
	var.array() += EPSILON_SIGMA;
	var.array() *= var.array();

	src.array() /= var.array();
	src.array() = src.array().exp();

	RowVectorXd k = var;

	k.array() *= 2*3.14159265359;
	k.array() = k.array().sqrt();
	k.array() = 1.0/k.array();

	src.array() *= k.array();
}

RowVectorXd Rbm::normalizeData(RowVectorXd const &src, RowVectorXd const &mu, RowVectorXd const &var)
{
	// Remove mean
	RowVectorXd res = src - mu;
//		res.array() /= var.array() + EPSILON_SIGMA;

//		cout << __PRETTY_FUNCTION__ << ": " << res << endl;
	return res;
}

RowVectorXd Rbm::calcMean(MatrixXd const &batch)
{
	// Remove mean
	RowVectorXd res = batch.colwise().mean();

//		cout << __PRETTY_FUNCTION__ << ": " << res << endl;
	return res;

}

RowVectorXd Rbm::calcSigma(MatrixXd const &batch)
{
	MatrixXd x = batch.rowwise() - batch.colwise().mean();

	x.array() *= x.array();

	RowVectorXd res = x.colwise().mean().array().sqrt();

//		cout << __PRETTY_FUNCTION__ << ": " << res << endl;
	return res;
}

MatrixXd Rbm::calcZ(MatrixXd &v, MatrixXd &h)
{

	MatrixXd t1(v.rows(), m_w.getNumVisible());

	t1 = v - m_w.visibleBias().transpose().replicate(v.rows(), 1);
	t1.array() *= t1.array();
	t1.array() *= 0.5;

	t1 -= (h * m_w.weights().transpose());

	return t1;
}

void Rbm::train(uint32_t numEpochs, double sigmaMin)
{
	uint32_t i;
	uint32_t epoch;
	uint32_t gibbs;

	size_t trainingSize = m_batch.rows();
	size_t trainingSizeRemain = trainingSize;
	size_t batchRowIndex = 0;
	
	double dProgress = 1.0/(numEpochs*(double)trainingSize/std::min(m_params.m_miniBatchSize, trainingSize));

	MatrixXd dBiasV_curr(MatrixXd::Zero(1, m_w.getNumVisible()));
	MatrixXd dBiasH_curr(MatrixXd::Zero(1, m_w.getNumHidden()));
	MatrixXd dW_curr(MatrixXd::Zero(m_w.getNumVisible(), m_w.getNumHidden()));
	MatrixXd dBiasV(MatrixXd::Zero(1, m_w.getNumVisible()));
	MatrixXd dBiasH(MatrixXd::Zero(1, m_w.getNumHidden()));
	MatrixXd dW(MatrixXd::Zero(m_w.getNumVisible(), m_w.getNumHidden()));

	m_progress = 0;
	while (trainingSizeRemain)
	{
		cout << "trainingSizeRemain: " << trainingSizeRemain << endl;
		size_t toSlice = std::min(m_params.m_miniBatchSize, trainingSizeRemain);
		MatrixXd batch = m_batch.block(batchRowIndex, 0, toSlice, m_w.getNumVisible());
		trainingSizeRemain -= toSlice;
		batchRowIndex += toSlice;
		size_t batchSize = batch.rows();
		double mu_w = m_params.m_muWeights/std::min(m_params.m_miniBatchSize, trainingSize);
		double mu_biasV = m_params.m_muWeights/std::min(m_params.m_miniBatchSize, trainingSize);
		double mu_biasH = m_params.m_muWeights/std::min(m_params.m_miniBatchSize, trainingSize);

		MatrixXd batch_sampled(batchSize, m_w.getNumVisible());
		MatrixXd v_sampled(batchSize, m_w.getNumVisible()); 
		MatrixXd vis(batchSize, m_w.getNumVisible());
		MatrixXd hid(batchSize, m_w.getNumHidden());
		
		if (m_params.m_doNormalizeData)
		{
			RowVectorXd mean = calcMean(batch);
			for (i=0; i < batchSize; i++)
			{
				RowVectorXd x = batch.row(i);
				batch.row(i) = normalizeData(x, mean, m_variableSigma);
			}
		}
		
		for (epoch=0; epoch < numEpochs; epoch++)
		{
			onProgressChanged();

			if (m_params.m_doSampleBatch)
			{
				// When the hidden units are being driven by data, always use stochastic binary states
				sample(batch_sampled, batch);

				// Create hidden layer base on sampled training data
				hid = batch_sampled * m_w.weights();
				hid += m_w.hiddenBias().replicate(batchSize, 1);
				probsLogistic(hid);
			}
			else
			{
				// Create hidden layer base on training data
				hid = batch * m_w.weights();
				hid += m_w.hiddenBias().replicate(batchSize, 1);
				probsLogistic(hid);
			}
			// Sample hidden
			if (!m_params.m_doRaoBlackwell)
			{
				sample(hid);
			}

			// Update weights (positive phase)
			dBiasV_curr = batch.colwise().sum();
			dBiasH_curr = hid.colwise().sum();
			dW_curr = batch.transpose() * hid;

			for (gibbs=0; gibbs < m_params.m_numGibbs; gibbs++)
			{
				if (m_params.m_useHiddenGaussian)
				{
					sampleGaussian(hid, m_params.m_constantSigma);
				}
				else
				{
					sample(hid);
				}
				if (m_params.m_useVisibleGaussian)
				{
					// Create visible reconstruction (a fantasy...) given hid
					vis = hid * m_w.weights().transpose();
					vis += m_w.visibleBias().replicate(batchSize, 1);
					sampleGaussian(v_sampled, vis, m_variableSigma.replicate(batchSize, 1));
					hid = v_sampled * m_w.weights();
					hid += m_w.hiddenBias().replicate(batchSize, 1);
					probsLogistic(hid);
					
				}
				else
				{
					// Create visible reconstruction (a fantasy...) given hid
					vis = hid * m_w.weights().transpose();
					vis += m_w.visibleBias().replicate(batchSize, 1);
					probsLogistic(vis, m_variableSigma.replicate(batchSize, 1));
					if (m_params.m_doSampleVisible)
					{
						sample(v_sampled, vis);
						// Create hidden representation given sampled v
						hid = v_sampled * m_w.weights();
						hid += m_w.hiddenBias().replicate(batchSize, 1);
						probsLogistic(hid);
					}
					else
					{
						// Create hidden representation given v
						hid = vis * m_w.weights();
						hid += m_w.hiddenBias().replicate(batchSize, 1);
						probsLogistic(hid);
					}
				}
			}

			// Update weights (negative phase)
			dBiasV_curr -= vis.colwise().sum();
			dBiasH_curr -= hid.colwise().sum();
			dW_curr -= vis.transpose() * hid;

			m_w.visibleBias() += mu_biasV*(m_params.m_momentum*dBiasV + (1-m_params.m_momentum)*dBiasV_curr);
			dBiasV = dBiasV_curr;

			if (m_params.m_doSparse)
			{
				MatrixXd h1 = hid-MatrixXd::Ones(hid.rows(), hid.cols())*m_params.m_sparsity;
				RowVectorXd hm = h1.colwise().mean();
				m_w.hiddenBias() -=  m_params.m_muSparsity * hm;
			}
			else
			{
				m_w.hiddenBias() += mu_biasH*(m_params.m_momentum*dBiasH + (1-m_params.m_momentum)*dBiasH_curr);
			}
			dBiasH = dBiasH_curr;

			m_w.weights() += mu_w*((m_params.m_momentum*dW + (1-m_params.m_momentum)*dW_curr) - m_params.m_weightDecay*m_w.weights());
			dW = dW_curr;

			if (m_params.m_sigmaDecay > 0)
			{
				if (m_variableSigma[0] > sigmaMin)
				{
					m_variableSigma.array() *= (1-m_params.m_sigmaDecay);
				}
			}

			m_progress += dProgress;

		} // Number of epochs
			
		MatrixXd diffErr = batch - vis;
		diffErr.array() *= diffErr.array();
		double err = diffErr.colwise().sum().sum();
		cout << "error (per mini batch) = " << err << endl;
	} // number of mini batches
	
	updateHiddenBatch();
	
	MatrixXd vis = m_h * m_w.weights().transpose();
	vis += m_w.visibleBias().replicate(m_batch.rows(), 1);
	probsLogistic(vis, m_variableSigma.replicate(m_batch.rows(), 1));
	MatrixXd diffErr = m_batch - vis;
	diffErr.array() *= diffErr.array();
	double err = diffErr.colwise().sum().sum();
	cout << "error (total) = " << err << endl;

	onProgressChanged();
	
}

double Rbm::getProgress() const
{
	return m_progress;
}

double Rbm::getEnergy(const VectorXd& visible, const VectorXd& hidden)
{
	double energy;
	double sigma = m_variableSigma.array().mean();

	energy = m_w.visibleBias() * visible;
	energy += m_w.hiddenBias() * hidden;
	energy += visible.transpose() * m_w.weights() * hidden;

	return -energy/(sigma*sigma);
}

void Rbm::toHidden(RowVectorXd &h, RowVectorXd const &v)
{
	h = v * m_w.weights();
	h += m_w.hiddenBias();
	probsLogistic(h);
}

void Rbm::toVisible(RowVectorXd &v, RowVectorXd const &h)
{
	v = h * m_w.weights().transpose();
	v += m_w.visibleBias();

	if (m_params.m_useVisibleGaussian)
	{
//		probsGaussian(v, m_variableSigma);
	}
	else
	{
		probsLogistic(v, m_variableSigma);
	}
}

void Rbm::setConstantSigma(double value)
{	
	 m_params.m_constantSigma = value;
	 m_variableSigma.fill(m_params.m_constantSigma);
	 onParamsChanged();
}

RowVectorXd& Rbm::getVariableSigma()
{
	 return m_variableSigma;
}

void Rbm::setSigmaDecay(double value)
{
	m_params.m_sigmaDecay = value;
	onParamsChanged();
}

void Rbm::setWeightDecay(double value)
{
	m_params.m_weightDecay = value;
	onParamsChanged();
}

void Rbm::setLambda(double value)
{
	m_params.m_lambda = value;
	onParamsChanged();
}

void Rbm::setSparsity(double value)
{
	m_params.m_sparsity = value;
	onParamsChanged();
}

void Rbm::setUseVisibleGaussian(bool flag)
{
	m_params.m_useVisibleGaussian = flag;
	onParamsChanged();
}

void Rbm::setUseHiddenGaussian(bool flag)
{
	m_params.m_useHiddenGaussian = flag;
	onParamsChanged();
}

void Rbm::setDoRaoBlackwell(bool flag)
{
	m_params.m_doRaoBlackwell = flag;
	onParamsChanged();
}

void Rbm::setDoSampleVisible(bool flag)
{
	m_params.m_doSampleVisible = flag;
	onParamsChanged();
}

void Rbm::setDoSampleBatch(bool flag)
{
	m_params.m_doSampleBatch = flag;
	onParamsChanged();
}

void Rbm::setDoSparse(bool flag)
{
	m_params.m_doSparse = flag;
	onParamsChanged();
}

void Rbm::setNormalizeData(bool flag)
{
	m_params.m_doNormalizeData = flag;
	onParamsChanged();
}

void Rbm::setDoLearnVariance(bool flag)
{
	m_params.m_doLearnVariance = flag;
	if (m_params.m_doLearnVariance && m_batch.rows())
	{
		m_variableSigma = calcSigma(m_batch);
	}
	else
	{
		 m_variableSigma.fill(m_params.m_constantSigma);
	}
	onParamsChanged();
}

void Rbm::setNumGibbs(size_t value)
{
	m_params.m_numGibbs = value;
	onParamsChanged();
}

void Rbm::setMiniBatchSize(size_t size)
{
	m_params.m_miniBatchSize = size;
}

void Rbm::setMuWeights(double value)
{
	m_params.m_muWeights = value;
	onParamsChanged();
}

void Rbm::setMuSparsity(double value)
{
	m_params.m_muSparsity = value;
	onParamsChanged();
}

void Rbm::setMomentum(double value)
{
	m_params.m_momentum = value;
	onParamsChanged();
}

MatrixXd const& Rbm::getHiddenBatch()
{
	return m_h;
}

MatrixXd const& Rbm::getBatch()
{
	return m_batch;
}

void Rbm::updateHiddenBatch()
{
	if (m_batch.rows() == 0)
	{
		return;
	}
	m_h.resize(m_batch.rows(), m_w.getNumHidden());
	m_h = m_batch * m_w.weights();
	m_h += m_w.hiddenBias().replicate(m_batch.rows(), 1);
	probsLogistic(m_h);
}

Rbm::Params const& Rbm::params()
{
	return m_params;
}