#include "precompile.h"
#include "matlab_debugger.h"
#include "timer_wrapper.h"

#ifdef ENABLE_MATLAB_DEBUGGING
MatlabDebugger::MatlabDebugger() :
	m_engine(NULL)
{
	m_enable = false;
	m_plot_type = PLOT_TYPE_SEMILOG_Y;
}

MatlabDebugger::~MatlabDebugger()
{
	if (m_engine != NULL)
	{
		engClose(m_engine);
	}
}

void MatlabDebugger::Init()
{
	m_enable = true;

	m_engine = engOpen(NULL);
	if (m_engine == NULL)
	{
		fprintf(stderr, "ERROR: Can not initialize matlab engine.\n");
		exit(EXIT_FAILURE);
	}

	m_double1x1_array = mxCreateDoubleMatrix(1,1,mxREAL);
	m_double1x1 = mxGetPr(m_double1x1_array);

	m_plot_x_axis_plot_iterations = true;

	Reset();
}

void MatlabDebugger::ResetCurrent()
{
	if (!m_enable)
		return;

	engEvalString(m_engine, "X = []");
	engEvalString(m_engine, "it = []");
	engEvalString(m_engine, "time = []");
	engEvalString(m_engine, "diff = []");
}

void MatlabDebugger::Reset()
{
	if (!m_enable)
		return;

	engEvalString(m_engine, "close");
	engEvalString(m_engine, "clear");
	engEvalString(m_engine, "X = []");
	engEvalString(m_engine, "it = []");
	engEvalString(m_engine, "time = []");
	engEvalString(m_engine, "diff = []");
	engEvalString(m_engine, "Hessian = cell(0)");
	engEvalString(m_engine, "Gradient = cell(0)");
	//engEvalString(m_engine, "addpath('C:/Libs/SuiteSparse/CHOLMOD/MATLAB');");

	engEvalString(m_engine, "dist_cell = cell(1, 5);");
	engEvalString(m_engine, "it_cell = cell(1, 5);");
	engEvalString(m_engine, "time_cell = cell(1, 5);");
	engEvalString(m_engine, "lgd_cell = cell(1, 5);");
	
	m_legend_string[0] = '\0';
	m_plot_index = 0;
	m_plot_style[0] = std::string("'-r*'");
	m_plot_style[1] = std::string("'-m*'");
	m_plot_style[2] = std::string("'-b*'");
	m_plot_style[3] = std::string("'-c*'");
	m_plot_style[4] = std::string("'-g*'");
}

void MatlabDebugger::Export()
{
	engEvalString(m_engine, "save('D:/Projects/Noninversion Skinning/noninversion_skinning_cleaned/noninversion_skinning/matlab_code/data.mat')");
}

void MatlabDebugger::SaveCurrentData(const char* filename)
{
	if (!m_enable)
		return;

	//std::string data_path_file = m_gs->m_exe_path+std::string("/matlab/")+std::string(filename);
	//std::string command = std::string("save('")+data_path_file+std::string("');");

	//engEvalString(m_engine, command.c_str());
}

void MatlabDebugger::SaveCurrentFigure(const char* filename)
{
	if (!m_enable)
		return;

	//std::string figure_path_file = m_gs->m_exe_path+std::string("/matlab/")+std::string(filename);
	//std::string command = std::string("saveas(handle, '")+figure_path_file+std::string("', 'fig');");

	//char handle_command[255];
	//sprintf(handle_command, "handle = figure(%d);", m_gs->m_choose_figure_to_save);
	//engEvalString(m_engine,handle_command);
	//engEvalString(m_engine, command.c_str());
}

void MatlabDebugger::SendVector(VectorX &x, char* name, int size)
{
	if (!m_enable)
		return;

	m_vector_array = mxCreateDoubleMatrix(size,1,mxREAL);
	m_vector = mxGetPr(m_vector_array);	

	char command[200];
	sprintf(command, "%s = []", name);

	engEvalString(m_engine, command);

	for (int i = 0; i < size; i++)
	{
		m_vector[i] = x(i);
	}

	engPutVariable(m_engine, name, m_vector_array);

	mxDestroyArray(m_vector_array);
}

void MatlabDebugger::SendSparseMatrixUsingTriplets(std::vector<SparseMatrixTriplet> &triplets, char* name, int d1, int d2)
{
	if (!m_enable)
		return;

	int size = triplets.size();

	m_triplet_array = mxCreateDoubleMatrix(size,3,mxREAL);
	m_triplet = mxGetPr(m_triplet_array);	
	
	engEvalString(m_engine, "triplets = [];");

	int i = 0;
	for (std::vector<SparseMatrixTriplet>::iterator t = triplets.begin(); t != triplets.end(); ++t)
	{
		m_triplet[i+0] = t->row()+1; //matlab starts index with 1
		m_triplet[i+1*size] = t->col()+1; //matlab starts index with 1
		m_triplet[i+2*size] = t->value();
		i ++;
	}

	engPutVariable(m_engine,"triplets",m_triplet_array);
	char command[255]; 
	sprintf(command, "%s = sparse(triplets(:,1), triplets(:,2), triplets(:,3), %d, %d);", name, d1, d2);
	engEvalString(m_engine, command);

	mxDestroyArray(m_triplet_array);
}

void MatlabDebugger::SendPositionIteration(VectorX &x, int size, double iteration)
{
	if (!m_enable)
		return;

	m_vector_array = mxCreateDoubleMatrix(size,1,mxREAL);
	m_vector = mxGetPr(m_vector_array);	

	char* name = "x";
	char command[200];
	sprintf(command, "%s = []", name);

	engEvalString(m_engine, command);

	for (int i = 0; i < size; i++)
	{
		m_vector[i] = x(i);
	}

	engPutVariable(m_engine, name, m_vector_array);

	engEvalString(m_engine, "X = [X x];");

	std::string iteration_name;
	iteration_name = std::string("it");
	m_double1x1[0] = iteration;
	engPutVariable(m_engine,"temp",m_double1x1_array);
	std::string iteration_commond;
	iteration_commond = iteration_name + std::string(" = [") + iteration_name + std::string(" temp];");
	engEvalString(m_engine, iteration_commond.c_str());	
}

void MatlabDebugger::SendPositionIterationTime(VectorX &x, int size, double iteration, double time)
{
	if (!m_enable)
		return;

	m_vector_array = mxCreateDoubleMatrix(size,1,mxREAL);
	m_vector = mxGetPr(m_vector_array);	

	char* name = "x";
	char command[200];
	sprintf(command, "%s = []", name);

	engEvalString(m_engine, command);

	for (int i = 0; i < size; i++)
	{
		m_vector[i] = x(i);
	}

	engPutVariable(m_engine, name, m_vector_array);

	engEvalString(m_engine, "X = [X x];");

	// iteration
	std::string iteration_name;
	iteration_name = std::string("it");
	m_double1x1[0] = iteration;
	engPutVariable(m_engine,"temp",m_double1x1_array);
	std::string iteration_commond;
	iteration_commond = iteration_name + std::string(" = [") + iteration_name + std::string(" temp];");
	engEvalString(m_engine, iteration_commond.c_str());	


	// time
	std::string time_name;
	time_name = std::string("time");
	m_double1x1[0] = time;
	engPutVariable(m_engine,"temp",m_double1x1_array);
	std::string time_commond;
	time_commond = time_name + std::string(" = [") + time_name + std::string(" temp];");
	engEvalString(m_engine, time_commond.c_str());	
}

void MatlabDebugger::SendEnergyIteration(double energy, double iteration)
{
	if (!m_enable)
		return;

	std::string energy_name;
	std::string iteration_name;
	energy_name = std::string("energy");
	iteration_name = std::string("it");
		
	std::string energy_commond;
	std::string iteration_commond;
	
	m_double1x1[0] = energy;
	engPutVariable(m_engine,"temp",m_double1x1_array);
	energy_commond = energy_name + std::string(" = [") + energy_name + std::string(" temp];");
	engEvalString(m_engine, energy_commond.c_str());	

	m_double1x1[0] = iteration;
	engPutVariable(m_engine,"temp",m_double1x1_array);
	iteration_commond = iteration_name + std::string(" = [") + iteration_name + std::string(" temp];");
	engEvalString(m_engine, iteration_commond.c_str());	
}

void MatlabDebugger::SetLastMatrixToLog(char* name, double iteration)
{
	if (iteration > 0)
	{
		char command[200];
		sprintf(command, "Hessian{%lf} = %s;", iteration, name);

		engEvalString(m_engine, command);
	}
}

void MatlabDebugger::SetLastVectorToLog(char* name, double iteration)
{
	if (iteration > 0)
	{
		char command[200];
		sprintf(command, "Gradient{%lf} = %s;", iteration, name);

		engEvalString(m_engine, command);
	}	
}

void MatlabDebugger::AddNewPlotData()
{
	if (m_legend_string[0] == '\0')
	{
		std::cout << "Please specify the data name." << std::endl;
		return;
	}
	if (m_plot_index >= 5)
	{
		std::cout << "Plot full." << std::endl;
		return;
	}

	char command[200];
	sprintf(command, "plot_index = %d;", m_plot_index+1);
	engEvalString(m_engine, command);

	engEvalString(m_engine, "diff =  X - repmat(X(:, it(end)), 1, size(it, 2));");
	engEvalString(m_engine, "dist = sqrt(sum(diff.^2));");

	engEvalString(m_engine, "dist_cell{plot_index} = dist");
	engEvalString(m_engine, "it_cell{plot_index} = it");
	engEvalString(m_engine, "time_cell{plot_index} = time");
	sprintf(command, "lgd_cell{plot_index} = '%s';", m_legend_string);
	engEvalString(m_engine, command);

	m_legend_string[0] = '\0';
	m_plot_index ++;
}

void MatlabDebugger::PlotDistToSolution()
{
	engEvalString(m_engine, "diff =  X - repmat(X(:, it(end)), 1, size(it, 2));");
	engEvalString(m_engine, "dist = sqrt(sum(diff.^2));");

	std::string x_axis_var;
	if (m_plot_x_axis_plot_iterations)
	{
		x_axis_var = std::string("it(1:end-1)");
	}
	else
	{
		x_axis_var = std::string("time(1:end-1)");
	}
	std::string y_axis_var = std::string("dist(1:end-1)");
	std::string line_parameter = std::string("'-b*'");

	std::string plot_type;
	switch(m_plot_type)
	{
	case PLOT_TYPE_PLOT:
		plot_type = std::string("plot(");
		break;
	case PLOT_TYPE_SEMILOG_X:
		plot_type = std::string("semilogx(");
		break;
	case PLOT_TYPE_SEMILOG_Y:
		plot_type = std::string("semilogy(");
		break;
	case PLOT_TYPE_LOGLOG:
		plot_type = std::string("loglog(");
		break;
	}

	std::string plot_command = plot_type\
		+ x_axis_var\
		+ std::string(", ")\
		+ y_axis_var\
		+ std::string(", ")\
		+ line_parameter\
		+ std::string(");");

	engEvalString(m_engine, plot_command.c_str());
}

void MatlabDebugger::Plot()
{


	std::string x_axis_var = std::string("it");
	std::string y_axis_var = std::string("energy");
	std::string line_parameter = std::string("'-b'");

	std::string plot_type;
	switch(m_plot_type)
	{
	case PLOT_TYPE_PLOT:
		plot_type = std::string("plot(");
		break;
	case PLOT_TYPE_SEMILOG_X:
		plot_type = std::string("semilogx(");
		break;
	case PLOT_TYPE_SEMILOG_Y:
		plot_type = std::string("semilogy(");
		break;
	case PLOT_TYPE_LOGLOG:
		plot_type = std::string("loglog(");
		break;
	}

	std::string plot_command = plot_type\
								+ x_axis_var\
								+ std::string(", ")\
								+ y_axis_var\
								+ std::string(", ")\
								+ line_parameter\
								+ std::string(");");

	engEvalString(m_engine, plot_command.c_str());
}

void MatlabDebugger::PlotAll()
{
	std::string plot_type;
	switch(m_plot_type)
	{
	case PLOT_TYPE_PLOT:
		plot_type = std::string("plot(");
		break;
	case PLOT_TYPE_SEMILOG_X:
		plot_type = std::string("semilogx(");
		break;
	case PLOT_TYPE_SEMILOG_Y:
		plot_type = std::string("semilogy(");
		break;
	case PLOT_TYPE_LOGLOG:
		plot_type = std::string("loglog(");
		break;
	}

	std::string lgd_command = std::string("legend(");
	char lgd[200];
	for (unsigned int i = 0; i != m_plot_index; i++)
	{
		char command[200];
		sprintf(command, "plot_index = %d;", i+1);
		engEvalString(m_engine, command);

		std::string x_axis_var;
		if (m_plot_x_axis_plot_iterations)
		{
			x_axis_var = std::string("it_cell{plot_index}(1:end-1)");
		}
		else
		{
			x_axis_var = std::string("time_cell{plot_index}(1:end-1)");
		}
		std::string y_axis_var = std::string("dist_cell{plot_index}(1:end-1)");
		std::string line_parameter = m_plot_style[i];
		sprintf(lgd, "lgd_cell{%d}", i+1);
		lgd_command = lgd_command + std::string(lgd);
		if (i < m_plot_index - 1)
		{
			lgd_command = lgd_command + std::string(", ");
		}

		std::string plot_command = plot_type\
			+ x_axis_var\
			+ std::string(", ")\
			+ y_axis_var\
			+ std::string(", ")\
			+ line_parameter\
			+ std::string(");");

		engEvalString(m_engine, plot_command.c_str());

		if (i == 0)
		{
			engEvalString(m_engine, "hold on;");
		}
	}
	engEvalString(m_engine, "hold off;");

	// plot legend
	lgd_command = lgd_command + std::string(");");
	engEvalString(m_engine, lgd_command.c_str());

	// plot title/xlabel/ylabel
	engEvalString(m_engine, "title('Convergence Rate');");
	if (m_plot_x_axis_plot_iterations)
	{
		engEvalString(m_engine, "xlabel('number of iterations');");
	}
	else
	{
		engEvalString(m_engine, "xlabel('time (seconds)');");
	}
	engEvalString(m_engine, "ylabel('||x-x*||');");

}

void MatlabDebugger::PlotResidue()
{
	std::string x_axis_var = std::string("it");
	std::string y_axis_var = std::string("energy-energy(end)");
	std::string line_parameter = std::string("'-b'");

	std::string plot_type;
	switch(m_plot_type)
	{
	case PLOT_TYPE_PLOT:
		plot_type = std::string("plot(");
		break;
	case PLOT_TYPE_SEMILOG_X:
		plot_type = std::string("semilogx(");
		break;
	case PLOT_TYPE_SEMILOG_Y:
		plot_type = std::string("semilogy(");
		break;
	case PLOT_TYPE_LOGLOG:
		plot_type = std::string("loglog(");
		break;
	}

	std::string plot_command = plot_type\
								+ x_axis_var\
								+ std::string(", ")\
								+ y_axis_var\
								+ std::string(", ")\
								+ line_parameter\
								+ std::string(");");

	engEvalString(m_engine, plot_command.c_str());
}

void MatlabDebugger::MatlabSolveLinearSystem(VectorX& x, char* vector_name, char* matrix_name)
{
	char command[255];
	sprintf(command, "x = %s\\%s", matrix_name, vector_name);

	engEvalString(m_engine, command);

	mxArray *res = NULL;
	ScalarType *data = NULL;
	res = engGetVariable(m_engine, "x");
	assert(res != NULL);
	data = mxGetPr(res);
	memcpy((char *) x.data(), (char *) data, x.size()*sizeof(double));

	mxDestroyArray(res);	
}

// matlab sdp
void MatlabDebugger::MatlabResetSDPMosek()
{
	if (!m_enable)
		return;

	m_mosek_number_of_iterations = 1;
	m_yalmip_initialzied = false;
}

void MatlabDebugger::MatlabInitSDPMosekYalmip()
{
	if (!m_enable)
		return;

	engEvalString(m_engine, "addpath('D:/Projects/noninversion_skinning_for_testing/noninversion_skinning/matlab_code');");
	engEvalString(m_engine, "mosekSDPYalmipInitialize");
}

void MatlabDebugger::MatlabSolveSDPMosekYalmip(int dim, VectorX& x, std::vector<SparseMatrixTriplet>& g_triplets, unsigned int g_dim1, unsigned int g_dim2, VectorX& vectorized_ExR, VectorX& consVIndices, VectorX& exV, ScalarType k, bool has_max_it)
{
	if (!m_enable)
		return;

	SendSparseMatrixUsingTriplets(g_triplets, "G", g_dim1, g_dim2);
	SendVector(vectorized_ExR, "exR", vectorized_ExR.size());
	SendVector(consVIndices, "consVIndices", consVIndices.size());
	SendVector(exV, "exV", exV.size());
	char command[255];
	if (has_max_it)
	{
		sprintf(command, "dim = %d, k = %lf; num_of_it = %d;", dim, k, m_mosek_number_of_iterations);
		std::cout << "Max Number of Mosek iterations: " << m_mosek_number_of_iterations << std::endl;
		m_mosek_number_of_iterations++;
	}
	else
	{
		sprintf(command, "dim = %d, k = %lf; num_of_it = 100;", dim, k);
		std::cout << "Max Number of Mosek iterations: unlimited" << std::endl;
		m_yalmip_initialzied = false;
	}
	engEvalString(m_engine, command);

	//engEvalString(m_engine, "optiX = mosekSDPYalmip(G, exR, consVIndices, exV, k, num_of_it);");
	TimerWrapper t_yalmip, t_mosek;
	if (!m_yalmip_initialzied)
	{
		t_yalmip.Tic();
		engEvalString(m_engine, "[c,objective,v] = mosekSDPYalmipPreparation(dim, G, exR, consVIndices, exV, k);");
		t_yalmip.Toc();
		t_yalmip.Report("Yalmip");
		m_yalmip_initialzied = true;
	}
	t_mosek.Tic();
	engEvalString(m_engine, "optiX = mosekSDPYalmipMosek( c,objective,v, num_of_it );");
	t_mosek.Toc();
	t_mosek.Report("Mosek SDP");

	mxArray *res = NULL;
	ScalarType *data = NULL;
	res = engGetVariable(m_engine, "optiX");
	assert(res != NULL);
	data = mxGetPr(res);
	memcpy((char *) x.data(), (char *) data, g_dim2*sizeof(double));

	mxDestroyArray(res);
}

void MatlabDebugger::MatlabInitSDPMosekEpigraph()
{
	if (!m_enable)
		return;

	engEvalString(m_engine, "addpath('D:/Projects/Noninversion Skinning/noninversion_skinning_cleaned/noninversion_skinning/matlab_code');");
	engEvalString(m_engine, "MosekPapth = 'C:/Program Files/Mosek/7/toolbox/r2012aom';");
	engEvalString(m_engine, "addpath(genpath(MosekPapth));");
}

void MatlabDebugger::MatlabSolveSDPMosekEpigraph(VectorX& x, std::vector<SparseMatrixTriplet>& g_triplets, unsigned int g_dim1, unsigned int g_dim2, VectorX& vectorized_ExR, VectorX& consVIndices, VectorX& exV, ScalarType k)
{
	if (!m_enable)
		return;

	SendSparseMatrixUsingTriplets(g_triplets, "G", g_dim1, g_dim2);
	SendVector(vectorized_ExR, "exR", vectorized_ExR.size());
	SendVector(consVIndices, "consVIndices", consVIndices.size());
	SendVector(exV, "exV", exV.size());
	char command[255];
	sprintf(command, "k = %lf; num_of_it = %d;", k, m_mosek_number_of_iterations);
	engEvalString(m_engine, command);

	engEvalString(m_engine, "optiX = mosekSDPEpigraph(G, exR, consVIndices, exV, k, num_of_it);");

	mxArray *res = NULL;
	ScalarType *data = NULL;
	res = engGetVariable(m_engine, "optiX");
	assert(res != NULL);
	data = mxGetPr(res);
	memcpy((char *) x.data(), (char *) data, g_dim2*sizeof(double));

	mxDestroyArray(res);

	m_mosek_number_of_iterations++;
}

void MatlabDebugger::MatlabSetConvergedSolution(VectorX& x_star)
{
	m_x_star = x_star;

	SendVector(x_star, "x_star", x_star.size());
}

#endif