/*
 * Prg_Simple.C, was Prg_Cute.C once
 *
 */

/*
    Copyright (C) 1999  Ruediger Franke
    Copyright (C) 2003 Johan Borg

    This library is free software; you can redistribute it and/or
    modify it under the terms of the GNU General Public
    License as published by the Free Software Foundation; 
    version 2 of the License.

    This library is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
    General Public License for more details.

    You should have received a copy of the GNU General Public
    License along with this library (file COPYING.LIB);
    if not, write to the Free Software Foundation, Inc.,
    59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */

#include "Prg_Simple.h"
#include "Hqp_Program.h"
#include "Hqp_SqpSolver.h"
#include "Hqp_SqpPowell.h"
#include "Hqp_Solver.h"
#include <signal.h>


#include <string.h>


//-------------------------------------------------------------------------
Prg_Simple::Prg_Simple(Sqp_Config *conf) : Hqp_SqpProgram(conf)
{
 _n=conf->n;
 _m=conf->m;
 _me=conf->me;
 _mb=conf->mb;
 _update=conf->prog;
 _icon=conf->icon;
 _econ=conf->econ;
 _bnd=conf->bnd;
 _my_x=conf->x;
}

//-------------------------------------------------------------------------
Prg_Simple::~Prg_Simple()
{
//  iv_free(_var_solver_idxs);
//  iv_free(_var_master_idxs);
//  v_free(_derivatives);
//  iv_free(_var_asc2hqp);
//  v_free(_var_ub);
//  v_free(_var_lb);
}






//-------------------------------------------------------------------------
void Prg_Simple::setup()
{
 int a,b;
 _qp->resize(_n, _me, _m+_mb);
 _x = v_resize(_x, _n);

 init_x();
 

 _update(&_f,_qp->c->ve,_qp->b->ve,_qp->d->ve,_x->ve,2);
 

 for(a=0;a<_mb;a++) 
  if (_bnd[a].type) sp_set_val(_qp->C, a, _bnd[a].ind, 1.0); 
       else sp_set_val(_qp->C, a, _bnd[a].ind, -1.0);

 //generic inequality constraints
 for(a=0;a<_m;a++) 
  for(b=0;_icon[a].ind[b]>=0;b++) 
   sp_set_val(_qp->C, a+_mb, _icon[a].ind[b], _icon[a].der[b]);

 //equality constraints
 for(a=0;a<_me;a++) 
  for(b=0;_econ[a].ind[b]>=0;b++) {
   sp_set_val(_qp->A, a, _econ[a].ind[b], _econ[a].der[b]);
 
 }
 

}

//-------------------------------------------------------------------------
void Prg_Simple::init_x() {
 int a;
 memcpy(_x->ve,_my_x,sizeof(double)*_n);
}

//-------------------------------------------------------------------------
void Prg_Simple::update_bounds() {
 int a;
 for(a=0;a<_mb;a++) 
  if (_bnd[a].type)  _qp->d[a]=_bnd[a].val-_x[_bnd[a].ind]; else
    _qp->d[a]=_x[_bnd[a].ind]-_bnd[a].val;		
}

//-------------------------------------------------------------------------
void Prg_Simple::update_fbd() {
 _update(&_f,_qp->c->ve,_qp->b->ve,_qp->d->ve,_x->ve,0);
 update_bounds();
}

//-------------------------------------------------------------------------
void Prg_Simple::update(const VECP y, const VECP z) {
 int a,b;
 update_bounds();

 _update(&_f,_qp->c->ve,_qp->b->ve,_qp->d->ve,_x->ve,3);
 
 //generic inequality constraints
 for(a=0;a<_m;a++) 
  for(b=0;_icon[a].ind[b]>=0;b++) 
   sp_update_val(_qp->C, a+_mb, _icon[a].ind[b], _icon[a].der[b]); 


 //equality constraints
 for(a=0;a<_me;a++) 
  for(b=0;_econ[a].ind[b]>=0;b++) {
   sp_update_val(_qp->A, a, _econ[a].ind[b], _econ[a].der[b]); 
//  printf("Q: %f\n",_econ[a].der[b]);
 }

}

//=========================================================================









extern "C" int Hqp_Init(Sqp_Config *conf) {
 
 Hqp_SqpProgram *theSqpProgram;
 Hqp_SqpSolver *theSqpSolver;
 Hqp_Solver *solver;
 
 double sqp_eps,sqp_sQs;
 int qp_iter,qp_iters,hela_restart,sqp_iter,nullsteps;

  // disable Meschach's error counting (otherwise program would exit if counter reaches 100)
 count_errs(0);

 
 theSqpProgram = new Prg_Simple(conf);

//  if (conf.Solver==1)
 theSqpSolver = new Hqp_SqpPowell(conf);
 theSqpSolver->set_prg(theSqpProgram);

 theSqpProgram->setup();
 theSqpSolver->init();


 sqp_eps=theSqpSolver->eps();
 qp_iters=0;
 hela_restart=0;
 

 solver=theSqpSolver->solver();


 int i;
 while (1) { 
  //if (qp_iter) 

  theSqpSolver->qp_update();

   
  if (theSqpSolver->xQx()<0.0) {
   theSqpSolver->hela_restart();
   hela_restart=1;
  } else hela_restart=0;



  sqp_iter=theSqpSolver->iter();

  if (sqp_iter) 
   if (theSqpSolver->norm_inf()<sqp_eps) 
    if (theSqpSolver->norm_grd_L()<sqp_eps) break;


  theSqpSolver->qp_solve();

  qp_iter=solver->iter();
  qp_iters+=qp_iter;

  if (!qp_iter) {
   printf("\nqp solver error:  %d\n",solver->result());
//   return -1;
   break;
  }
 
  sqp_sQs=theSqpSolver->sQs();
 
  if (sqp_sQs<0.0) theSqpSolver->hela_restart();

   

  if ((sqp_iter>0) && (sqp_sQs >= 0.0) && (!hela_restart)) {
   if ((theSqpSolver->norm_inf() < sqp_eps) && (solver->result() == Hqp_Optimal)) {
    if (sqp_sQs < sqp_eps*sqp_eps) break;
    if (sqp_iter > 2) {
     if ((theSqpSolver->norm_dx() < sqp_eps*theSqpSolver->norm_x())
       && (theSqpSolver->norm_df() < sqp_eps*fabs(theSqpProgram->f()))
       && (sqp_sQs < sqp_eps)) break;
     }
   }
  }


  
  theSqpSolver->step();


  if (solver->iter() >= solver->max_iters()) {
   printf("\nerror subiters\n");
   break;
//   return(-1);
  }
  
  if (theSqpSolver->iter() >= theSqpSolver->max_iters()) {
   printf("\nerror iters\n");
//   return(-1);
   break;
  }

  if (theSqpSolver->inf_iters() >= theSqpSolver->max_inf_iters()) {
   if (solver->result() == Hqp_Suboptimal ) {
    printf("\nerror infeasible\n");
   } else {
    printf("\nerror degenerate\n");
   }
//   return -1;
   break;
  }

  if ((theSqpSolver->alpha() < 1e-8) && (theSqpSolver->norm_df() < sqp_eps*fabs(theSqpProgram->f()))) nullsteps++; 
     else nullsteps=0;
    
  if (nullsteps > 5) {
   printf("\nerror stall\n");
//   return -1;
   break;
  }

 }



 printf("\nweee!\n");

 memcpy(conf->x,theSqpProgram->x()->ve,sizeof(double)*conf->n);



  // install a handler for signal interrupt
//  signal(SIGINT, &signal_handler);

  // ignore signals from floating point arithmetics
  // (needed for ADOL-C 1.7 on Alpha with OSF3.2, OSF4.0)

  signal(SIGFPE, SIG_IGN);


 theSqpSolver->~Hqp_SqpSolver();
 theSqpProgram->~Hqp_SqpProgram();



  return 0;
}






