igofmm.hpp

#ifndef IGOFMM_HPP
#define IGOFMM_HPP


using namespace std;
using namespace hmlp;




namespace hmlp
{
namespace gofmm
{


template<typename T>
class Factor
{
  public:

    Factor() {};
    
    void SetupFactor
    (
      bool issymmetric, bool do_ulv_factorization,
      bool isleaf, bool isroot,
      size_t n, size_t nl, size_t nr,
      size_t s, size_t sl, size_t sr
    )
    {
      this->issymmetric = issymmetric;
      this->do_ulv_factorization = do_ulv_factorization;
      this->isleaf = isleaf;
      this->isroot = isroot;
      this->n = n; this->nl = nl; this->nr = nr;
      this->s = s; this->sl = sl; this->sr = sr;
    };

    void SetupFactor
    (
      bool issymmetric, bool do_ulv_factorization,
      bool isleaf, bool isroot,
      size_t n, size_t nl, size_t nr,
      size_t s, size_t sl, size_t sr,
      Data<T> &U,
      Data<T> &V
    )
    {
      SetupFactor( issymmetric, do_ulv_factorization, 
          isleaf, isroot, n, nl, nr, s, sl, sr );
    };

    bool DoULVFactorization()
    {
      return do_ulv_factorization;
    };

    bool IsSymmetric() { return issymmetric; };





    void CheckCondition()
    {
      assert( do_ulv_factorization && issymmetric );
      T max_diag = 0.0;
      T min_diag = 0.0;

      for ( size_t i = 0; i < Z.row(); i ++ )
      {
        T abs_diag =  std::abs( Z( i, i ) );

        if ( !i )
        {
          max_diag = abs_diag;
          min_diag = abs_diag;
        }

        if ( abs_diag > max_diag ) max_diag = abs_diag;
        if ( abs_diag < min_diag ) min_diag = abs_diag;
      }

      printf( "condiditon( Z ): min_diag %3.1E max_diag %3.1E ratio %3.1E\n",
          min_diag, max_diag, min_diag / max_diag );
    };

    void Factorize( Data<T> &Kaa ) 
    {
      assert( isleaf );
      assert( Kaa.row() == n ); assert( Kaa.col() == n );

      Z = Kaa;

      ipiv.resize( n, 0 );

      T nrm1 = 0.0;
      for ( auto &z : Z ) nrm1 += z;

      xgetrf( n, n, Z.data(), n, ipiv.data() );

      T rcond1 = 0.0;
      Data<T> work( Z.row(), 4 );
      vector<int> iwork( Z.row() );
      xgecon( "1", Z.row(), Z.data(), Z.row(), nrm1, 
          &rcond1, work.data(), iwork.data() );
      if ( 1.0 / rcond1 > 1E+6 )
        printf( "Warning! large 1-norm condition number %3.1E, nrm1( Z ) %3.1E\n", 
            1.0 / rcond1, nrm1 );
    }; 
    void PartialFactorize( Data<T> &A )
    {
      Z = A;
      ChangeBasis( Z );

      Zv.Set( false, Z );
      Zv.Partition2x2( Ztl, Ztr,
                       Zbl, Zbr, s, s, BOTTOMRIGHT );

      //printf( "Ztl %lux%lu Ztr %lux%lu\n", Ztl.row(), Ztl.col(), Ztr.row(), Ztr.col() ); fflush( stdout );
      //printf( "Zbl %lux%lu Zbr %lux%lu\n", Zbl.row(), Zbl.col(), Zbr.row(), Zbr.col() ); fflush( stdout );

      ipiv.resize( Ztl.row(), 0 );
      xgetrf( Ztl.row(), Z.col(), Z.data(), Z.row(), ipiv.data() );
      xtrsm( "Right", "Upper", "No transpose", "Non-unit", Zbl.row(), Zbl.col(),
          1.0,  Ztl.data(), Ztl.ld(), Zbl.data(), Zbl.ld() );
      xgemm( "No transpose", "No transpose", Zbr.row(), Zbr.col(), Ztl.col(),
          -1.0, Zbl.data(), Zbl.ld(),
                Ztr.data(), Ztr.ld(),
           1.0, Zbr.data(), Zbr.ld() );

    }; 
    void Factorize
    ( 
      Data<T> &Ul, 
      Data<T> &Ur, 
      Data<T> &Vl,
      Data<T> &Vr
    )
    {
      assert( !isleaf );
      //assert( Ul.row() == nl ); assert( Ul.col() == sl );
      //assert( Ur.row() == nr ); assert( Ur.col() == sr );
      //assert( Vl.row() == nl ); assert( Vl.col() == sl );
      //assert( Vr.row() == nr ); assert( Vr.col() == sr );

      if ( issymmetric )
      {
        assert( Crl.row() == sr ); assert( Crl.col() == sl );
      }
      else
      {
        assert( Clr.row() == sl ); assert( Clr.col() == sr );
        assert( Crl.row() == sr ); assert( Crl.col() == sl );
      }
     
      Z.resize( 0, 0 );
      Z.resize( sl + sr, sl + sr, 0.0 );
      for ( size_t i = 0; i < sl + sr; i ++ ) Z[ i * Z.row() + i ] = 1.0;



      if ( do_ulv_factorization )
      {
        if ( issymmetric ) 
        {
          hmlp::Data<T> Zbl = Crl;

          //printf( "Crl\n" );
          //Crl.Print();


          xtrmm
          ( 
            "Right", "Upper", "Transpose", "Non-unit",
            Zbl.row(), Zbl.col(),
            1.0,  Ul.data(),  Ul.row(),
                 Zbl.data(), Zbl.row()
          );
          //printf( "Ul.row() %lu Zbl.row() %lu Zbl.col() %lu\n",
          //    Ul.row(), Zbl.row(), Zbl.col() );

          xtrmm
          ( 
            "Left", "Upper", "Non-transpose", "Non-unit",
            Zbl.row(), Zbl.col(),
            1.0,  Ur.data(),  Ur.row(),
                 Zbl.data(), Zbl.row()
          );
          //printf( "Ur.row() %lu Zbl.row() %lu Zbl.col() %lu\n",
          //    Ur.row(), Zbl.row(), Zbl.col() );

          for ( size_t j = 0; j < sl; j ++ )
            for ( size_t i = 0; i < sr; i ++ )
            {
              Z( sl + i, j ) = Zbl( i, j );
              Z( j, sl + i ) = Zbl( i, j );
            }

          if ( 1 )
          {
            xpotrf( "Lower", Z.row(), Z.data(), Z.row() );
            //CheckCondition();
          }
          else
          {
            ipiv.resize( Z.row(), 0 );
            xgetrf( Z.row(), Z.col(), Z.data(), Z.row(), ipiv.data() );
          }
        }
        else 
        {
          ipiv.resize( Z.row(), 0 );
        }
      }
      else 
      {
        ipiv.resize( Z.row(), 0 );

        std::vector<T> VltUl( sl * sl, 0.0 );
        std::vector<T> VrtUr( sr * sr, 0.0 );

        xgemm( "T", "N", sl, sl, nl, 
            1.0,    Vl.data(), nl, 
            Ul.data(), nl, 
            0.0, VltUl.data(), sl );

        xgemm( "T", "N", sr, sr, nr, 
            1.0,    Vr.data(), nr, 
            Ur.data(), nr, 
            0.0, VrtUr.data(), sr );

        xgemm( "N", "N", sr, sl, sl,
            1.0,   Crl.data(), sr, 
            VltUl.data(), sl, 
            0.0,     Z.data() + sl, sl + sr );


        if ( issymmetric )
        {
          xgemm( "T", "N", sl, sr, sr,
              1.0,   Crl.data(), sr, 
              VrtUr.data(), sr, 
              0.0,     Z.data() + ( sl + sr ) * sl, sl + sr );
        }
        else
        {
          printf( "bug\n" ); exit( 1 );
          xgemm( "N", "N", sl, sr, sr,
              1.0,   Clr.data(), sl, 
              VrtUr.data(), sr, 
              0.0,     Z.data() + ( sl + sr ) * sl, sl + sr );
        }

        T nrm1 = 0.0;
        for ( size_t i = 0; i < Z.size(); i ++ ) 
          nrm1 += std::abs( Z[ i ] );

        xgetrf( Z.row(), Z.col(), Z.data(), Z.row(), ipiv.data() );

        this->Ul = &Ul;
        this->Ur = &Ur;
        this->Vl = &Vl;
        this->Vr = &Vr;

        T rcond1 = 0.0;
        hmlp::Data<T> work( Z.row(), 4 );
        std::vector<int> iwork( Z.row() );
        xgecon( "1", Z.row(), Z.data(), Z.row(), nrm1, 
            &rcond1, work.data(), iwork.data() );
        if ( 1.0 / rcond1 > 1E+6 )
          printf( "Warning! large 1-norm condition number %3.1E\n", 
              1.0 / rcond1 ); fflush( stdout );
      }

    }; 
    void PartialFactorize( 
      View<T> &Zl, View<T> &Zr,
      Data<T> &Ul, Data<T> &Ur, 
      Data<T> &Vl, Data<T> &Vr )
    {
      Z.resize( 0, 0 );
      Z.resize( sl + sr, sl + sr, 0.0 );

      Zv.Set( false, Z );
      Zv.Partition2x2( Ztl, Ztr,
                       Zbl, Zbr, sl, sl, TOPLEFT );

      //printf( "Ztl %lux%lu Ztr %lux%lu\n", Ztl.row(), Ztl.col(), Ztr.row(), Ztr.col() ); fflush( stdout );
      //printf( "Zbl %lux%lu Zbr %lux%lu\n", Zbl.row(), Zbl.col(), Zbr.row(), Zbr.col() ); fflush( stdout );


      Zbl.CopyValuesFrom( Crl );
      xtrmm( "Right", "Upper",     "Transpose", "Non-unit", Zbl.row(), Zbl.col(),
        1.0,  Ul.data(),  Ul.row(), Zbl.data(), Zbl.ld() );
      xtrmm(  "Left", "Upper", "Non-transpose", "Non-unit", Zbl.row(), Zbl.col(),
        1.0,  Ur.data(),  Ur.row(), Zbl.data(), Zbl.ld() );

      Ztl.CopyValuesFrom( Zl );
      Zbr.CopyValuesFrom( Zr );

      for ( size_t j = 0; j < sl; j ++ )
        for ( size_t i = 0; i < sr; i ++ )
          Ztr( j, i ) = Zbl( i, j );

      PartialFactorize( Z );

    }; 
    void Multiply( View<T> &bl, View<T> &br )
    {
      assert( !isleaf && bl.col() == br.col() );
    
      size_t nrhs = bl.col();

      std::vector<T> ta( ( sl + sr ) * nrhs );
      std::vector<T> tl(      sl * nrhs );
      std::vector<T> tr(      sr * nrhs );

      xgemm( "T", "N", sl, nrhs, nl,
          1.0, Vl->data(), nl, 
                bl.data(), bl.ld(), 
          0.0,  tl.data(), sl );
      xgemm( "T", "N", sr, nrhs, nr,
          1.0, Vr->data(), nr, 
                br.data(), br.ld(), 
          0.0,  tr.data(), sr );

      xgemm( "N", "N", sr, nrhs, sl,
          1.0, Crl.data(), sr, 
                tl.data(), sl, 
          0.0,  ta.data() + sl, sl + sr );

      if ( issymmetric )
      {
        xgemm( "T", "N", sl, nrhs, sr,
            1.0, Crl.data(), sr, 
                  tr.data(), sr, 
            0.0,  ta.data(), sl + sr );
      }
      else
      {
        printf( "bug here !!!!!\n" ); fflush( stdout ); exit( 1 );
        xgemm( "N", "N", sl, nrhs, sr,
            1.0, Clr.data(), sl, 
                  tr.data(), sr, 
            0.0,  ta.data(), sl + sr );
      }

      xgemm( "N", "N", nl, nrhs, sl,
        -1.0, Ul->data(), nl, 
               ta.data(), sl + sr, 
         1.0,  bl.data(), bl.ld() );

      xgemm( "N", "N", nr, nrhs, sr,
         -1.0, Ur->data(), nr, 
                ta.data() + sl, sl + sr, 
          1.0,  br.data(), br.ld() );
    };

    void Solve( View<T> &rhs ) 
    {
      assert( isleaf );
      assert( !do_ulv_factorization );
      assert( rhs.data() && Z.data() );
      assert( ipiv.data() );

      //rhs.Print();

      size_t nrhs = rhs.col();

      xgetrs( "Non-transpose", rhs.row(), nrhs, 
          Z.data(), Z.row(), ipiv.data(), 
          rhs.data(), rhs.ld() );

    }; 
    void Solve( View<T> &bl, View<T> &br ) 
    {
      size_t nrhs = bl.col();

      //bl.Print();
      //br.Print();

      assert( !do_ulv_factorization );
      assert( bl.col() == br.col() );
      assert( bl.row() == nl );
      assert( br.row() == nr );
      assert( Ul && Ur && Vl && Vr );

//      hmlp::Data<T> ta( sl + sr, nrhs );
//      hmlp::Data<T> tl(      sl, nrhs );
//      hmlp::Data<T> tr(      sr, nrhs );

      vector<T> ta( ( sl + sr ) * nrhs );
      vector<T> tl(      sl * nrhs );
      vector<T> tr(      sr * nrhs );


      //hmlp::View<T> xa( ta ), xl, xr;

      //xa.Partition2x1
      //( 
      //  xl, 
      //  xr, sl
      //);


        xgemm( "T", "N", sl, nrhs, nl,
            1.0, Vl->data(), nl, 
                  bl.data(), bl.ld(), 
            0.0,  tl.data(), sl );
        xgemm( "T", "N", sr, nrhs, nr,
            1.0, Vr->data(), nr, 
                  br.data(), br.ld(), 
            0.0,  tr.data(), sr );


        xgemm( "N", "N", sr, nrhs, sl,
            1.0, Crl.data(), sr, 
            tl.data(), sl, 
            0.0,  ta.data() + sl, sl + sr );

        if ( issymmetric )
        {
          xgemm( "T", "N", sl, nrhs, sr,
              1.0, Crl.data(), sr, 
              tr.data(), sr, 
              0.0,  ta.data(), sl + sr );
        }
        else
        {
          printf( "bug here !!!!!\n" ); fflush( stdout ); exit( 1 );
          xgemm( "N", "N", sl, nrhs, sr,
              1.0, Clr.data(), sl, 
              tr.data(), sr, 
              0.0,  ta.data(), sl + sr );
        }

        xgetrs( "N", sl + sr, nrhs, 
            Z.data(), Z.row(), ipiv.data(), 
            ta.data(), sl + sr );

      xgemm( "N", "N", nl, nrhs, sl,
          -1.0, Ul->data(), nl, 
          ta.data(), sl + sr, 
          1.0,  bl.data(), bl.ld() );

      xgemm( "N", "N", nr, nrhs, sr,
          -1.0, Ur->data(), nr, 
          ta.data() + sl, sl + sr, 
          1.0,  br.data(), br.ld() );

    }; 
    void Telescope
    (
      bool DO_INVERSE,
      Data<T> &Pa,
      Data<T> &Palr 
    )
    {
      assert( isleaf ); 
      Pa.resize( n, s, 0.0 );

      //hmlp::View<T> Xa;

      //Xa.Set( Pa ); 

      assert( Palr.row() == s ); assert( Palr.col() == n );

      for ( size_t j = 0; j < Pa.col(); j ++ )
        for ( size_t i = 0; i < Pa.row(); i ++ )
          Pa( i, j ) = Palr( j, i );

      if ( DO_INVERSE )
      {
        if ( do_ulv_factorization )
        {
          xtrsm( "Left", "Lower", "No transpose", "Non-unit", 
              Pa.row(), Pa.col(), 
              1.0,  Z.data(),  Z.row(), Pa.data(), Pa.row() );
        }
        else
        {
          assert( ipiv.size() );
          xgetrs( "Non-transpose", 
              n, s, Z.data(), n, ipiv.data(), Pa.data(), n );
        }
      }


      //printf( "call solve from telescope\n" ); fflush( stdout );
      //if ( DO_INVERSE ) Solve<true>( Xa );
      //printf( "call solve from telescope (exist)\n" ); fflush( stdout );

    }; 
    void Telescope
    ( 
      bool DO_INVERSE,
      Data<T> &Pa,
      Data<T> &Palr,
      Data<T> &Pl,
      Data<T> &Pr
    ) 
    {
      assert( !isleaf );
      assert( n == nl + nr );
      assert( Pl.col() == sl );
      assert( Pr.col() == sr );
      assert( Palr.row() == s  ); assert( Palr.col() == ( sl + sr ) );

      Pa.resize( 0, 0 );

      //hmlp::View<T> Xa;

      //Xa.Set( Pa ); 
      //assert( Xa.row() == Pa.row() ); 
      //assert( Xa.col() == Pa.col() ); 

      if ( do_ulv_factorization )
      {
        Pa.resize( sl + sr, s, 0.0 );

        for ( size_t j = 0; j < Pa.col(); j ++ )
          for ( size_t i = 0; i < Pa.row(); i ++ )
            Pa[ j * Pa.row() + i ] = Palr[ i * Palr.row() + j ];

        xtrmm( "Left", "Upper", "No Transpose", "Non-unit", sl, s,
           1.0,   Pl.data(), Pl.row(), 
                  Pa.data(), Pa.row() );
        //  printf( "Pl.row() %lu Pa.row() %lu Pa.col() %lu\n",
        //      Pl.row(), Pa.row(), Pa.col() );

        xtrmm( "Left", "Upper", "No Transpose", "Non-unit", sr, s,
           1.0,   Pr.data()     , Pr.row(), 
                  Pa.data() + sl, Pa.row() ); 
        //  printf( "Pr.row() %lu Pa.row() %lu Pa.col() %lu\n",
        //      Pr.row(), Pa.row(), Pa.col() );

        if ( DO_INVERSE )
        {
          if ( 1 )
          {
            xtrsm( "Left", "Lower", "No transpose", "Non-unit", 
                Pa.row(), Pa.col(), 
                1.0,  Z.data(),  Z.row(), 
                     Pa.data(), Pa.row() ); 
          }
          else
          {
            xlaswp( Pa.col(), Pa.data(), Pa.row(), 
                1, Pa.row(), ipiv.data(), 1 );
            xtrsm( "Left", "Lower", "No transpose", "Unit", Pa.row(), Pa.col(), 
                1.0,  Z.data(),  Z.row(), 
                     Pa.data(), Pa.row() ); 
          }
          //printf( "Z.row() %lu Z.col() %lu\n", Z.row(), Z.col() );
        }

      }
      else 
      {
        Pa.resize( nl + nr, s, 0.0 );

        //hmlp::View<T> Xl, Xr;

        //Xa.Partition2x1
        //( 
        //  Xl, 
        //  Xr, nl
        //);

        //assert( Xl.row() == nl );
        //assert( Xr.row() == nr );
        //assert( Xl.col() == s );
        //assert( Xr.col() == s );
        

        xgemm( "N", "T", nl, s, sl, 
            1.0,   Pl.data(), nl, 
                 Palr.data(), s, 
            0.0,   Pa.data(), n );
        xgemm( "N", "T", nr, s, sr, 
            1.0,   Pr.data(), nr, 
                 Palr.data() + s * sl, s, 
            0.0,   Pa.data() + nl, n );

        

        //if ( DO_INVERSE ) Solve<true>( Xl, Xr );
        //printf( "end inner solve from telescope\n" ); fflush( stdout );

        if ( DO_INVERSE )
        {
            Data<T> x( sl + sr, s );
            Data<T> xl( sl, s );
            Data<T> xr( sr, s );

            xgemm( "T", "N", sl, s, nl, 
                1.0, Vl->data(), nl, 
                Pa.data(), n, 
                0.0,  xl.data(), sl );
            xgemm( "T", "N", sr, s, nr, 
                1.0, Vr->data(), nr, 
                Pa.data() + nl, n, 
                0.0,  xr.data(), sr );

            xgemm( "T", "N", sl, s, sr, 
                1.0, Crl.data(), sr, 
                xr.data(), sr, 
                0.0,   x.data(), sl + sr );
            xgemm( "N", "N", sr, s, sl, 
                1.0, Crl.data(), sr, 
                xl.data(), sl, 
                0.0,   x.data() + sl, sl + sr );

            xgetrs( "N", x.row(), x.col(), 
                Z.data(), Z.row(), ipiv.data(), 
                x.data(), x.row() );

            xgemm( "N", "N", nl, s, sl, 
                -1.0, Ul->data(), nl, 
                x.data(), sl + sr, 
                1.0,  Pa.data(), n );
            xgemm( "N", "N", nr, s, sr, 
                -1.0, Ur->data(), nr, 
                x.data() + sl, sl + sr, 
                1.0,  Pa.data() + nl, n );
        } 
      } 
    };

    void Orthogonalization()
    {
      tau.resize( std::min( U.row(), U.col() ) );
      Data<T> work( U.col() * 512, 1 );
      xgeqrf( U.row(), U.col(), U.data(), U.row(),
          tau.data(), work.data(), work.size() );
      Q = U;
      Q.resize( U.row(), U.row() );
      xorgqr( Q.row(), Q.col(), U.col(), Q.data(), Q.row(), tau.data(), 
          work.data(), work.size() );



      Qv.Set( false, Q );
      Qv.Partition1x2( Q1, Q2, tau.size(), LEFT );
      Data<T> C = Q;
      Data<T> D = Q;

      xgemm( "Transpose", "No Transpose", C.row(), C.col(), Q.row(),
          1.0, Q.data(), Q.row(), 
               Q.data(), Q.row(),
          0.0, C.data(), C.row() );

      xgemm( "No Transpose", "Transpose", D.row(), D.col(), Q.row(),
          1.0, Q.data(), Q.row(), 
               Q.data(), Q.row(),
          0.0, D.data(), D.row() );

      for ( size_t j = 0; j < Q.col(); j ++ )
      {
        for ( size_t i = 0; i < Q.row(); i ++ )
        {
          if ( i == j ) assert( std::fabs( C( i, j ) - 1 ) < 1E-5 );
          else          assert( std::fabs( C( i, j ) - 0 ) < 1E-5 );
        }
      }
      for ( size_t j = 0; j < Q.col(); j ++ )
      {
        for ( size_t i = 0; i < Q.row(); i ++ )
        {
          if ( i == j ) assert( std::fabs( D( i, j ) - 1 ) < 1E-5 );
          else          assert( std::fabs( D( i, j ) - 0 ) < 1E-5 );
        }
      }

    };




    void ChangeBasis( SideType side, Data<T> &B )
    {
      if ( !Q.size() ) return;

      Data<T> A = B;

      View<T> Av( false, A );
      View<T> Bv( false, B );
      View<T> Bl, Br, Bt, Bb;
     
      switch ( side )
      {
        case LEFT:
        {
          Bv.Partition2x1( Bt,
                           Bb,     Q2.col(),  TOP );
          //printf("Bt %lux%lu Bb %lux%lu\n", Bt.row(), Bt.col(), 
          //                                  Bb.row(), Bb.col() ); fflush( stdout );

          xgemm( "Transpose", "No Transpose", Bt.row(), Bt.col(), Q2.row(),
              1.0, Q2.data(), Q2.ld(),
                   Av.data(), Av.ld(),
              0.0, Bt.data(), Bt.ld() );
          xgemm( "Transpose", "No Transpose", Bb.row(), Bb.col(), Q1.row(),
              1.0, Q1.data(), Q1.ld(),
                   Av.data(), Av.ld(),
              0.0, Bb.data(), Bb.ld() );
          break;
        }
        case RIGHT:
        {
          Bv.Partition1x2( Bl, Br, Q2.col(), LEFT );

          //printf("Bl %lux%lu Br %lux%lu\n", Bl.row(), Bl.col(), 
          //                                  Br.row(), Br.col() ); fflush( stdout );

          xgemm( "No Transpose", "No Transpose", Bl.row(), Bl.col(), Q2.row(),
              1.0, Av.data(), Av.ld(),
                   Q2.data(), Q2.ld(),
              0.0, Bl.data(), Bl.ld() );
          xgemm( "No Transpose", "No Transpose", Br.row(), Br.col(), Q1.row(),
              1.0, Av.data(), Av.ld(),
                   Q1.data(), Q1.ld(),
              0.0, Br.data(), Br.ld() );
          break;
        }
        default:
        {
          throw "Value of (SideType) side is not recognized.";
        }
      }
      //printf( "end ChangeBasis\n" ); fflush( stdout );
    }; 
    void ChangeBasis( Data<T> &A )
    {
      ChangeBasis(  LEFT, A );
      ChangeBasis( RIGHT, A );
    }; 
    void ULVForward()
    {
      if ( isleaf ) B = bview.toData();
      ChangeBasis( LEFT, B );
      xlaswp( Bf.col(), Bf.data(), Bf.ld(), 1, Bf.row(), ipiv.data(), 1 );
      xtrsm( "Left", "Lower", "No transpose", "Unit", Bf.row(), Bf.col(), 
          1.0, Ztl.data(), Ztl.ld(), Bf.data(), Bf.ld() );
      xgemm( "No Transpose", "No Transpose", Bc.row(), Bc.col(), Bf.row(),
          -1.0, Zbl.data(), Zbl.ld(), Bf.data(), Bf.ld(), 1.0, Bc.data(), Bc.ld() );
      //printf( "Bc %lux%lu Bp %lux%lu\n", Bc.row(), Bc.col(), Bp.row(), Bp.col() ); fflush( stdout );
      Bp.CopyValuesFrom( Bc );
    }; 
    void ULVBackward()
    {
      Bc.CopyValuesFrom( Bp );
      xgemm( "No Transpose", "No Transpose", Bf.row(), Bf.col(), Bc.row(),
          -1.0, Ztr.data(), Ztr.ld(), Bc.data(), Bc.ld(), 1.0, Bf.data(), Bf.ld() );
      xtrsm( "Left", "Upper", "No transpose", "Non-unit", Bf.row(), Bf.col(), 
          1.0, Ztl.data(), Ztl.ld(), Bf.data(), Bf.ld() );
      if ( Q.size() )
      {
        Data<T> A = B;
        xgemm( "No Transpose", "No Transpose", A.row(), A.col(), Bf.row(),
            1.0, Q2.data(), Q2.ld(), Bf.data(), Bf.ld(), 0.0, A.data(), A.row() );
        xgemm( "No Transpose", "No Transpose", A.row(), A.col(), Bc.row(),
            1.0, Q1.data(), Q1.ld(), Bc.data(), Bc.ld(), 1.0, A.data(), A.row() );
        if ( isleaf ) bview.CopyValuesFrom( A );
        else Bv.CopyValuesFrom( A );
      }
    }; 
    bool isleaf = false;

    bool isroot = false;

    size_t n = 0;

    size_t nl = 0;

    size_t nr = 0;

    size_t s = 0;

    size_t sl = 0;

    size_t sr = 0;


    Data<T> Z;
    View<T> Zv;
    View<T> Ztl, Ztr, Zbl, Zbr;

    vector<int> ipiv;

    Data<T> U, V;

    Data<T> Crl, Clr;

    View<T> bview;

    Data<T> *Ul = NULL;
    Data<T> *Ur = NULL;
    Data<T> *Vl = NULL;
    Data<T> *Vr = NULL;

    Data<T> Q;
    View<T> Qv, Q1, Q2;

    vector<T> tau;

    Data<T> B;
    View<T> Bv, Bp, Bsibling, Bf, Bc;

  private: 
    bool issymmetric = true;

    bool do_ulv_factorization = false;

}; 
template<typename NODE, typename T>
void SetupFactor( NODE *node )
{
  size_t n, nl, nr, s, sl, sr;
  bool issymmetric, do_ulv_factorization;
  

#ifdef DEBUG_IGOFMM
  printf( "begin SetupFactor %lu\n", node->treelist_id ); fflush( stdout );
#endif

  issymmetric = node->setup->IsSymmetric();
  do_ulv_factorization = node->setup->do_ulv_factorization;
  n  = node->n;
  nl = 0;
  nr = 0;
  s  = node->data.skels.size();
  sl = 0;
  sr = 0;

  if ( !node->isleaf )
  {
    nl = node->lchild->n;
    nr = node->rchild->n;
    sl = node->lchild->data.skels.size();
    sr = node->rchild->data.skels.size();
  }


  node->data.SetupFactor( issymmetric, do_ulv_factorization,
    node->isleaf, !node->l, n, nl, nr, s, sl, sr );

#ifdef DEBUG_IGOFMM
  printf( "end SetupFactor %lu\n", node->treelist_id ); fflush( stdout );
#endif

}; 
template<typename NODE, typename T>
class SetupFactorTask : public Task
{
  public:

    NODE *arg = NULL;

    void Set( NODE *user_arg )
    {
      arg = user_arg;
      name = string( "sf" );
      label = to_string( arg->treelist_id );
      cost = 1.0;
    };

    void GetEventRecord()
    {
      double flops = 0.0, mops = 0.0;
      event.Set( label + name, flops, mops );
    };

    void DependencyAnalysis()
    {
      arg->DependencyAnalysis( W, this );
      this->TryEnqueue();
    };

    void Execute( Worker* user_worker )
    {
      SetupFactor<NODE, T>( arg );
    };

}; 
template<typename NODE>
void SolverTreeView( NODE *node )
{
  auto &data   = node->data;
  auto *setup  = node->setup;
  auto &input  = *(setup->input);
  auto &output = *(setup->output);
  if ( node->isleaf ) data.B.resize( data.n, input.col() );
  else data.B.resize( data.sl + data.sr, input.col() );

  data.Bv.Set( data.B );
  data.Bv.Partition2x1( data.Bf,
                        data.Bc,  data.s, BOTTOM );

  if ( !node->parent ) data.bview.Set( output );

  if ( !node->isleaf )
  {
    auto &ldata = node->lchild->data;
    auto &rdata = node->rchild->data;
    data.bview.Partition2x1( ldata.bview, 
                             rdata.bview, data.nl, TOP );
    data.Bv.Partition2x1( ldata.Bp,
                          rdata.Bp, data.sl, TOP );
  }
}; 
template<typename NODE>
class SolverTreeViewTask : public Task
{
  public:

    NODE *arg = NULL;

    void Set( NODE *user_arg )
    {
      arg = user_arg;
      name = string( "TreeView" );
      label = to_string( arg->treelist_id );
      cost = 1.0;
    };

    void GetEventRecord()
    {
      double flops = 0.0, mops = 0.0;
      event.Set( label + name, flops, mops );
    };

    void DependencyAnalysis() { arg->DependOnParent( this ); };

    void Execute( Worker* user_worker ) { SolverTreeView( arg ); };

}; 
template<bool FORWARD, typename NODE>
class MatrixPermuteTask : public hmlp::Task
{
  public:

    NODE *arg;

    void Set( NODE *user_arg )
    {
      name = std::string( "MatrixPermutation" );
      arg = user_arg;
      cost = 1.0;
    };

    void GetEventRecord()
    {
      double flops = 0.0, mops = 0.0;
      event.Set( label + name, flops, mops );
    };

    void DependencyAnalysis()
    {
      if ( FORWARD )
      {
        arg->DependencyAnalysis( RW, this );
      }
      else
      {
        this->Enqueue();
      }
    };

    void Execute( Worker* user_worker )
    {
      //printf( "PermuteMatrix %lu\n", arg->treelist_id );
      auto *node   = arg;
      auto &gids   = node->gids;
      auto &input  = *(node->setup->input);
      auto &output = *(node->setup->output);
      auto &A      = node->data.bview;

      assert( A.row() == gids.size() );
      assert( A.col() == input.col() );

      //for ( size_t i = 0; i < gids.size(); i ++ )
      //  printf( "%lu ", gids[ i ] );
      //printf( "\n" );

      for ( size_t j = 0; j < input.col(); j ++ )
        for ( size_t i = 0; i < gids.size(); i ++ )
          if ( FORWARD ) A( i, j ) = input( gids[ i ], j );
          else           input( gids[ i ], j ) = A( i, j );

      //for ( size_t j = 0; j < 1; j ++ )
      //  for ( size_t i = 0; i < gids.size(); i ++ )
      //    printf( "%E ", A( i, j ) );
      //printf( "\n" );

      //printf( "end PermuteMatrix %lu\n", arg->treelist_id );
    };

}; 
template<typename NODE, typename T>
void Apply( NODE *node )
{
  auto &data = node->data;
  auto &setup = node->setup;
  auto &K = *setup->K;

  if ( node->isleaf )
  {
    auto lambda = setup->lambda;
    auto &amap = node->gids;
    auto Kaa = K( amap, amap );
    for ( size_t i = 0; i < Kaa.row(); i ++ ) 
      Kaa[ i * Kaa.row() + i ] += lambda;
  }
  else
  {
    auto &bl = node->lchild->data.bview;
    auto &br = node->rchild->data.bview;
    data.Apply<true>( bl, br );
  }
}; 
//template<typename NODE, typename T>
//void ULVForwardSolve( NODE *node ) { node->data.ULVForward(); };



template<typename NODE, typename T>
class ULVForwardSolveTask : public Task
{
  public:

    NODE *arg = NULL;

    void Set( NODE *user_arg )
    {
      arg = user_arg;
      name = string( "ulvforward" );
      label = to_string( arg->treelist_id );
      cost = 1.0;
    };

    //void DependencyAnalysis()
    //{      
    //  arg->DependencyAnalysis( RW, this );
    //  /** depend on two children */
    //  if ( !arg->isleaf )
    //  {
    //    arg->lchild->DependencyAnalysis( R, this );
    //    arg->rchild->DependencyAnalysis( R, this );
    //  }
    //  /** dispatch the task if there is no dependency */
    //  this->TryEnqueue();
    //};

    void DependencyAnalysis() { arg->DependOnChildren( this ); };


    void Execute( Worker* user_worker ) { arg->data.ULVForward(); };
    
}; 
template<typename NODE, typename T>
class ULVBackwardSolveTask : public Task
{
  public:

    NODE *arg;

    void Set( NODE *user_arg )
    {
      arg = user_arg;
      name = string( "ulvbackward" );
      label = std::to_string( arg->treelist_id );
      cost = 1.0;

      //printf( "Set treelist_id %lu\n", arg->treelist_id ); fflush( stdout );
    };

    //void DependencyAnalysis()
    //{
    //  /** depend on parent */
    //  if ( arg->parent )
    //    arg->parent->DependencyAnalysis( hmlp::ReadWriteType::R, this );
    //  arg->DependencyAnalysis( hmlp::ReadWriteType::RW, this );
    //  /** dispatch the task if there is no dependency */
    //  this->TryEnqueue();
    //};


    void DependencyAnalysis() { arg->DependOnParent( this ); };

    void Execute( Worker* user_worker ) { arg->data.ULVBackward(); };
    
}; 
template<typename NODE, typename T>
void Solve( NODE *node )
{
  
  auto &data = node->data;
  auto &setup = node->setup;
  auto &K = *setup->K;


  //printf( "%lu beg Solve\n", node->treelist_id ); fflush( stdout );

  if ( node->isleaf )
  {
    auto &b = data.bview;
    data.Solve( b );
    //printf( "Solve %lu, m %lu n %lu\n", node->treelist_id, b.row(), b.col() );
  }
  else
  {
    auto &bl = node->lchild->data.bview;
    auto &br = node->rchild->data.bview;
    data.Solve( bl, br );
    //printf( "Solve %lu, m %lu n %lu\n", node->treelist_id, bl.row(), bl.col() );
  }

  //printf( "%lu end Solve\n", node->treelist_id ); fflush( stdout );

}; 
template<typename NODE, typename T>
class SolveTask : public Task
{
  public:

    NODE *arg = NULL;

    void Set( NODE *user_arg )
    {
      arg = user_arg;
      name = string( "sl" );
      label = to_string( arg->treelist_id );
      cost = 1.0;

      //printf( "Set treelist_id %lu\n", arg->treelist_id ); fflush( stdout );
    };

    void GetEventRecord()
    {
      double flops = 0.0, mops = 0.0;
      event.Set( label + name, flops, mops );
    };

    void DependencyAnalysis()
    {
      arg->DependencyAnalysis( RW, this );
      if ( !arg->isleaf )
      {
        arg->lchild->DependencyAnalysis( R, this );
        arg->rchild->DependencyAnalysis( R, this );
      }
    };

    void Execute( Worker* user_worker )
    {
      Solve<NODE, T>( arg );
    };

}; 
template<typename T, typename TREE>
void Solve( TREE &tree, Data<T> &input )
{
  using NODE = typename TREE::NODE;

  const bool AUTO_DEPENDENCY = true;
  const bool USE_RUNTIME     = true;

  auto *output = new Data<T>( input.row(), input.col() );

  SolverTreeViewTask<NODE>             treeviewtask;
  MatrixPermuteTask<true,  NODE> forwardpermutetask;
  MatrixPermuteTask<false, NODE> inversepermutetask;
  SolveTask<NODE, T>      solvetask1;
  ULVForwardSolveTask<NODE, T>   ulvforwardsolvetask;
  ULVBackwardSolveTask<NODE, T>  ulvbackwardsolvetask;

  tree.setup.input  = &input;
  tree.setup.output = output;

  if ( tree.setup.do_ulv_factorization )
  {
    tree.DependencyCleanUp();
    tree.TraverseDown( treeviewtask );
    tree.TraverseLeafs( forwardpermutetask );
    tree.TraverseUp( ulvforwardsolvetask );
    tree.TraverseDown( ulvbackwardsolvetask );
    if ( USE_RUNTIME ) hmlp_run();

    tree.DependencyCleanUp();
    tree.TraverseLeafs( inversepermutetask );
    if ( USE_RUNTIME ) hmlp_run();
  }
  else
  {
    tree.DependencyCleanUp();
    tree.TraverseDown( treeviewtask );
    tree.TraverseLeafs( forwardpermutetask );
    tree.TraverseUp( solvetask1 );
    if ( USE_RUNTIME ) hmlp_run();
    tree.DependencyCleanUp();
    tree.TraverseLeafs( inversepermutetask );
    if ( USE_RUNTIME ) hmlp_run();
  }

  delete output;

}; 
template<typename NODE, typename T>
void LowRankError( NODE *node )
{
  auto &data = node->data;
  auto &setup = node->setup;
  auto &K = *setup->K;

  if ( !node->isleaf )
  {
    auto Krl = K( node->rchild->gids, node->lchild->gids );

    auto nrm2 = hmlp_norm( Krl.row(),  Krl.col(), 
                           Krl.data(), Krl.row() ); 


    hmlp::Data<T> VrCrl( data.nr, data.sl );

    xgemm( "N", "N", data.nr, data.sl, data.sr,
        1.0, data.Vr->data(), data.nr,
             data.Crl.data(), data.sr,
        0.0, VrCrl.data(), data.nr );

    xgemm( "N", "T", data.nr, data.nl, data.sl,
       -1.0, VrCrl.data(), data.nr,
             data.Vl->data(), data.nl,
        1.0, Krl.data(), data.nr );

    auto err = hmlp_norm( Krl.row(),  Krl.col(), 
                          Krl.data(), Krl.row() ); 

    printf( "%4lu ||Krl -VrCrlVl|| %3.1E\n", 
        node->treelist_id, std::sqrt( err / nrm2 ) );
  }

}; 
template<typename NODE, typename T>
void Factorize( NODE *node )
{
  auto &data = node->data;
  auto &setup = node->setup;
  auto &K = *setup->K;
  auto &proj = data.proj;

  auto do_ulv_factorization = setup->do_ulv_factorization;

  if ( node->isleaf )
  {
    auto lambda = setup->lambda;
    auto &amap = node->gids;

    Data<T> Kaa = K( amap, amap );

    for ( size_t i = 0; i < Kaa.row(); i ++ ) Kaa( i, i ) += lambda;

    if ( do_ulv_factorization )
    {
      data.Telescope( false, data.U, proj );
      data.Orthogonalization();
      data.PartialFactorize( Kaa );
    }
    else
    {
      data.Factorize( Kaa );
      data.Telescope( true, data.U, proj );
      data.Telescope( false, data.V, proj );
    }
  }
  else
  {
    auto &Ul = node->lchild->data.U;
    auto &Vl = node->lchild->data.V;
    auto &Zl = node->lchild->data.Zbr;
    auto &Ur = node->rchild->data.U;
    auto &Vr = node->rchild->data.V;
    auto &Zr = node->rchild->data.Zbr;

    auto &amap = node->lchild->data.skels;
    auto &bmap = node->rchild->data.skels;

    node->data.Crl = K( bmap, amap );

    if ( do_ulv_factorization )
    {
      if ( !node->data.isroot )
      {
        data.Telescope( false, data.U, proj, Ul, Ur );
        data.Orthogonalization();
      }
      data.PartialFactorize( Zl, Zr, Ul, Ur, Vl, Vr );
    }
    else
    {
      data.Factorize( Ul, Ur, Vl, Vr );
      if ( !node->data.isroot )
      {
        data.Telescope(  true, data.U, proj, Ul, Ur );
        data.Telescope( false, data.V, proj, Vl, Vr );
      }
    }
  }








//    /** SMW factorization (LU or Cholesky) */
//    data.Factorize<true>( Ul, Ur, Vl, Vr );
//
//    /** telescope U and V */
//    if ( !node->data.isroot )
//    {
//      if ( do_ulv_factorization )
//      {
//        data.Telescope( true, data.U, proj, Ul, Ur );
//        data.Orthogonalization();
//      }
//      else
//      {
//        /** U = inv( I + UCV' ) * [ Ul; Ur ] * proj' */
//        data.Telescope( true, data.U, proj, Ul, Ur );
//        /** V = [ Vl; Vr ] * proj' */
//        data.Telescope( false, data.V, proj, Vl, Vr );
//      }
//    }
//    else
//    {
//      /** output Crl from children */
//      
//      //size_t L = 3;
//
//      auto *cl = node->lchild;
//      auto *cr = node->rchild;
//      auto *c1 = cl->lchild;
//      auto *c2 = cl->rchild;
//      auto *c3 = cr->lchild;
//      auto *c4 = cr->rchild;
//
//      //hmlp::Data<T> C21 = K( c2->data.skels, c1->data.skels );
//      //hmlp::Data<T> C31 = K( c3->data.skels, c1->data.skels );
//      //hmlp::Data<T> C41 = K( c4->data.skels, c1->data.skels );
//      //hmlp::Data<T> C32 = K( c3->data.skels, c2->data.skels );
//      //hmlp::Data<T> C42 = K( c4->data.skels, c2->data.skels );
//      //hmlp::Data<T> C43 = K( c4->data.skels, c3->data.skels );
//
//      //C21.WriteFile( "C21.m" );
//      //C31.WriteFile( "C31.m" );
//      //C41.WriteFile( "C41.m" );
//      //C32.WriteFile( "C32.m" );
//      //C42.WriteFile( "C42.m" );
//      //C43.WriteFile( "C43.m" );
//
//
//      //hmlp::Data<T> V11( c1->data.V.col(), c1->data.V.col() );
//      //hmlp::Data<T> V22( c2->data.V.col(), c2->data.V.col() );
//      //hmlp::Data<T> V33( c3->data.V.col(), c3->data.V.col() );
//      //hmlp::Data<T> V44( c4->data.V.col(), c4->data.V.col() );
//
//      //xgemm( "T", "N", c1->data.V.col(), c1->data.V.col(), c1->data.V.row(),
//      //    1.0, c1->data.V.data(), c1->data.V.row(),
//      //         c1->data.V.data(), c1->data.V.row(), 
//      //    0.0,        V11.data(), V11.row() );
//
//      //xgemm( "T", "N", c2->data.V.col(), c2->data.V.col(), c2->data.V.row(),
//      //    1.0, c2->data.V.data(), c2->data.V.row(),
//      //         c2->data.V.data(), c2->data.V.row(), 
//      //    0.0,        V22.data(), V22.row() );
//
//      //xgemm( "T", "N", c3->data.V.col(), c3->data.V.col(), c3->data.V.row(),
//      //    1.0, c3->data.V.data(), c3->data.V.row(),
//      //         c3->data.V.data(), c3->data.V.row(), 
//      //    0.0,        V33.data(), V33.row() );
//
//      //xgemm( "T", "N", c4->data.V.col(), c4->data.V.col(), c4->data.V.row(),
//      //    1.0, c4->data.V.data(), c4->data.V.row(),
//      //         c4->data.V.data(), c4->data.V.row(), 
//      //    0.0,        V44.data(), V44.row() );
//
//      //V11.WriteFile( "V11.m" );
//      //V22.WriteFile( "V22.m" );
//      //V33.WriteFile( "V33.m" );
//      //V44.WriteFile( "V44.m" );
//    }
//    //printf( "end inner forward telescoping\n" ); fflush( stdout );
//
//    /** check the offdiagonal block VrCrlVl' accuracy */
//    if ( !do_ulv_factorization ) 
//      LowRankError<NODE, T>( node );
//  }

}; 
template<typename NODE, typename T>
class FactorizeTask : public Task
{
  public:

    NODE *arg = NULL;

    void Set( NODE *user_arg )
    {
      arg = user_arg;
      name = string( "fa" );
      label = to_string( arg->treelist_id );
      // Need an accurate cost model.
      cost = 1.0;
    };

    void GetEventRecord()
    {
      double flops = 0.0, mops = 0.0;
      event.Set( label + name, flops, mops );
    };

    void DependencyAnalysis() { arg->DependOnChildren( this ); };

    void Execute( Worker* user_worker ) { Factorize<NODE, T>( arg ); };

}; 
template<typename T, typename TREE>
void Factorize( TREE &tree, T lambda )
{
  using NODE = typename TREE::NODE;

  tree.DependencyCleanUp();

  tree.setup.lambda = lambda;

  tree.setup.do_ulv_factorization = true;

  SetupFactorTask<NODE, T> setupfactortask; 
  tree.TraverseUp( setupfactortask );
  tree.ExecuteAllTasks();

  FactorizeTask<NODE, T> factorizetask; 
  tree.TraverseUp( factorizetask );
  tree.ExecuteAllTasks();

}; 
template<typename TREE, typename T>
void ComputeError( TREE &tree, T lambda, Data<T> weights, Data<T> potentials )
{
  using NODE = typename TREE::NODE;
  

  assert( weights.row() == potentials.row() );
  assert( weights.col() == potentials.col() );

  size_t n    = weights.row();
  size_t nrhs = weights.col();

  Data<T> rhs( n, nrhs );
  for ( size_t j = 0; j < nrhs; j ++ )
    for ( size_t i = 0; i < n; i ++ )
      rhs( i, j ) = potentials( i, j ) + lambda * weights( i, j );

  Solve( tree, rhs );


  printf( "========================================================\n" );
  printf( "Inverse accuracy report\n" );
  printf( "========================================================\n" );
  printf( "#rhs,  max err,        @,  min err,        @,  relative \n" );
  printf( "========================================================\n" );
  size_t ntest = 10;
  T total_err  = 0.0;
  for ( size_t j = 0; j < std::min( nrhs, ntest ); j ++ )
  {
    T nrm2 = 0.0, err2 = 0.0;
    T max2 = 0.0, min2 = std::numeric_limits<T>::max(); 
    size_t maxi = 0, mini = 0;

    for ( size_t i = 0; i < n; i ++ )
    {
      T sse = rhs( i, j ) - weights( i, j );
      assert( rhs( i, j ) == rhs( i, j ) );
      sse = sse * sse;

      nrm2 += weights( i, j ) * weights( i, j );
      err2 += sse;

      //printf( "%lu %3.1E\n", i, sse );


      if ( sse > max2 ) { max2 = sse; maxi = i; }
      if ( sse < min2 ) { min2 = sse; mini = i; }
    }
    total_err += std::sqrt( err2 / nrm2 );

    printf( "%4lu,  %3.1E,  %7lu,  %3.1E,  %7lu,   %3.1E\n", 
        j, std::sqrt( max2 ), maxi, std::sqrt( min2 ), mini, 
        std::sqrt( err2 / nrm2 ) );
  }
  printf( "========================================================\n" );
  printf( "                             avg over %2lu rhs,   %3.1E \n",
      std::min( nrhs, ntest ), total_err / std::min( nrhs, ntest ) );
  printf( "========================================================\n\n" );

}; 
}; 
}; 
#endif