#include "OptTestSuite.h"
#include "Opt_Namespace.h"
//
//
//

#include "core/optimization/blackbox/DownhillSimplexOptimizer.h"
#include "GradientDescentOptimizer.h"
#include "BFGSOptimizer.h"
#include "AdaptiveDirectionRandomSearchOptimizer.h"
#include "CombinatorialOptimizer.h"
#include "AdditionalIceUtils.h"
#include "CostFunction_ndim_2ndOrder.h"

using namespace OPTIMIZATION;

#include <cstdlib> // rand
//#include <cmath> 
#include <time.h> // time for srand
double getRand()
{
	return static_cast<double>(rand())/static_cast<double>(RAND_MAX);
}

void OptTestSuite::runAllTests()
{
	// seed for test
	srand ( time(NULL) );

	std::cout << "beginning runAllTests()"<< std::endl;

	// run simple opt test grid
	runSimpleOptTestGrid();	

	// any other test: to be continued ...

}


void OptTestSuite::runSimpleOptTestGrid()
{
	std::cout << "beginning runSimpleOptTestGrid" << std::endl;
	std::vector<CostFunction*> cfns;
	std::vector<Optimizer*> opts;

	SimpleOptTestGrid testGrid;
	
	// insert optimizers
	insertOptsInGrid(testGrid,opts);

	// insert optprobs
	insertOptProbsInGrid(testGrid,cfns);
	
	// run
	std::cout << "running tests"<< std::endl;
	testGrid.test();
	std::cout << "finisshed tests"<< std::endl;

	// delete the optimizers and costfunctions!
	for (int i =0; i < static_cast<int>(opts.size());++i)
	{
		//cout << "deleting" << i << endl;
		delete opts[i];
	}
	opts.clear();
	for (int i =0; i < static_cast<int>(cfns.size());++i)
	{
		//cout << "deleting" << i << endl;
		delete cfns[i];
	}
	cfns.clear();

	std::cout << "finished runSimpleOptTestGrid" << std::endl;
}




void OptTestSuite::insertOptsInGrid(SimpleOptTestGrid &testGrid, std::vector<Optimizer*> &opts )
{
	std::cout << "insert opts" << std::endl;

	// 0
	// DownhillSimplexOptimizer
	DownhillSimplexOptimizer *dho= new DownhillSimplexOptimizer();
	dho->setMaxNumIter(true, 5000); // 0-order info
	//dho->setParamTol(true, 1.0e-8);
	testGrid.addSimpleOptimizer(dho);
	opts.push_back(dho);

	// 1
	// GradientDescentOptimizer
	GradientDescentOptimizer *gdo= new GradientDescentOptimizer();
	gdo->setMaxNumIter(true, 5000); // 1st-order info
	gdo->setParamTol(true, 1.0e-8);
	//gdo->setVerbose(true);
	testGrid.addSimpleOptimizer(gdo);
	opts.push_back(gdo);

	// 2 
	// BFGSOptimizer
	BFGSOptimizer *bfgso = new BFGSOptimizer();
	bfgso->setMaxNumIter(true,1000); // 1+ order: approximating 2nd order info
	bfgso->setParamTol(true, 1.0e-8);
	//bfgso->setLineSearchOptions(0.0,1.0,1.0e-8);
	//bfgso->setVerbose(true);
	testGrid.addSimpleOptimizer(bfgso);
	opts.push_back(bfgso);


	// 3
	//AdaptiveDirectionRandomSearchOptimizer
	AdaptiveDirectionRandomSearchOptimizer *adrso = new AdaptiveDirectionRandomSearchOptimizer(5); // 0-order random info
	adrso->setMaxNumIter(true,50000);
	testGrid.addSimpleOptimizer(adrso);
	opts.push_back(adrso);

	// 4
	// CombinatorialOptimizer
	CombinatorialOptimizer *co = new CombinatorialOptimizer();
	co->setMaxNumIter(true,250000);
	testGrid.addSimpleOptimizer(co);
	opts.push_back(co);



}


void OptTestSuite::insertOptProbsInGrid(SimpleOptTestGrid &testGrid, std::vector<CostFunction*> &cfns)
{
	std::cout << "insert opt probs" << std::endl;

	int dim=0;

	// 0
	dim=1;
	CostFunction_ndim_2ndOrder *secOrder1 = new CostFunction_ndim_2ndOrder(dim);

	cfns.push_back(secOrder1);
	matrix_type A(dim,dim);	A(0,0)= 3;
	matrix_type b(dim,1); b(0,0)=1;
	double c=14.378; secOrder1->setAbc(A,b,c);
	
	matrix_type secOrder1_solution(dim,1); secOrder1_solution(0,0)=-2.0/6.0; double secOrder1_succdist=1.0e-3;

	matrix_type secOrder1_inits(dim,1); secOrder1_inits(0,0)=-15.789;
	matrix_type secOrder1_scales(dim,1); secOrder1_scales(0,0)=1.0;
	SimpleOptProblem secOrderProb1(secOrder1,secOrder1_inits,secOrder1_scales);
	testGrid.addSimpleOptProblem( secOrderProb1, secOrder1_solution, secOrder1_succdist);


	// 1
	dim=15;
	CostFunction_ndim_2ndOrder *secOrder2 = new CostFunction_ndim_2ndOrder(dim);
	cfns.push_back(secOrder2);
	matrix_type A2(dim,dim);
	matrix_type b2(dim,1);
	matrix_type secOrder2_inits(dim,1);
	matrix_type secOrder2_scales(dim,1); 
	for(int i=0; i < dim;++i)
	{
		for(int j=i;j< dim;++j)
		{
			if(i == j)
			{
				A2(i,j)=dim+getRand()*200;
			}
			else
			{
				A2(i,j)=getRand();
				A2(j,i)=A2(i,j);
			}
		}
		b2(i,0)=(getRand()-0.5) *dim ;
		secOrder2_inits(i,0)=(getRand()-0.5) *dim;
		secOrder2_scales(i,0)=1.0;
	}
	//A2 = A2 * (!A2);
	double c2=1.23546; secOrder2->setAbc(A2,b2,c2);

	matrix_type secOrder2_solution(dim,1);
	matrix_type mb2=b2;
  mb2*=-1.0;
	linSolve(A2, secOrder2_solution, mb2);
	double secOrder2_succdist=1.0e-3;
	// test if matrix is spd
	matrix_type L;
	matrix_type V;
	getEig(A2, L, V);

	//cout << "Eigenvals: " << L << endl;
	//cout << "A2 " << endl << A2 << endl;
	//cout << "b2 " << endl << b2 << endl;
	//cout << "init " << endl << secOrder2_inits << endl;
	//cout << "sol " << endl << secOrder2_solution << endl;

	SimpleOptProblem secOrderProb2(secOrder2,secOrder2_inits,secOrder2_scales);
	testGrid.addSimpleOptProblem( secOrderProb2, secOrder2_solution, secOrder2_succdist);

}