//////////////////////////////////////////////////////////////////////
//
//	CostFunction_ndim_2ndOrder.cpp: implementation of a test
//	CostFunction class.
//
//	Written By: Matthias Wacker
//
//////////////////////////////////////////////////////////////////////


#include "CostFunction_ndim_2ndOrder.h"

using namespace OPTIMIZATION;

CostFunction_ndim_2ndOrder::CostFunction_ndim_2ndOrder() : SuperClass()
{
}


CostFunction_ndim_2ndOrder::CostFunction_ndim_2ndOrder(unsigned int dim)  : SuperClass(dim)
{
  m_hasAnalyticGradient = true;
  m_hasAnalyticHessian = false; // not implemented

  m_A  = matrix_type(dim,dim);
  m_bt = matrix_type(1,dim);
  m_c	 = 0.0;

}

CostFunction_ndim_2ndOrder::CostFunction_ndim_2ndOrder(const CostFunction_ndim_2ndOrder &func):SuperClass(func)
{
  m_A = func.m_A;
  m_bt = func.m_bt;
  m_c = func.m_c;
}

CostFunction_ndim_2ndOrder & CostFunction_ndim_2ndOrder::operator=(const CostFunction_ndim_2ndOrder & func)
{
  /*
      avoid self-copy
  */
  if(this != &func)
  {
    
    /*
      =operator of SuperClass
    */
    SuperClass::operator=(func);

    /*
      own values:
    */
    m_A		= func.m_A;
    m_bt	= func.m_bt;
    m_c		= func.m_c;
  }

    return *this;
}

CostFunction_ndim_2ndOrder::~CostFunction_ndim_2ndOrder()
{
}


void CostFunction_ndim_2ndOrder::init()
{
}

int CostFunction_ndim_2ndOrder::setAbc(const matrix_type &A,const matrix_type &b, double c)
{
  if(A.rows() == static_cast<int>(m_numOfParameters) && A.cols() == static_cast<int>(m_numOfParameters) &&
    b.rows() == static_cast<int>(m_numOfParameters) && b.cols() == 1)
  {

    m_A = A;
    m_bt = b.transpose();
    m_c = c;

    return 0;
  }
  else
  {
    return -1;
  }
}


double CostFunction_ndim_2ndOrder::evaluate(const matrix_type &parameter)
{
  
  //return ( !parameter * 0.5 * m_A * parameter + m_bt * parameter + m_c)(0,0);
  return ( parameter.transpose() * 0.5 * m_A * parameter + m_bt * parameter)(0,0) + m_c;
  
}

const matrix_type CostFunction_ndim_2ndOrder::getAnalyticGradient(const matrix_type &parameter)
{
  matrix_type tmp = parameter * 0;

  tmp = m_A * parameter + m_bt.transpose();

  return tmp;
}