//////////////////////////////////////////////////////////////////////
//
//	LineSearcher.cpp: Implementation of the LineSearcher class.
//
//	Written by Matthias Wacker
//
//////////////////////////////////////////////////////////////////////

#include "optimization/LineSearcher.h"
#include <iostream>

using namespace optimization;

LineSearcher::LineSearcher()
{
	m_maximize = false;
	m_pEq = NULL;
	m_numEq = 0;
	m_pIneq = NULL;
	m_numIneq = 0;
	m_PP = 1000.0;
	m_verbose= false;
}

LineSearcher::LineSearcher(CostFunction *costFunc,OptLogBase* loger) : m_pFunc(costFunc), m_pLoger(loger)
{
	m_maximize = false;
	m_pEq = NULL;
	m_numEq = 0;
	m_pIneq = NULL;
	m_numIneq = 0;
	m_PP = 10.0;
	m_verbose= false;
}

LineSearcher::LineSearcher(const LineSearcher &lin)
{
	m_pFunc		= lin.m_pFunc;
	m_pLoger	= lin.m_pLoger;
	m_maximize	= lin.m_maximize;
	m_pEq		= lin.m_pEq;
	m_numEq		= lin.m_numEq;
	m_pIneq		= lin.m_pIneq;
	m_numIneq	= lin.m_numIneq;
	m_PP		= lin.m_PP;
	m_verbose	= lin.m_verbose;
}

LineSearcher & LineSearcher::operator =(const LineSearcher &lin)
{
	/*
			avoid self-copy
	*/
	if(this != &lin)
	{
		/*
			=operator of SuperClass
		*/
		//SuperClass::operator=(opt);
		
		m_pFunc = lin.m_pFunc;
		m_pLoger = lin.m_pLoger;
		m_maximize = lin.m_maximize;
		m_pEq		= lin.m_pEq;
		m_numEq		= lin.m_numEq;
		m_pIneq		= lin.m_pIneq;
		m_numIneq	= lin.m_numIneq;
		m_PP		= lin.m_PP;
		m_verbose	= lin.m_verbose;

	}

	return *this;

}

LineSearcher::~LineSearcher()
{
}


bool LineSearcher::setH0(const matrix_type &H0)
{
	bool tmp = true;
	if(m_pFunc)
	{
		if(m_pFunc->setH0(H0) == false)
		{
			tmp = false;
		}
	}
	if(m_pEq)
	{
		if(m_pEq->setH0(H0) == false)
		{
			tmp = false;
		}
	}
	if(m_pIneq)
	{
		if(m_pIneq->setH0(H0) == false)
		{
			tmp = false;
		}
	}

	return tmp;


}

bool LineSearcher::setX0(const matrix_type &x0)
{
	bool tmp = true;
	if(m_pFunc)
	{
		if(m_pFunc->setX0(x0) == false)
		{
			tmp = false;
		}
	}
	if(m_pEq)
	{
		if(m_pEq->setX0(x0) == false)
		{
			tmp = false;
		}
	}
	if(m_pIneq)
	{
		if(m_pIneq->setX0(x0) == false)
		{
			tmp = false;
		}
	}

	return tmp;
	
}

bool LineSearcher::setConstraints(InequalityConstraints *ineq,EqualityConstraints *eq)
{
	
	if(ineq)
	{
		if(ineq->getNumOfParameters() != m_pFunc->getNumOfParameters() )
		{
			return false;
		}
		m_numIneq = ineq->getNumOfConstraints();
		m_pIneq = ineq;
	}
	else
	{
		m_numIneq = 0;
	}

	if(eq)
	{
		if(eq->getNumOfParameters() != m_pFunc->getNumOfParameters() )
		{
			return false;
		}
		m_numEq = eq->getNumOfConstraints();
		m_pEq = eq;
	}
	else
	{
		m_numEq = 0;
	}


	return true;

}

bool LineSearcher::setPP(double pp)
{ 
	if( pp> 0.0)
	{
		m_PP = pp;
		return true;
	}
	else
	{
		return false;
	}
}

double LineSearcher::evaluateSub(double lambda)
{
	double val = 0;

	val = (m_maximize == true ? (-1.0 * m_pFunc->evaluateSub(lambda)): (m_pFunc->evaluateSub(lambda)));
	if(m_numEq)
	{
		matrix_type tmpval = m_pEq->evaluateSub(lambda);
		for(int j = 0; j<static_cast<int>(m_numEq);j++)
		{
			val += m_PP * (m_maximize == true ? (-1.0 * (1+(tmpval[j][0]* tmpval[j][0]))) : (1+ (tmpval[j][0]* tmpval[j][0])));
		}
	}
	if(m_numIneq)
	{
		matrix_type tmpval = m_pIneq->evaluateSub(lambda);
		for(int j = 0; j<static_cast<int>(m_numEq);j++)
		{
			val += m_PP * (m_maximize == true ? (-1.0 * (tmpval[j][0] > 0.0 ?(1 + tmpval[j][0] * tmpval[j][0]) : 0.0))
											  :         (tmpval[j][0]) > 0.0 ?(1+ tmpval[j][0] * tmpval[j][0]) : 0.0);
		}
	}

	return val;
}