// This file is part of libigl, a simple c++ geometry processing library.
// 
// Copyright (C) 2013 Alec Jacobson <alecjacobson@gmail.com>
// 
// This Source Code Form is subject to the terms of the Mozilla Public License 
// v. 2.0. If a copy of the MPL was not distributed with this file, You can 
// obtain one at http://mozilla.org/MPL/2.0/.
#include "mosek_quadprog.h"
#include "mosek_guarded.h"
#include <cstdio>
#include "../find.h"
#include "../verbose.h"
#include "../speye.h"
#include "../matrix_to_list.h"
#include "../list_to_matrix.h"
#include "../harwell_boeing.h"
#include "../EPS.h"

// Little function mosek needs in order to know how to print to std out
static void MSKAPI printstr(void *handle, char str[])
{
	printf("%s",str);
}

igl::MosekData::MosekData()
{
	// These are the default settings that worked well for BBW. Your miles may
	// very well be kilometers.

	// >1e0 NONSOLUTION
	// 1e-1 artifacts in deformation
	// 1e-3 artifacts in isolines
	// 1e-4 seems safe
	// 1e-8 MOSEK DEFAULT SOLUTION
	douparam[MSK_DPAR_INTPNT_TOL_REL_GAP]=1e-8;
	// Force using multiple threads, not sure if MOSEK is properly destorying
	//extra threads...
#if MSK_VERSION_MAJOR >= 7
	intparam[MSK_IPAR_NUM_THREADS] = 6;
#elif MSK_VERSION_MAJOR == 6
	intparam[MSK_IPAR_INTPNT_NUM_THREADS] = 6;
#endif
#if MSK_VERSION_MAJOR == 6
	// Force turn off data check
	intparam[MSK_IPAR_DATA_CHECK]=MSK_OFF;
#endif
	// Turn off presolving
	// intparam[MSK_IPAR_PRESOLVE_USE] = MSK_PRESOLVE_MODE_OFF;
	// Force particular matrix reordering method
	// MSK_ORDER_METHOD_NONE cuts time in half roughly, since half the time is
	//   usually spent reordering the matrix
	// !! WARNING Setting this parameter to anything but MSK_ORDER_METHOD_FREE
	//   seems to have the effect of setting it to MSK_ORDER_METHOD_NONE
	//   *Or maybe Mosek is spending a bunch of time analyzing the matrix to
	//   choose the right ordering method when really any of them are
	//   instantaneous
	intparam[MSK_IPAR_INTPNT_ORDER_METHOD] = MSK_ORDER_METHOD_NONE;
	// 1.0 means optimizer is very leniant about declaring model infeasible
	douparam[MSK_DPAR_INTPNT_TOL_INFEAS] = 1e-8;
	// Hard to say if this is doing anything, probably nothing dramatic
	douparam[MSK_DPAR_INTPNT_TOL_PSAFE]= 1e2;
	// Turn off convexity check
	intparam[MSK_IPAR_CHECK_CONVEXITY] = MSK_CHECK_CONVEXITY_NONE;
}

template <typename Index, typename Scalar>
IGL_INLINE bool igl::mosek_quadprog(
	const Index n,
	std::vector<Index> & Qi,
	std::vector<Index> & Qj,
	std::vector<Scalar> & Qv,
	const std::vector<Scalar> & c,
	const Scalar cf,
	const Index m,
	std::vector<Scalar> & Av,
	std::vector<Index> & Ari,
	const std::vector<Index> & Acp,
	const std::vector<Scalar> & lc,
	const std::vector<Scalar> & uc,
	const std::vector<Scalar> & lx,
	const std::vector<Scalar> & ux,
	MosekData & mosek_data,
	std::vector<Scalar> & x)
{
	// I J V vectors of Q should all be same length
	assert(Qv.size() == Qi.size());
	assert(Qv.size() == Qj.size());
	// number of columns in linear constraint matrix must be ≤ number of
	// variables
	assert( (int)Acp.size() == (n+1));
	// linear bound vectors must be size of number of constraints or empty
	assert( ((int)lc.size() == m) || ((int)lc.size() == 0));
	assert( ((int)uc.size() == m) || ((int)uc.size() == 0));
	// constant bound vectors must be size of number of variables or empty
	assert( ((int)lx.size() == n) || ((int)lx.size() == 0));
	assert( ((int)ux.size() == n) || ((int)ux.size() == 0));

	// allocate space for solution in x
	x.resize(n);

	// variables for mosek task, env and result code
	MSKenv_t env;
	MSKtask_t task;

	// Create the MOSEK environment
#if MSK_VERSION_MAJOR >= 7
	mosek_guarded(MSK_makeenv(&env,NULL));
#elif MSK_VERSION_MAJOR == 6
	mosek_guarded(MSK_makeenv(&env,NULL,NULL,NULL,NULL));
#endif
	///* Directs the log stream to the 'printstr' function. */
	//mosek_guarded(MSK_linkfunctoenvstream(env,MSK_STREAM_LOG,NULL,printstr));
	// initialize mosek environment
	mosek_guarded(MSK_initenv(env));
	// Create the optimization task
	mosek_guarded(MSK_maketask(env,m,n,&task));
	verbose("Creating task with %ld linear constraints and %ld variables...\n",m,n);
	//// Tell mosek how to print to std out
	//mosek_guarded(MSK_linkfunctotaskstream(task,MSK_STREAM_LOG,NULL,printstr));
	// Give estimate of number of variables
	mosek_guarded(MSK_putmaxnumvar(task,n));
	if(m>0)
	{
		// Give estimate of number of constraints
		mosek_guarded(MSK_putmaxnumcon(task,m));
		// Give estimate of number of non zeros in A
		mosek_guarded(MSK_putmaxnumanz(task,Av.size()));
	}
	// Give estimate of number of non zeros in Q
	mosek_guarded(MSK_putmaxnumqnz(task,Qv.size()));
	if(m>0)
	{
		// Append 'm' empty constraints, the constrainst will initially have no
		// bounds
#if MSK_VERSION_MAJOR >= 7
		mosek_guarded(MSK_appendcons(task,m));
#elif MSK_VERSION_MAJOR == 6
		mosek_guarded(MSK_append(task,MSK_ACC_CON,m));
#endif
	}
	// Append 'n' variables
#if MSK_VERSION_MAJOR >= 7
	mosek_guarded(MSK_appendvars(task,n));
#elif MSK_VERSION_MAJOR == 6
	mosek_guarded(MSK_append(task,MSK_ACC_VAR,n));
#endif
	// add a contant term to the objective
	mosek_guarded(MSK_putcfix(task,cf));

	// loop over variables
	for(int j = 0;j<n;j++)
	{
		if(c.size() > 0)
		{
			// Set linear term c_j in the objective
			mosek_guarded(MSK_putcj(task,j,c[j]));
		}

		// Set constant bounds on variable j
		if(lx[j] == ux[j])
		{
			mosek_guarded(MSK_putbound(task,MSK_ACC_VAR,j,MSK_BK_FX,lx[j],ux[j]));
		}else
		{
			if(lx[j]!=-1e10 || ux[j]!=1e10)
				mosek_guarded(MSK_putbound(task,MSK_ACC_VAR,j,MSK_BK_RA,lx[j],ux[j]));
			else
				mosek_guarded(MSK_putbound(task,MSK_ACC_VAR,j,MSK_BK_FR,lx[j],ux[j]));
		}

		if(m>0)
		{
			// Input column j of A
#if MSK_VERSION_MAJOR >= 7
			mosek_guarded(
				MSK_putacol(
				task,
				j,
				Acp[j+1]-Acp[j],
				&Ari[Acp[j]],
				&Av[Acp[j]])
				);
#elif MSK_VERSION_MAJOR == 6
			mosek_guarded(
				MSK_putavec(
				task,
				MSK_ACC_VAR,
				j,
				Acp[j+1]-Acp[j],
				&Ari[Acp[j]],
				&Av[Acp[j]])
				);
#endif
		}
	}

	// loop over constraints
	for(int i = 0;i<m;i++)
	{
		// put bounds  on constraints
		if(uc[i]!=1e10)
			mosek_guarded(MSK_putbound(task,MSK_ACC_CON,i,MSK_BK_RA,lc[i],uc[i]));
		else
			mosek_guarded(MSK_putbound(task,MSK_ACC_CON,i,MSK_BK_LO,lc[i],uc[i]));
	}

	// Input Q for the objective (REMEMBER Q SHOULD ONLY BE LOWER TRIANGLE)
	mosek_guarded(MSK_putqobj(task,Qv.size(),&Qi[0],&Qj[0],&Qv[0]));

	// Set up task parameters
	for(
		std::map<MSKiparame,int>::iterator pit = mosek_data.intparam.begin();
		pit != mosek_data.intparam.end(); 
	pit++)
	{
		mosek_guarded(MSK_putintparam(task,pit->first,pit->second));
	}
	for(
		std::map<MSKdparame,double>::iterator pit = mosek_data.douparam.begin();
		pit != mosek_data.douparam.end(); 
	pit++)
	{
		mosek_guarded(MSK_putdouparam(task,pit->first,pit->second));
	}

	// Now the optimizer has been prepared
	MSKrescodee trmcode;
	// run the optimizer
	mosek_guarded(MSK_optimizetrm(task,&trmcode));

	//// Print a summary containing information about the solution for debugging
	//// purposes
	//MSK_solutionsummary(task,MSK_STREAM_LOG);

	// Get status of solution
	MSKsolstae solsta;
#if MSK_VERSION_MAJOR >= 7
	MSK_getsolsta (task,MSK_SOL_ITR,&solsta);
#elif MSK_VERSION_MAJOR == 6
	MSK_getsolutionstatus(task,MSK_SOL_ITR,NULL,&solsta);
#endif

	bool success = false;
	switch(solsta)
	{
	case MSK_SOL_STA_OPTIMAL:   
	case MSK_SOL_STA_NEAR_OPTIMAL:
		MSK_getsolutionslice(task,MSK_SOL_ITR,MSK_SOL_ITEM_XX,0,n,&x[0]);
		//printf("Optimal primal solution\n");
		//for(size_t j=0; j<n; ++j)
		//{
		//  printf("x[%ld]: %g\n",j,x[j]);
		//}
		success = true;
		break;
	case MSK_SOL_STA_DUAL_INFEAS_CER:
	case MSK_SOL_STA_PRIM_INFEAS_CER:
	case MSK_SOL_STA_NEAR_DUAL_INFEAS_CER:
	case MSK_SOL_STA_NEAR_PRIM_INFEAS_CER:  
		//printf("Primal or dual infeasibility certificate found.\n");
		break;
	case MSK_SOL_STA_UNKNOWN:
		//printf("The status of the solution could not be determined.\n");
		break;
	default:
		//printf("Other solution status.");
		break;
	}

	MSK_deletetask(&task);
	MSK_deleteenv(&env);

	return success;
}

IGL_INLINE bool igl::mosek_quadprog(
	const Eigen::SparseMatrix<double> & Q,
	const Eigen::VectorXd & c,
	const double cf,
	const Eigen::SparseMatrix<double> & A,
	const Eigen::VectorXd & lc,
	const Eigen::VectorXd & uc,
	const Eigen::VectorXd & lx,
	const Eigen::VectorXd & ux,
	MosekData & mosek_data,
	Eigen::VectorXd & x)
{
	using namespace Eigen;
	using namespace std;
	using namespace igl;

	// Q should be square
	assert(Q.rows() == Q.cols());
	// Q should be symmetric
#ifdef EIGEN_HAS_A_BUG_AND_FAILS_TO_LET_ME_COMPUTE_Q_MINUS_Q_TRANSPOSE
	assert( (Q-Q.transpose()).sum() < FLOAT_EPS);
#endif
	// Only keep lower triangular part of Q
	SparseMatrix<double> QL;
	//QL = Q.template triangularView<Lower>();
	QL = Q.triangularView<Lower>();
	VectorXi Qi,Qj;
	VectorXd Qv;
	find(QL,Qi,Qj,Qv);
	vector<int> vQi = matrix_to_list(Qi);
	vector<int> vQj = matrix_to_list(Qj);
	vector<double> vQv = matrix_to_list(Qv);

	// Convert linear term
	vector<double> vc = matrix_to_list(c);

	assert(lc.size() == A.rows());
	assert(uc.size() == A.rows());
	// Convert A to harwell boeing format
	vector<double> vAv;
	vector<int> vAr,vAc;
	int nr;
	harwell_boeing(A,nr,vAv,vAr,vAc);

	assert(lx.size() == Q.rows());
	assert(ux.size() == Q.rows());
	vector<double> vlc = matrix_to_list(lc);
	vector<double> vuc = matrix_to_list(uc);
	vector<double> vlx = matrix_to_list(lx);
	vector<double> vux = matrix_to_list(ux);

	vector<double> vx;
	bool ret = mosek_quadprog(
		Q.rows(),vQi,vQj,vQv,
		vc,
		cf,
		nr,
		vAv, vAr, vAc,
		vlc,vuc,
		vlx,vux,
		mosek_data,
		vx);
	list_to_matrix(vx,x);
	return ret;
}
#ifdef IGL_STATIC_LIBRARY
// Explicit template declarations
#endif