    // @(#)root/quadp:$Name:  $:$Id: TQpResidual.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.                                        *
     *************************************************************************/

    //////////////////////////////////////////////////////////////////////////
    //                                                                      //
    // TQpResidual                                                          //
    //                                                                      //
    // Residuals for the general QP formulation                             //
    //                                                                      //
    //////////////////////////////////////////////////////////////////////////

    #include "Riostream.h"
    #include "TQpResidual.h"
    #include "TMatrixD.h"

    ClassImp(TQpResidual)

    //______________________________________________________________________________
     TQpResidual::TQpResidual()
    {
      fNx   = 0;
      fMy   = 0;
      fMz   = 0;

      fNxup = 0.0;
      fNxlo = 0.0;
      fMcup = 0.0;
      fMclo = 0.0;
    }

    //______________________________________________________________________________
     TQpResidual::TQpResidual(Int_t nx,Int_t my,Int_t mz,TVectorD &ixlo,TVectorD &ixup,
                                                        TVectorD &iclo,TVectorD &icup)
    {
      fNx = nx;
      fMy = my;
      fMz = mz;

      if (ixlo.GetNrows() > 0) fXloIndex.Use(ixlo.GetNrows(),ixlo.GetMatrixArray());
      if (ixup.GetNrows() > 0) fXupIndex.Use(ixup.GetNrows(),ixup.GetMatrixArray());
      if (iclo.GetNrows() > 0) fCloIndex.Use(iclo.GetNrows(),iclo.GetMatrixArray());
      if (icup.GetNrows() > 0) fCupIndex.Use(icup.GetNrows(),icup.GetMatrixArray());
      fNxlo = ixlo.NonZeros();
      fNxup = ixup.NonZeros();
      fMclo = iclo.NonZeros();
      fMcup = icup.NonZeros();

      fRQ.ResizeTo(fNx);
      fRA.ResizeTo(fMy);
      fRC.ResizeTo(fMz);

      fRz.ResizeTo(fMz);
      if (fMclo > 0) {
        fRt.ResizeTo(fMz);
        fRlambda.ResizeTo(fMz);
      }
      if (fMcup > 0) {
        fRu.ResizeTo(fMz);
        fRpi.ResizeTo(fMz);
      }
      if (fNxlo > 0) {
        fRv.ResizeTo(fNx);
        fRgamma.ResizeTo(fNx);
      }
      if (fNxup > 0) {
        fRw.ResizeTo(fNx);
        fRphi.ResizeTo(fNx);
      }
    }

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

    //______________________________________________________________________________
     void TQpResidual::CalcResids(TQpDataBase *prob_in,TQpVar *vars)
    {
      TQpDataDens *prob = (TQpDataDens *) prob_in;

      fRQ.ResizeTo(prob->fG); fRQ = prob->fG;
      prob->Qmult(1.0,fRQ,1.0,vars->fX);

      // calculate x^T (g+Qx) - contribution to the duality gap
      Double_t gap = fRQ*vars->fX;

      prob->ATransmult(1.0,fRQ,-1.0,vars->fY);
      prob->CTransmult(1.0,fRQ,-1.0,vars->fZ);
      if (fNxlo > 0) Add(fRQ,-1.0,vars->fGamma);
      if (fNxup > 0) Add(fRQ, 1.0,vars->fPhi);

      Double_t norm = 0.0;
      Double_t componentNorm = fRQ.NormInf();
      if (componentNorm > norm) norm = componentNorm;

      fRA.ResizeTo(prob->fBa); fRA = prob->fBa;
      prob->Amult(-1.0,fRA,1.0,vars->fX);

      // contribution -d^T y to duality gap
      gap -= prob->fBa*vars->fY;

      componentNorm = fRA.NormInf();
      if( componentNorm > norm ) norm = componentNorm;

      fRC.ResizeTo(vars->fS); fRC = vars->fS;
      prob->Cmult(-1.0,fRC,1.0,vars->fX);

      componentNorm = fRC.NormInf();
      if( componentNorm > norm ) norm = componentNorm;

      fRz.ResizeTo(vars->fZ); fRz = vars->fZ;

      if (fMclo > 0) {
        Add(fRz,-1.0,vars->fLambda);

        fRt.ResizeTo(vars->fS); fRt = vars->fS;
        Add(fRt,-1.0,prob->GetSlowerBound());
        fRt.SelectNonZeros(fCloIndex);
        Add(fRt,-1.0,vars->fT);

        gap -= prob->fCloBound*vars->fLambda;

        componentNorm = fRt.NormInf();
        if( componentNorm > norm ) norm = componentNorm;
      }

      if (fMcup > 0) {
        Add(fRz,1.0,vars->fPi);

        fRu.ResizeTo(vars->fS); fRu = vars->fS;
        Add(fRu,-1.0,prob->GetSupperBound() );
        fRu.SelectNonZeros(fCupIndex);
        Add(fRu,1.0,vars->fU);

        gap += prob->fCupBound*vars->fPi;

        componentNorm = fRu.NormInf();
        if( componentNorm > norm ) norm = componentNorm;
      }

      componentNorm = fRz.NormInf();
      if( componentNorm > norm ) norm = componentNorm;

      if (fNxlo > 0) {
        fRv.ResizeTo(vars->fX); fRv = vars->fX;
        Add(fRv,-1.0,prob->GetXlowerBound());
        fRv.SelectNonZeros(fXloIndex);
        Add(fRv,-1.0,vars->fV);

        gap -= prob->fXloBound*vars->fGamma;

        componentNorm = fRv.NormInf();
        if( componentNorm > norm ) norm = componentNorm;
      }

      if (fNxup > 0) {
        fRw.ResizeTo(vars->fX); fRw = vars->fX;
        Add(fRw,-1.0,prob->GetXupperBound());
        fRw.SelectNonZeros(fXupIndex);
        Add(fRw,1.0,vars->fW);

        gap += prob->fXupBound*vars->fPhi;

        componentNorm = fRw.NormInf();
        if (componentNorm > norm) norm = componentNorm;
      }

      fDualityGap   = gap;
      fResidualNorm = norm;
    }

    //______________________________________________________________________________
     void TQpResidual::Add_r3_xz_alpha(TQpVar *vars,Double_t alpha)
    {
      if (fMclo > 0) AddElemMult(fRlambda,1.0,vars->fT,vars->fLambda);
      if (fMcup > 0) AddElemMult(fRpi    ,1.0,vars->fU,vars->fPi);
      if (fNxlo > 0) AddElemMult(fRgamma ,1.0,vars->fV,vars->fGamma);
      if (fNxup > 0) AddElemMult(fRphi   ,1.0,vars->fW,vars->fPhi);

      if (alpha != 0.0) {
        if (fMclo > 0) fRlambda.AddSomeConstant(alpha,fCloIndex);
        if (fMcup > 0) fRpi    .AddSomeConstant(alpha,fCupIndex);
        if (fNxlo > 0) fRgamma .AddSomeConstant(alpha,fXloIndex);
        if (fNxup > 0) fRphi   .AddSomeConstant(alpha,fXupIndex);
      }
    }

    //______________________________________________________________________________
     void TQpResidual::Set_r3_xz_alpha(TQpVar *vars,Double_t alpha)
    {
      this->Clear_r3();
      this->Add_r3_xz_alpha(vars,alpha);
    }

    //______________________________________________________________________________
     void TQpResidual::Clear_r3()
    {
      if (fMclo > 0) fRlambda.Zero();
      if (fMcup > 0) fRpi    .Zero();
      if (fNxlo > 0) fRgamma .Zero();
      if (fNxup > 0) fRphi   .Zero();
    }

    //______________________________________________________________________________
     void TQpResidual::Clear_r1r2()
    {
      fRQ.Zero();
      fRA.Zero();
      fRC.Zero();
      fRz.Zero();
      if (fNxlo > 0) fRv.Zero();
      if (fNxup > 0) fRw.Zero();
      if (fMclo > 0) fRt.Zero();
      if (fMcup > 0) fRu.Zero();
    }

    //______________________________________________________________________________
     void TQpResidual::Project_r3(Double_t rmin,Double_t rmax)
    {
      if (fMclo > 0) {
        GondzioProjection(fRlambda,rmin,rmax);
        fRlambda.SelectNonZeros(fCloIndex);
      }
      if (fMcup > 0) {
        GondzioProjection(fRpi,rmin,rmax);
        fRpi.SelectNonZeros(fCupIndex);
      }
      if (fNxlo > 0) {
        GondzioProjection(fRgamma,rmin,rmax);
        fRgamma.SelectNonZeros(fXloIndex);
      }
      if (fNxup > 0) {
        GondzioProjection(fRphi,rmin,rmax);
        fRphi.SelectNonZeros(fXupIndex);
      }
    }

    //______________________________________________________________________________
     Bool_t TQpResidual::ValidNonZeroPattern()
    {
      if (fNxlo > 0 &&
          (!fRv    .MatchesNonZeroPattern(fXloIndex) ||
           !fRgamma.MatchesNonZeroPattern(fXloIndex)) ) {
        return kFALSE;
      }

      if (fNxup > 0 &&
          (!fRw  .MatchesNonZeroPattern(fXupIndex) ||
           !fRphi.MatchesNonZeroPattern(fXupIndex)) ) {
        return kFALSE;
      }
      if (fMclo > 0 &&
          (!fRt     .MatchesNonZeroPattern(fCloIndex) ||
           !fRlambda.MatchesNonZeroPattern(fCloIndex)) ) {
        return kFALSE;
      }

      if (fMcup > 0 &&
         (!fRu .MatchesNonZeroPattern(fCupIndex) ||
          !fRpi.MatchesNonZeroPattern(fCupIndex)) ) {
        return kFALSE;
      }

      return kTRUE;
    }

    //______________________________________________________________________________
     void TQpResidual::GondzioProjection(TVectorD &v,Double_t rmin,Double_t rmax)
    {
            Double_t *        ep = v.GetMatrixArray();
      const Double_t * const fep = ep+v.GetNrows();

      while (ep < fep) {
        if (*ep < rmin)
          *ep = rmin - *ep;
        else if (*ep > rmax)
          *ep = rmax - *ep;
        else
          *ep = 0.0;
        if (*ep < -rmax) *ep = -rmax;
        ep++;
      }
    }

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

        fNx   = source.fNx;
        fMy   = source.fMy;
        fMz   = source.fMz;

        fNxup = source.fNxup;
        fNxlo = source.fNxlo;
        fMcup = source.fMcup;
        fMclo = source.fMclo;

        fXupIndex.ResizeTo(source.fXupIndex); fXupIndex = source.fXupIndex;
        fXloIndex.ResizeTo(source.fXloIndex); fXloIndex = source.fXloIndex;
        fCupIndex.ResizeTo(source.fCupIndex); fCupIndex = source.fCupIndex;
        fCloIndex.ResizeTo(source.fCloIndex); fCupIndex = source.fCupIndex;

        fRQ     .ResizeTo(source.fRQ);      fRQ      = source.fRQ;
        fRA     .ResizeTo(source.fRA);      fRA      = source.fRA;
        fRC     .ResizeTo(source.fRC);      fRC      = source.fRC;
        fRz     .ResizeTo(source.fRz);      fRz      = source.fRz;
        fRv     .ResizeTo(source.fRv);      fRv      = source.fRv;
        fRw     .ResizeTo(source.fRw);      fRw      = source.fRw;
        fRt     .ResizeTo(source.fRt);      fRt      = source.fRt;
        fRu     .ResizeTo(source.fRu);      fRu      = source.fRu;
        fRgamma .ResizeTo(source.fRgamma);  fRgamma  = source.fRgamma;
        fRphi   .ResizeTo(source.fRphi);    fRphi    = source.fRphi;
        fRlambda.ResizeTo(source.fRlambda); fRlambda = source.fRlambda;
        fRpi    .ResizeTo(source.fRpi);     fRpi     = source.fRpi;
      }
      return *this;
    }