    // @(#)root/quadp:$Name:  $:$Id: TQpSolverBase.cxx,v 1.3 2004/06/09 12:23:16 brun Exp $
    // Author: Eddy Offermann   May 2004

    /*************************************************************************
     * Copyright (C) 1995-2000, Rene Brun and Fons Rademakers.               *
     * All rights reserved.                                                  *
     *                                                                       *
     * For the licensing terms see $ROOTSYS/LICENSE.                         *
     * For the list of contributors see $ROOTSYS/README/CREDITS.             *
     *************************************************************************/

    /*************************************************************************
     * Parts of this file are copied from the OOQP distribution and          *
     * are subject to the following license:                                 *
     *                                                                       *
     * COPYRIGHT 2001 UNIVERSITY OF CHICAGO                                  *
     *                                                                       *
     * The copyright holder hereby grants you royalty-free rights to use,    *
     * reproduce, prepare derivative works, and to redistribute this software*
     * to others, provided that any changes are clearly documented. This     *
     * software was authored by:                                             *
     *                                                                       *
     *   E. MICHAEL GERTZ      gertz@mcs.anl.gov                             *
     *   Mathematics and Computer Science Division                           *
     *   Argonne National Laboratory                                         *
     *   9700 S. Cass Avenue                                                 *
     *   Argonne, IL 60439-4844                                              *
     *                                                                       *
     *   STEPHEN J. WRIGHT     swright@cs.wisc.edu                           *
     *   Computer Sciences Department                                        *
     *   University of Wisconsin                                             *
     *   1210 West Dayton Street                                             *
     *   Madison, WI 53706   FAX: (608)262-9777                              *
     *                                                                       *
     * Any questions or comments may be directed to one of the authors.      *
     *                                                                       *
     * ARGONNE NATIONAL LABORATORY (ANL), WITH FACILITIES IN THE STATES OF   *
     * ILLINOIS AND IDAHO, IS OWNED BY THE UNITED STATES GOVERNMENT, AND     *
     * OPERATED BY THE UNIVERSITY OF CHICAGO UNDER PROVISION OF A CONTRACT   *
     * WITH THE DEPARTMENT OF ENERGY.                                        *
     *************************************************************************/

    //////////////////////////////////////////////////////////////////////////
    //                                                                      //
    // TSolverBase                                                          //
    //                                                                      //
    //////////////////////////////////////////////////////////////////////////

    #include "TQpSolverBase.h"

    ClassImp(TQpSolverBase)

    //______________________________________________________________________________
    TQpSolverBase::TQpSolverBase()
    {
      fSys = 0;

      fDnorm = 0.;

      // define parameters associated with the step length heuristic
      fMutol   = 1.0e-8;
      fArtol   = 1.0e-8;
      fGamma_f = 0.99;
      fGamma_a = 1.0/(1.0-fGamma_f);

      fPhi   = 0.0;

      fMaxit = 100;

      // allocate space to track the sequence of complementarity gaps,
      // residual norms, and merit functions.
      fMu_history      = new Double_t[fMaxit];
      fRnorm_history   = new Double_t[fMaxit];
      fPhi_history     = new Double_t[fMaxit];
      fPhi_min_history = new Double_t[fMaxit];
    }

    //______________________________________________________________________________
    TQpSolverBase::TQpSolverBase(const TQpSolverBase &another) : TObject(another)
    {
      *this = another;
    }

    //______________________________________________________________________________
     TQpSolverBase::~TQpSolverBase()
    {
      if (fSys) { delete fSys; fSys = 0; }

      if (fMu_history) { delete [] fMu_history;      fMu_history      = 0; }
      if (fMu_history) { delete [] fRnorm_history;   fRnorm_history   = 0; }
      if (fMu_history) { delete [] fPhi_history;     fPhi_history     = 0; }
      if (fMu_history) { delete [] fPhi_min_history; fPhi_min_history = 0; }
    }

    //______________________________________________________________________________
     void TQpSolverBase::Start(TQpProbBase *formulation,TQpVar *iterate,TQpDataBase *prob,
                              TQpResidual *resid,TQpVar *step)
    {
      this->DefStart(formulation,iterate,prob,resid,step);
    }

    //______________________________________________________________________________
     void TQpSolverBase::DefStart(TQpProbBase * /* formulation */,TQpVar *iterate,
                                 TQpDataBase *prob,TQpResidual *resid,TQpVar *step)
    {
      Double_t sdatanorm = TMath::Sqrt(fDnorm);
      Double_t a         = sdatanorm;
      Double_t b         = sdatanorm;

      iterate->InteriorPoint(a,b);
      resid->CalcResids(prob,iterate);
      resid->Set_r3_xz_alpha(iterate,0.0);

      fSys->Factor(prob,iterate);
      fSys->Solve(prob,iterate,resid,step);
      step->Negate();

      // Take the full affine scaling step
      iterate->Saxpy(step,1.0);
      // resid.CalcResids(prob,iterate); // Calc the resids if debugging.
      Double_t shift = 1.e3+2*iterate->Violation();
      iterate->ShiftBoundVariables(shift,shift);
    }

    //______________________________________________________________________________
     void TQpSolverBase::SteveStart(TQpProbBase * /* formulation */,
                                   TQpVar *iterate,TQpDataBase *prob,
                                   TQpResidual *resid,TQpVar *step)
    {
      Double_t sdatanorm = TMath::Sqrt(fDnorm);
      Double_t a = 0.0;
      Double_t b = 0.0;

      iterate->InteriorPoint(a,b);

      // set the r3 component of the rhs to -(norm of data), and calculate
      // the residuals that are obtained when all values are zero.

      resid->Set_r3_xz_alpha(iterate,-sdatanorm);
      resid->CalcResids(prob,iterate);

      // next, assign 1 to all the complementary variables, so that there
      // are identities in the coefficient matrix when we do the solve.

      a = 1.0; b = 1.0;
      iterate->InteriorPoint(a,b);
      fSys->Factor(prob,iterate);
      fSys->Solve (prob,iterate,resid,step);
      step->Negate();

      // copy the "step" into the current vector

      iterate = step;

      // calculate the maximum violation of the complementarity
      // conditions, and shift these variables to restore positivity.
      Double_t shift = 1.5*iterate->Violation();
      iterate->ShiftBoundVariables(shift,shift);

      // do Mehrotra-type adjustment

      const Double_t mutemp = iterate->GetMu();
      const Double_t xsnorm = iterate->Norm1();
      const Double_t delta = 0.5*iterate->fNComplementaryVariables*mutemp/xsnorm;
      iterate->ShiftBoundVariables(delta,delta);
    }

    //______________________________________________________________________________
     void TQpSolverBase::DumbStart(TQpProbBase * /* formulation */,
                                  TQpVar *iterate,TQpDataBase * /* prob */,
                                  TQpResidual * /* resid */,TQpVar * /* step */)
    {
      const Double_t sdatanorm = fDnorm;
      const Double_t a = 1.e3;
      const Double_t b = 1.e5;
      const Double_t bigstart = a*sdatanorm+b;
      iterate->InteriorPoint(bigstart,bigstart);
    }

    //______________________________________________________________________________
     Double_t TQpSolverBase::FinalStepLength(TQpVar *iterate,TQpVar *step)
    {
      Int_t firstOrSecond;
      Double_t primalValue; Double_t primalStep; Double_t dualValue; Double_t dualStep;
      const Double_t maxAlpha = iterate->FindBlocking(step,primalValue,primalStep,
                                                      dualValue,dualStep,firstOrSecond);
      Double_t mufull = iterate->MuStep(step,maxAlpha);

      mufull /= fGamma_a;

      Double_t alpha = 1.0;
      switch (firstOrSecond) {
      case 0:
        alpha = 1; // No constraints were blocking
        break;
      case 1:
        alpha = (-primalValue+mufull/(dualValue+maxAlpha*dualStep))/primalStep;
        break;
      case 2:
        alpha = (-dualValue+mufull/(primalValue+maxAlpha*primalStep))/dualStep;
        break;
      default:
        Assert(0 && "Can't get here");
        break;
      }
      // make it at least fGamma_f * maxStep
      if (alpha < fGamma_f*maxAlpha) alpha = fGamma_f*maxAlpha;
      // back off just a touch
      alpha *= .99999999;

      return alpha;
    }

    //______________________________________________________________________________
     void TQpSolverBase::DoMonitor(TQpDataBase *data,TQpVar *vars,TQpResidual *resids,
                                  Double_t alpha,Double_t sigma,Int_t i,Double_t mu,
                                  Int_t stop_code,Int_t level)
    {
      this->DefMonitor(data,vars,resids,alpha,sigma,i,mu,stop_code,level);
    }

    //______________________________________________________________________________
     Int_t TQpSolverBase::DoStatus(TQpDataBase *data,TQpVar *vars,TQpResidual *resids,
                                  Int_t i,Double_t mu,Int_t level)
    {
      return this->DefStatus(data,vars,resids,i,mu,level);
    }

    //______________________________________________________________________________
     Int_t TQpSolverBase::DefStatus(TQpDataBase * /* data */,TQpVar * /* vars */,
                                   TQpResidual *resids,Int_t iterate,Double_t mu,Int_t /* level */)
    {
      Int_t stop_code = kNOT_FINISHED;

      const Double_t gap   = TMath::Abs(resids->GetDualityGap());
      const Double_t rnorm = resids->GetResidualNorm();

      Int_t idx = iterate-1;
      if (idx <  0     ) idx = 0;
      if (idx >= fMaxit) idx = fMaxit-1;

      // store the historical record
      fMu_history[idx] = mu;
      fRnorm_history[idx] = rnorm;
      fPhi = (rnorm+gap)/fDnorm;
      fPhi_history[idx] = fPhi;

      if (idx > 0) {
        fPhi_min_history[idx] = fPhi_min_history[idx-1];
        if (fPhi < fPhi_min_history[idx]) fPhi_min_history[idx] = fPhi;
      } else
        fPhi_min_history[idx] = fPhi;

      if (iterate >= fMaxit) {
        stop_code = kMAX_ITS_EXCEEDED;
      } else if (mu <= fMutol && rnorm <= fArtol*fDnorm) {
        stop_code = kSUCCESSFUL_TERMINATION;
      }
      if (stop_code != kNOT_FINISHED)  return stop_code;

      // check infeasibility condition
      if (idx >= 10 && fPhi >= 1.e-8 && fPhi >= 1.e4*fPhi_min_history[idx])
        stop_code = kINFEASIBLE;
      if (stop_code != kNOT_FINISHED)  return stop_code;

      // check for unknown status: slow convergence first
      if (idx >= 30 && fPhi_min_history[idx] >= .5*fPhi_min_history[idx-30])
        stop_code = kUNKNOWN;

      if (rnorm/fDnorm > fArtol &&
         (fRnorm_history[idx]/fMu_history[idx])/(fRnorm_history[0]/fMu_history[0]) >= 1.e8)
        stop_code = kUNKNOWN;

      return stop_code;
    }

    //______________________________________________________________________________
    TQpSolverBase &TQpSolverBase::operator=(const TQpSolverBase &source)
    {
      if (this != &source) {
        TObject::operator=(source);

        fSys     = source.fSys;
        fDnorm   = source.fDnorm;
        fMutol   = source.fMutol;
        fArtol   = source.fArtol;
        fGamma_f = source.fGamma_f;
        fGamma_a = source.fGamma_a;
        fPhi     = source.fPhi;
        fIter    = source.fIter;

        if (fMaxit != source.fMaxit) {
          if (fMu_history) delete [] fMu_history;
          fMu_history = new Double_t[fMaxit];
          if (fRnorm_history) delete [] fRnorm_history;
          fRnorm_history = new Double_t[fMaxit];
          if (fPhi_history) delete [] fPhi_history;
          fPhi_history = new Double_t[fMaxit];
          if (fPhi_min_history) delete [] fPhi_min_history;
          fPhi_min_history = new Double_t[fMaxit];
        }

        fMaxit = source.fMaxit;
        memcpy(fMu_history,source.fMu_history,fMaxit*sizeof(Double_t));
        memcpy(fRnorm_history,source.fRnorm_history,fMaxit*sizeof(Double_t));
        memcpy(fPhi_history,source.fPhi_history,fMaxit*sizeof(Double_t));
        memcpy(fPhi_min_history,source.fPhi_min_history,fMaxit*sizeof(Double_t));
      }
      return *this;
    }