/*
  qlpsolve.cc -- implement Active_constraints, Inactive_iter

  source file of the GNU LilyPond music typesetter

  (c) 1996,  1997--1998 Han-Wen Nienhuys <hanwen@cs.uu.nl>

  TODO:
  try fixed point arithmetic, to speed up lily.
  */

#include "ineq-constrained-qp.hh"
#include "qlpsolve.hh"
#include "debug.hh"
#include "choleski.hh"

const Real TOL=1e-1;		// roughly 1/30 mm

String
Active_constraints::status() const
{
  String s ("Active|Inactive [");
  for (int i=0; i< active.size(); i++)
    {
      s += to_str (active[i]) + " ";
    }

  s+="| ";
  for (int i=0; i< inactive.size(); i++)
    {
      s += to_str (inactive[i]) + " ";
    }
  s+="]";

  return s;
}

void
Active_constraints::OK()
{
#ifndef NDEBUG
  H.OK();
  A.OK();
  assert (active.size() +inactive.size () == opt->cons_.size ());
  assert (H.dim() == opt->dim ());
  assert (active.size() == A.rows ());
  Array<int> allcons;

  for (int i=0; i < opt->cons_.size(); i++)
    allcons.push (0);
  for (int i=0; i < active.size(); i++)
    {
      int j = active[i];
      allcons[j]++;
    }
  for (int i=0; i < inactive.size(); i++)
    {
      int j = inactive[i];
      allcons[j]++;
    }
  for (int i=0; i < allcons.size(); i++)
    assert (allcons[i] == 1);
#endif
}

Vector
Active_constraints::get_lagrange (Vector gradient)
{
  return (A*gradient);
}

void
Active_constraints::add_constraint (int k)
{
  // add indices
  int cidx=inactive[k];

  Vector a (opt->cons_[cidx]);
  // update of matrices
  Vector Ha = H*a;
  Real aHa = a*Ha;
  Vector addrow (Ha.dim());
  bool degenerate =  (abs (aHa) < EPS);

  if (degenerate)
    {
      warning (String ("Active_constraints::add ():")
	+ _("degenerate constraints"));
      DOUT << "Ha = "  << Ha.str () << '\n';
      /*
	a != 0, so if Ha = O(EPS), then
	Ha * aH / aHa = O(EPS^2/EPS)

	if H*a == 0, the constraints are dependent.
      */
      degenerate_count_i_ ++;
    }
  if (!degenerate)
    {
      active.push (cidx);
      inactive.swap (k,inactive.size()-1);
      inactive.pop();

      H -= Matrix (Ha/aHa , Ha);
      
      addrow=Ha;
      addrow /= aHa;
      A -= Matrix (A*a, addrow);
      A.insert_row (addrow,A.rows());
    }
}

void
Active_constraints::drop_constraint (int k)
{
  int q=active.size()-1;


  Vector a (A.row (q));
  if (a.norm() > EPS)
    {
      // drop indices
      inactive.push (active[k]);
      active.swap (k,q);
      A.swap_rows (k,q);
      active.pop();
      /*

       */
      Real aqa = a*opt->quad_*a;
      Matrix aaq (a,a/aqa);
      H += aaq;
      A -= A*opt->quad_*aaq;
 
      A.delete_row (q);
    }else {
      degenerate_count_i_ ++;
      warning (String ("Active_constraints::drop ():")
	+ _("degenerate constraints"));
    }
}


Active_constraints::Active_constraints (Ineq_constrained_qp const *op)
  : A(0,op->dim()),
    H(op->dim()),
    opt (op)
{
  for (int i=0; i < op->cons_.size(); i++)
    inactive.push (i);
  Choleski_decomposition chol (op->quad_);

  /*
    ugh.
    */
  H=chol.inverse();
  OK();

  degenerate_count_i_ = 0;
}

/** Find the optimum which is in the planes generated by the active
  constraints.
  */
Vector
Active_constraints::find_active_optimum (Vector g)
{
  return H*g;
}
