/*
 * Copyright 2020 IFPEN-CEA
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 * SPDX-License-Identifier: Apache-2.0
 */

/*!
 * \file NormalizeOpt.cc
 * \brief NormalizeOpt.cc
 */

#include "NormalizeOpt.h"

#include <alien/core/impl/MultiVectorImpl.h>
#include <alien/kernels/simple_csr/SimpleCSRMatrix.h>
#include <alien/kernels/simple_csr/SimpleCSRVector.h>

#ifdef ALIEN_USE_EIGEN2
#include <Eigen/Core>
#include <Eigen/LU>
#include <iostream>

#endif

/*---------------------------------------------------------------------------*/
/*---------------------------------------------------------------------------*/

namespace Alien
{

using namespace Arccore;

/*---------------------------------------------------------------------------*/
/*---------------------------------------------------------------------------*/

NormalizeOpt::NormalizeOpt()
: m_algo(StdLU)
, m_sum_first_eq(false)
{}

/*---------------------------------------------------------------------------*/
/*---------------------------------------------------------------------------*/

void NormalizeOpt::setAlgo(eAlgoType algo)
{
  m_algo = algo;
}

/*---------------------------------------------------------------------------*/
/*---------------------------------------------------------------------------*/

void NormalizeOpt::setOpt(eOptType opt, bool flag)
{
  switch (opt) {
  case SumFirstEq:
    m_sum_first_eq = flag;
    break;
  default:
    FatalErrorException(A_FUNCINFO, String::format("Unhandle option type: ", opt));
    break;
  }
}

/*---------------------------------------------------------------------------*/
/*---------------------------------------------------------------------------*/

NormalizeOpt::eErrorType
NormalizeOpt::normalize(IMatrix& m, IVector& x) const
{
  MatrixImpl& A = m.impl()->get<Alien::BackEnd::tag::simplecsr>(true);
  VectorImpl& b = x.impl()->get<Alien::BackEnd::tag::simplecsr>(true);
  return _normalize(A, b);
}

/*---------------------------------------------------------------------------*/
/*---------------------------------------------------------------------------*/

NormalizeOpt::eErrorType
NormalizeOpt::normalize(
CompositeMatrix& m, CompositeVector& x, ConstArrayView<Integer> eq_ids) const
{
  // need to update timestamp of all submatrices
  MatrixImpl& A00 = m(0, 0).impl()->get<Alien::BackEnd::tag::simplecsr>(true);
  MatrixImpl& A01 = m(0, 1).impl()->get<Alien::BackEnd::tag::simplecsr>(true);
  MatrixImpl& A10 = m(1, 0).impl()->get<Alien::BackEnd::tag::simplecsr>(true);
  MatrixImpl& A11 = m(1, 1).impl()->get<Alien::BackEnd::tag::simplecsr>(true);
  VectorImpl& b0 = x[0].impl()->get<Alien::BackEnd::tag::simplecsr>(true);
  VectorImpl& b1 = x[1].impl()->get<Alien::BackEnd::tag::simplecsr>(true);
  {
    eErrorType error =
    _normalize(A00, A01, m(0, 1).impl()->hasFeature("transposed"), eq_ids, b0);
    if (error != NoError)
      return error;
  }
  {
    eErrorType error =
    _normalize(A11, eq_ids, A10, m(1, 0).impl()->hasFeature("transposed"), b1);
    return error;
  }
}

/*---------------------------------------------------------------------------*/
/*---------------------------------------------------------------------------*/

template <>
void NormalizeOpt::Op::multInvDiag<0>()
{}

/*---------------------------------------------------------------------------*/
/*---------------------------------------------------------------------------*/

template <>
void NormalizeOpt::Op::multInvDiag<1>()
{
  for (Integer irow = 0; irow < m_local_size; ++irow) {
    Integer off = m_row_offset[irow];
    Real diag = m_matrix[off];
    for (Integer col = off; col < m_row_offset[irow + 1]; ++col) {
      m_matrix[col] /= diag;
    }
    m_rhs[irow] /= diag;

    if (!m_keep_diag)
      m_matrix[off] = 0.;
  }
}

/*---------------------------------------------------------------------------*/
/*---------------------------------------------------------------------------*/

template <Integer N>
void NormalizeOpt::Op::multInvDiag()
{
#ifdef ALIEN_USE_EIGEN2
  if (m_algo == EigenLU) {
    using namespace Eigen;
    using namespace std;
    typedef Eigen::Matrix<Real, N, N, RowMajor> MatrixType;
    typedef Eigen::Matrix<Real, N, 1> VectorType;
    const Integer NxN = N * N;
    // TOCHECK : to be removed ?
    // cout<<"MULT INV DIAG B"<<m_equations_num<<endl;
    UniqueArray<Real> block(NxN);
    UniqueArray<Real> vect(N);
    Map<MatrixType> m(block.begin(), N, N);
    Map<VectorType> v(vect.begin(), N);
    if (m_diag_first) {
      for (Integer irow = 0; irow < m_local_size; ++irow) {
        Integer off = m_row_offset[irow];

        Map<MatrixType> diag(&m_matrix[off * NxN], N, N);
        if (diag.determinant() == 0) {
          cout << "Non inversible diagonal : row=" << irow << endl;
          cout << " DIAG :" << endl;
          cout << diag << endl;
        }
        MatrixType inv_diag = diag.inverse();
        // TOCHECK : to be removed ?
        // PartialPivLU< MatrixType > lu(diag);
        // MatrixType inv_diag = lu.inverse() ;

        // OFF DIAGONAL treatment
        for (Integer col = off + 1; col < m_row_offset[irow + 1]; ++col) {
          block.copy(ArrayView<Real>(NxN, &m_matrix[col * NxN]));
          Map<MatrixType> matrix(&m_matrix[col * NxN], N, N);
          matrix = inv_diag * m;
          // TOCHECK : to be removed ?
          // matrix = lu.solve(m) ;
        }

        // RHS treatment
        vect.copy(ArrayView<Real>(N, &m_rhs[irow * N]));
        Map<VectorType> rhs(&m_rhs[irow * N], N);
        rhs = inv_diag * v;
        // TOCHECK : to be removed
        // rhs = lu.solve(v) ;

        if (m_keep_diag)
          diag.setIdentity();
        else
          diag.setZero();
      }
    }
    else {
      for (Integer irow = 0; irow < m_local_size; ++irow) {
        Integer off = m_row_offset[irow] + m_upper_diag_index[irow];

        Map<MatrixType> diag(&m_matrix[off * NxN], N, N);
        if (diag.determinant() == 0) {
          cout << "Non inversible diagonal : row=" << irow << endl;
          cout << " DIAG :" << endl;
          cout << diag << endl;
        }
        MatrixType inv_diag = diag.inverse();
        // TOCHECK : to be removed ?
        // PartialPivLU< MatrixType > lu(diag);
        // MatrixType inv_diag = lu.inverse() ;

        // OFF DIAGONAL treatment
        for (Integer col = m_row_offset[irow]; col < off; ++col) {
          block.copy(ArrayView<Real>(NxN, &m_matrix[col * NxN]));
          Map<MatrixType> matrix(&m_matrix[col * NxN], N, N);
          matrix = inv_diag * m;
          // TOCHECK : to be removed ?
          // matrix = lu.solve(m) ;
        }
        // skip diagonal block
        for (Integer col = off + 1; col < m_row_offset[irow + 1]; ++col) {
          block.copy(ArrayView<Real>(NxN, &m_matrix[col * NxN]));
          Map<MatrixType> matrix(&m_matrix[col * NxN], N, N);
          matrix = inv_diag * m;
          // TOCHECK : to be removed ?
          // matrix = lu.solve(m) ;
        }

        // RHS treatment
        vect.copy(ArrayView<Real>(N, &m_rhs[irow * N]));
        Map<VectorType> rhs(&m_rhs[irow * N], N);
        rhs = inv_diag * v;
        // TOCHECK : to be removed ?
        // rhs = lu.solve(v) ;

        if (m_keep_diag)
          diag.setIdentity();
        else
          diag.setZero();
      }
    }
  }
  else
#endif
  {
    if (m_diag_first) {
      for (Integer irow = 0; irow < m_local_size; ++irow) {
        Integer off = m_row_offset[irow];

        Real* diag = &m_matrix[off * N * N];
        LU<N> lu(diag);

        // OFF DIAGONAL treatment
        for (Integer col = off + 1; col < m_row_offset[irow + 1]; ++col) {
          lu.template solve<N, false>(&m_matrix[col * N * N]);
        }

        // RHS treatment
        lu.template solve<1, true>(&m_rhs[irow * N]);

        if (m_keep_diag)
          lu.setIdentity();
        else
          lu.setZero();
      }
    }
    else {
      for (Integer irow = 0; irow < m_local_size; ++irow) {
        Integer off = m_row_offset[irow] + m_upper_diag_index[irow];

        Real* diag = &m_matrix[off * N * N];
        LU<N> lu(diag);

        // OFF DIAGONAL treatment
        for (Integer col = m_row_offset[irow]; col < off; ++col) {
          lu.template solve<N, false>(&m_matrix[col * N * N]);
        }
        // skip diagonal block
        for (Integer col = off + 1; col < m_row_offset[irow + 1]; ++col) {
          lu.template solve<N, false>(&m_matrix[col * N * N]);
        }
        // RHS treatment
        lu.template solve<1, true>(&m_rhs[irow * N]);

        if (m_keep_diag)
          lu.setIdentity();
        else
          lu.setZero();
      }
    }
  }
}

/*---------------------------------------------------------------------------*/
/*---------------------------------------------------------------------------*/

NormalizeOpt::eErrorType
NormalizeOpt::_normalize(MatrixImpl& m, VectorImpl& x) const
{
  Op op(m, x, m_algo, m_sum_first_eq);
  if (m.block()) {
    switch (m.block()->size()) {
    case 1:
      op.multInvDiag<1>();
      break;
    case 2:
      op.multInvDiag<2>();
      break;
    case 3:
      op.multInvDiag<3>();
      break;
    case 4:
      op.multInvDiag<4>();
      break;
    default:
      op.multInvDiag<0>();
      break;
    }
  }
  return NoError;
}

/*---------------------------------------------------------------------------*/
/*---------------------------------------------------------------------------*/

template <>
void NormalizeOpt::Op2::multInvDiag<0>()
{}

/*---------------------------------------------------------------------------*/
/*---------------------------------------------------------------------------*/

template <>
void NormalizeOpt::Op2::multInvDiag<1>()
{
  if (m_submatrix2) {
    if (m_diag_first) {
      ////////////////////////////////////////////
      //
      // NORMALIZE SubMatrix00 and RHS
      for (Integer irow = 0; irow < m_local_size; ++irow) {
        Integer off = m_row_offset[irow];
        Real diag = m_matrix[off];
        for (Integer col = off + 1; col < m_row_offset[irow + 1]; ++col) {
          m_matrix[col] /= diag;
        }
        m_rhs[irow] /= diag;
      }

      ////////////////////////////////////////////
      //
      // NORMALIZE SubMatrix01
      if (m_trans)
        for (Integer i = 0; i < m_eq_ids.size(); ++i) {
          Integer ieq = m_eq_ids[i];
          for (Integer k = m_extra_eq_row_offset[ieq]; k < m_extra_eq_row_offset[ieq + 1];
               ++k) {
            Integer irow = m_extra_eq_cols[k];
            Integer off = m_row_offset[irow];
            Real diag = m_matrix[off];
            m_extra_eq_matrix[k] /= diag;
          }
        }
      else
        for (Integer i = 0; i < m_eq_ids.size(); ++i) {
          Integer irow = m_eq_ids[i];
          Integer off = m_row_offset[irow];
          Real diag = m_matrix[off];
          for (Integer k = m_extra_eq_row_offset[irow];
               k < m_extra_eq_row_offset[irow + 1]; ++k) {
            m_extra_eq_matrix[k] /= diag;
          }
        }
      ////////////////////////////////////////////
      //
      // SET DIAG TO Id
      for (Integer irow = 0; irow < m_local_size; ++irow) {
        Integer off_diag = m_row_offset[irow];
        m_matrix[off_diag] = m_keep_diag ? 1. : 0.;
      }
    }
    else {
      ////////////////////////////////////////////
      //
      // NORMALIZE SubMatrix00 and RHS
      for (Integer irow = 0; irow < m_local_size; ++irow) {
        Integer off_diag = m_row_offset[irow] + m_upper_diag_index[irow];
        Real diag = m_matrix[off_diag];
        for (Integer col = m_row_offset[irow]; col < off_diag; ++col) {
          m_matrix[col] /= diag;
        }
        for (Integer col = off_diag + 1; col < m_row_offset[irow + 1]; ++col) {
          m_matrix[col] /= diag;
        }
        m_rhs[irow] /= diag;
      }

      ////////////////////////////////////////////
      //
      // NORMALIZE SubMatrix01
      if (m_trans)
        for (Integer i = 0; i < m_eq_ids.size(); ++i) {
          Integer ieq = m_eq_ids[i];
          for (Integer k = m_extra_eq_row_offset[ieq]; k < m_extra_eq_row_offset[ieq + 1];
               ++k) {
            Integer irow = m_extra_eq_cols[k];
            Integer off = m_row_offset[irow] + m_upper_diag_index[irow];
            Real diag = m_matrix[off];
            m_extra_eq_matrix[k] /= diag;
          }
        }
      else
        for (Integer i = 0; i < m_eq_ids.size(); ++i) {
          Integer irow = m_eq_ids[i];
          Integer off = m_row_offset[irow] + m_upper_diag_index[irow];
          Real diag = m_matrix[off];
          for (Integer k = m_extra_eq_row_offset[irow];
               k < m_extra_eq_row_offset[irow + 1]; ++k) {
            m_extra_eq_matrix[k] /= diag;
          }
        }
      ////////////////////////////////////////////
      //
      // SET DIAG TO Id
      for (Integer irow = 0; irow < m_local_size; ++irow) {
        Integer off_diag = m_row_offset[irow] + m_upper_diag_index[irow];
        m_matrix[off_diag] = m_keep_diag ? 1. : 0.;
      }
    }
  }

  if (m_submatrix1) {
    ////////////////////////////////////////////
    //
    // NORMALIZE SubMatrix10 SubMatrix11 RHS
    for (Integer i = 0; i < m_eq_ids.size(); ++i) {
      Integer ieq = m_eq_ids[i];
      Real diag = m_matrix[ieq];
      for (Integer j = m_extra_eq_row_offset[ieq]; j < m_extra_eq_row_offset[ieq + 1];
           ++j) {
        for (Integer k = 0; k < m_nuk2; ++k)
          m_extra_eq_matrix[j * m_nuk2 + k] /= diag;
      }
      m_rhs[ieq] /= diag;
      m_matrix[ieq] = 1.;
    }
  }
}

/*---------------------------------------------------------------------------*/
/*---------------------------------------------------------------------------*/

template <Integer N>
void NormalizeOpt::Op2::multInvDiag()
{
  if (m_submatrix2) {
    if (m_diag_first) {
      ////////////////////////////////////////////
      //
      // NORMALIZE SubMatrix00
      for (Integer irow = 0; irow < m_local_size; ++irow) {
        Integer off = m_row_offset[irow];

        Real* diag = &m_matrix[off * N * N];
        LU<N> lu(diag);

        // OFF DIAGONAL treatment
        for (Integer col = off + 1; col < m_row_offset[irow + 1]; ++col) {
          lu.template solve<N, false>(&m_matrix[col * N * N]);
        }

        // RHS treatment
        lu.template solve<1, true>(&m_rhs[irow * N]);
      }

      ////////////////////////////////////////////
      //
      // NORMALIZE SubMatrix01
      if (m_trans)
        for (Integer i = 0; i < m_eq_ids.size(); ++i) {
          Integer ieq = m_eq_ids[i];
          for (Integer k = m_extra_eq_row_offset[ieq]; k < m_extra_eq_row_offset[ieq + 1];
               ++k) {
            Integer irow = m_extra_eq_cols[k];
            Integer off = m_row_offset[irow];
            Real* diag = &m_matrix[off * N * N];
            LU<N> lu(diag, false);

            lu.template solve<1, true>(&m_extra_eq_matrix[k * N]);
          }
        }
      else
        for (Integer i = 0; i < m_eq_ids.size(); ++i) {
          Integer irow = m_eq_ids[i];
          Integer off = m_row_offset[irow];
          Real* diag = &m_matrix[off * N * N];
          LU<N> lu(diag, false);
          for (Integer k = m_extra_eq_row_offset[irow];
               k < m_extra_eq_row_offset[irow + 1]; ++k) {
            lu.template solve<N, false>(&m_extra_eq_matrix[k * N]);
          }
        }

      ////////////////////////////////////////////
      //
      // SET DIAG TO Id
      if (m_keep_diag)
        for (Integer irow = 0; irow < m_local_size; ++irow) {
          Integer off = m_row_offset[irow];
          Real* diag = &m_matrix[off * N * N];
          LU<N> lu(diag, false);

          lu.setIdentity();
        }
      else
        for (Integer irow = 0; irow < m_local_size; ++irow) {
          Integer off = m_row_offset[irow];
          Real* diag = &m_matrix[off * N * N];
          LU<N> lu(diag, false);
          lu.setZero();
        }
    }
    else {
      ////////////////////////////////////////////
      //
      // NORMALIZE SubMatrix01
      for (Integer irow = 0; irow < m_local_size; ++irow) {
        Integer off = m_row_offset[irow] + m_upper_diag_index[irow];

        Real* diag = &m_matrix[off * N * N];
        LU<N> lu(diag);

        // OFF DIAGONAL treatment
        for (Integer col = m_row_offset[irow]; col < off; ++col) {
          lu.template solve<N, false>(&m_matrix[col * N * N]);
        }
        // skip diagonal block
        for (Integer col = off + 1; col < m_row_offset[irow + 1]; ++col) {
          lu.template solve<N, false>(&m_matrix[col * N * N]);
        }
        // RHS treatment
        lu.template solve<1, true>(&m_rhs[irow * N]);
      }
      ////////////////////////////////////////////
      //
      // NORMALIZE SubMatrix01
      if (m_trans)
        for (Integer i = 0; i < m_eq_ids.size(); ++i) {
          Integer ieq = m_eq_ids[i];
          for (Integer k = m_extra_eq_row_offset[ieq]; k < m_extra_eq_row_offset[ieq + 1];
               ++k) {
            Integer irow = m_extra_eq_cols[k] - m_local_offset;

            Integer off = m_row_offset[irow] + m_upper_diag_index[irow];
            Real* diag = &m_matrix[off * N * N];
            LU<N> lu(diag, false);

            lu.template solve<1, true>(&m_extra_eq_matrix[k * N]);
          }
        }
      else {
        for (Integer i = 0; i < m_eq_ids.size(); ++i) {
          Integer irow = m_eq_ids[i];
          Integer off = m_row_offset[irow] + m_upper_diag_index[irow];
          Real* diag = &m_matrix[off * N * N];
          LU<N> lu(diag, false);
          for (Integer k = m_extra_eq_row_offset[irow];
               k < m_extra_eq_row_offset[irow + 1]; ++k) {
            lu.template solve<N, false>(&m_extra_eq_matrix[k * N]);
          }
        }
      }

      ////////////////////////////////////////////
      //
      // SET DIAG TO Id
      if (m_keep_diag)
        for (Integer irow = 0; irow < m_local_size; ++irow) {
          Integer off = m_row_offset[irow] + m_upper_diag_index[irow];
          Real* diag = &m_matrix[off * N * N];
          LU<N> lu(diag, false);

          lu.setIdentity();
        }
      else
        for (Integer irow = 0; irow < m_local_size; ++irow) {
          Integer off = m_row_offset[irow] + m_upper_diag_index[irow];
          Real* diag = &m_matrix[off * N * N];
          LU<N> lu(diag, false);
          lu.setZero();
        }
    }
  }

  if (m_submatrix1) {
    ////////////////////////////////////////////
    //
    // NORMALIZE SubMatrix10 SubMatrix11 RHS
    for (Integer i = 0; i < m_eq_ids.size(); ++i) {
      Integer ieq = m_eq_ids[i];
      Real diag = m_matrix[ieq];
      for (Integer j = m_extra_eq_row_offset[ieq]; j < m_extra_eq_row_offset[ieq + 1];
           ++j) {
        for (Integer k = 0; k < N; ++k)
          m_extra_eq_matrix[j * N + k] /= diag;
      }
      m_rhs[ieq] /= diag;
      m_matrix[ieq] = 1.;
    }
  }
}

/*---------------------------------------------------------------------------*/
/*---------------------------------------------------------------------------*/

NormalizeOpt::eErrorType
NormalizeOpt::_normalize(MatrixImpl& m, MatrixImpl& m2, bool trans,
                         ConstArrayView<Integer> eq_ids, VectorImpl& x) const
{
  Op2 op(m, m2, trans, eq_ids, x, m_algo, m_sum_first_eq);

  switch (x.block()->size()) {
  case 1:
    op.multInvDiag<1>();
    break;
  case 2:
    op.multInvDiag<2>();
    break;
  case 3:
    op.multInvDiag<3>();
    break;
  case 4:
    op.multInvDiag<4>();
    break;
  default:
    op.multInvDiag<0>();
    break;
  }
  return NoError;
}

/*---------------------------------------------------------------------------*/
/*---------------------------------------------------------------------------*/

NormalizeOpt::eErrorType
NormalizeOpt::_normalize(MatrixImpl& m, ConstArrayView<Integer> eq_ids, MatrixImpl& m2,
                         bool trans, VectorImpl& x) const
{
  Op2 op(m, eq_ids, m2, trans, x, m_algo, m_sum_first_eq);

  switch (x.block()->size()) {
  case 1:
    op.multInvDiag<1>();
    break;
  case 2:
    op.multInvDiag<2>();
    break;
  case 3:
    op.multInvDiag<3>();
    break;
  case 4:
    op.multInvDiag<4>();
    break;
  default:
    op.multInvDiag<0>();
    break;
  }
  return NoError;
}

/*---------------------------------------------------------------------------*/
/*---------------------------------------------------------------------------*/

} // namespace Alien

/*---------------------------------------------------------------------------*/
/*---------------------------------------------------------------------------*/


/*
 *            Copyright 2016 The MUSCET Development Team
 *
 *      Licensed under the Apache License, Version 2.0 (the "License")
 *
 * You may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *              http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */

#include <votca/xtp/gyration.h>
#include <boost/format.hpp>
#include <votca/xtp/numerical_integrations.h>
#include <votca/xtp/orbitals.h>
//#include <votca/xtp/units.h>
#include <votca/tools/linalg.h>

namespace votca { namespace xtp {

void Density2Gyration::Initialize(Property* options) {
    string key = Identify();

    _state    = options->get(key + ".state").as<string> ();
    _state_no = options->get(key + ".statenumber").as<int> ();
    _spin     = options->get(key + ".spin").as<string> ();
    if ( options->exists(key+".ecp")) {
       _use_ecp=options->get(key + ".ecp").as<bool> ();
    }

    _integrationmethod     = options->get(key + ".integrationmethod").as<string> ();

    if (!(_integrationmethod=="numeric" || _integrationmethod=="analytic")){
        std::runtime_error("Method not recognized. Only numeric and analytic available");
    }
    if ( options->exists(key+".gridsize")) {
         _gridsize = options->get(key+".gridsize").as<string>();
         }
    else _gridsize="medium";
    if ( options->exists(key+".openmp")) {
         _openmp_threads = options->get(key+".openmp").as<int>();
         }
    else _openmp_threads=0;

    // get the path to the shared folders with xml files
    char *votca_share = getenv("VOTCASHARE");
    if(votca_share == NULL) throw std::runtime_error("VOTCASHARE not set, cannot open help files.");

}

void Density2Gyration::AnalyzeDensity( Orbitals & _orbitals ){
    int threads=1;
#ifdef _OPENMP
            if ( _openmp_threads > 0 ) omp_set_num_threads(_openmp_threads);
            threads=omp_get_max_threads();
#endif
   CTP_LOG(ctp::logDEBUG, *_log) << "===== Running on "<< threads << " threads ===== " << flush;

        vector< ctp::QMAtom* > Atomlist =_orbitals.QMAtoms();
        std::vector< ctp::QMAtom* >::iterator at;
        for (at=Atomlist.begin();at<Atomlist.end();++at){
            ctp::QMAtom * atom=new ctp::QMAtom(*(*at));
            _Atomlist.push_back(atom);
        }
        ub::matrix<double> DMAT_tot;
        BasisSet bs;
        bs.LoadBasisSet(_orbitals.getDFTbasis());
        AOBasis basis;
        basis.AOBasisFill(&bs, _Atomlist );

        // Analyze geometry
        AnalyzeGeometry( _Atomlist );

        std::vector<ub::matrix<double> > DMAT;

        //basis.ReorderMOs(_orbitals.MOCoefficients(), _orbitals.getQMpackage(), "votca" );

        if(_state=="transition"){
                DMAT_tot=_orbitals.TransitionDensityMatrix(_spin, _state_no-1);
        }
        else if (_state=="ground" || _state=="excited" || _state=="exciton" ){
             CTP_LOG(ctp::logDEBUG, *_log) << "Calculating density matrix:        " << _state << " No. " << _state_no << flush;

            ub::matrix<double> DMATGS=_orbitals.DensityMatrixGroundState();
            DMAT_tot=DMATGS;
            if ( _state_no > 0 && ( _state=="excited" || _state=="exciton" ) ){

                DMAT = _orbitals.DensityMatrixExcitedState( _spin, _state_no-1);

                if (_state == "excited" ){
                    DMAT_tot=DMAT_tot-DMAT[0]+DMAT[1];
                }

            }
	   // Ground state + hole_contribution + electron contribution
	}
        else throw std::runtime_error("State entry not recognized");

        if (_integrationmethod=="numeric")  {

            // setup numerical integration grid
            NumericalIntegration numway;
            numway.GridSetup(_gridsize,&bs,_Atomlist,&basis);

            if ( _state=="ground" || _state=="excited") {
                //LOG(logDEBUG, *_log) << TimeStamp() << " Calculate Densities at Numerical Grid with gridsize "<< _gridsize  << flush;
                ub::vector<double> _analysis=numway.IntegrateGyrationTensor(DMAT_tot);

                // convert to eigenframe
                ub::vector<double> _gyration_tensor_diagonal;
                ub::matrix<double> _gyration_tensor_eigenframe;
                CTP_LOG(ctp::logDEBUG, *_log) << ctp::TimeStamp() << " Converting to Eigenframe " << flush;
                Convert2Eigenframe( _analysis, _gyration_tensor_diagonal, _gyration_tensor_eigenframe );

                // determine quaternion for rotation of xyz to EF
                CTP_LOG(ctp::logDEBUG, *_log) << ctp::TimeStamp() << " Calculating Quaternion " << flush;
                ub::vector<double> _quaternion = get_quaternion( _gyration_tensor_eigenframe );

                // report results
                CTP_LOG(ctp::logDEBUG, *_log) << ctp::TimeStamp() << " Reporting " << flush;
                ReportAnalysis( _state, _analysis, _gyration_tensor_diagonal, _gyration_tensor_eigenframe  );

            } else if ( _state == "exciton" ){
                // hole density first
                ub::vector<double> _analysis_hole=numway.IntegrateGyrationTensor(DMAT[0]);

                // convert to eigenframe
                ub::vector<double> _gyration_tensor_diagonal_hole;
                ub::matrix<double> _gyration_tensor_eigenframe_hole;
                Convert2Eigenframe( _analysis_hole, _gyration_tensor_diagonal_hole, _gyration_tensor_eigenframe_hole );

                // determine quaternion for rotation of xyz to EF
                ub::vector<double> _quaternion_hole = get_quaternion( _gyration_tensor_eigenframe_hole );

                // report results
                ReportAnalysis( "hole", _analysis_hole, _gyration_tensor_diagonal_hole, _gyration_tensor_eigenframe_hole  );

                // electron density
                ub::vector<double> _analysis_electron=numway.IntegrateGyrationTensor(DMAT[1]);

                // convert to eigenframe
                ub::vector<double> _gyration_tensor_diagonal_electron;
                ub::matrix<double> _gyration_tensor_eigenframe_electron;
                Convert2Eigenframe( _analysis_electron, _gyration_tensor_diagonal_electron, _gyration_tensor_eigenframe_electron );

                // determine quaternion for rotation of xyz to EF
                 ub::vector<double> _quaternion_electron = get_quaternion( _gyration_tensor_eigenframe_electron );

                // report results
                ReportAnalysis( "electron", _analysis_electron, _gyration_tensor_diagonal_electron,  _gyration_tensor_eigenframe_electron  );

            }

        }
          else if (_integrationmethod=="analytic") {
              //esp.Fit2Density_analytic(_Atomlist,DMAT_tot,basis);
        }
        }

    void Density2Gyration::AnalyzeGeometry(vector<ctp::QMAtom*> _atoms){

        Elements _elements;
        ub::vector<double> _analysis = ub::zero_vector<double>(10);
        std::vector< ctp::QMAtom* >::iterator at;
        for (at=_atoms.begin();at<_atoms.end();++at){

            double m = _elements.getMass( (*at)->type );
            double x = (*at)->x ;
            double y = (*at)->y ;
            double z = (*at)->z ;
            _analysis(0) += m   ;
            _analysis(1) += m*x ;
            _analysis(2) += m*y ;
            _analysis(3) += m*z ;

            _analysis(4) += m*x*x;
            _analysis(5) += m*x*y;
            _analysis(6) += m*x*z;
            _analysis(7) += m*y*y;
            _analysis(8) += m*y*z;
            _analysis(9) += m*z*z;

        }

        // normalize
        for ( unsigned i =1 ; i < 4; i++){
            _analysis(i) = _analysis(i)/_analysis(0)/tools::conv::bohr2ang;
        }
                // normalize
        for ( unsigned i =4 ; i < _analysis.size(); i++){
            _analysis(i) = _analysis(i)/_analysis(0)/tools::conv::bohr2ang/tools::conv::bohr2ang;
        }

        // gyration tensor
        _analysis(4) -= _analysis(1)*_analysis(1);
        _analysis(5) -= _analysis(1)*_analysis(2);
        _analysis(6) -= _analysis(1)*_analysis(3);
        _analysis(7) -= _analysis(2)*_analysis(2);
        _analysis(8) -= _analysis(2)*_analysis(3);
        _analysis(9) -= _analysis(3)*_analysis(3);

        // convert to eigenframe
        ub::vector<double> _gyration_tensor_diagonal;
        ub::matrix<double> _gyration_tensor_eigenframe;
        Convert2Eigenframe( _analysis, _gyration_tensor_diagonal, _gyration_tensor_eigenframe );

        cout << "\n";

        for ( int i =0 ; i< 3; i++){

            cout << _gyration_tensor_eigenframe(0,i) << " " << _gyration_tensor_eigenframe(1,i) << "  " <<  _gyration_tensor_eigenframe(2,i) << "\n" << endl;

        }

        /*
        // test for right-handedness
        double check_x = _gyration_tensor_eigenframe(1,0)* _gyration_tensor_eigenframe(2,1) -  _gyration_tensor_eigenframe(2,0)* _gyration_tensor_eigenframe(1,1) ;
        double check_y = _gyration_tensor_eigenframe(2,0)* _gyration_tensor_eigenframe(0,1) -  _gyration_tensor_eigenframe(0,0)* _gyration_tensor_eigenframe(2,1) ;
        double check_z = _gyration_tensor_eigenframe(0,0)* _gyration_tensor_eigenframe(1,1) -  _gyration_tensor_eigenframe(1,0)* _gyration_tensor_eigenframe(0,1) ;

        cout << " check x" << check_x << " vs " << _gyration_tensor_eigenframe(0,2) << "\n" <<endl;
        cout << " check y" << check_y << " vs " << _gyration_tensor_eigenframe(1,2) << "\n" <<endl;
        cout << " check z" << check_z << " vs " << _gyration_tensor_eigenframe(2,2) << "\n" <<endl;
        */
        // determine quaternion for rotation of xyz to EF
        ub::vector<double> _quaternion = get_quaternion( _gyration_tensor_eigenframe );

        // report results
        ReportAnalysis( "geometry", _analysis, _gyration_tensor_diagonal, _gyration_tensor_eigenframe  );

    }

    void Density2Gyration::ReportAnalysis(string label, ub::vector<double> _tensor_elements, ub::vector<double> _tensor_diagonal, ub::matrix<double> _tensor_frame){

            CTP_LOG(ctp::logINFO, *_log) << "---------------- " << label << " ----------------" << flush;
            CTP_LOG(ctp::logINFO, *_log) << (boost::format("  Norm               = %1$9.4f ") % (_tensor_elements(0)) ) << flush;

            //LOG(logINFO,*_pLog) << (format("  Level = %1$4d DFT = %2$+1.4f VXC = %3$+1.4f S-X = %4$+1.4f S-C = %5$+1.4f GWA = %6$+1.4f") % (_i+_qpmin+1) % _dft_energies( _i + _qpmin ) % _vxc(_i,_i) % _sigma_x(_i,_i) % _sigma_c(_i,_i) % _qp_energies(_i + _qpmin ) ).str() << flush;

            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Centroid x         = %1$9.4f Ang") % (_tensor_elements(1)*tools::conv::bohr2ang) ) << flush;
            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Centroid y         = %1$9.4f Ang") % (_tensor_elements(2)*tools::conv::bohr2ang) ) << flush;
            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Centroid y         = %1$9.4f Ang") % (_tensor_elements(3)*tools::conv::bohr2ang) ) << flush;

            double RA2 = tools::conv::bohr2ang  *tools::conv::bohr2ang;
            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Gyration Tensor xx = %1$9.4f Ang^2") % (_tensor_elements(4)*RA2) ) << flush;
            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Gyration Tensor xy = %1$9.4f Ang^2") % (_tensor_elements(5)*RA2) ) << flush;
            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Gyration Tensor xz = %1$9.4f Ang^2") % (_tensor_elements(6)*RA2) ) << flush;
            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Gyration Tensor yy = %1$9.4f Ang^2") % (_tensor_elements(7)*RA2) ) << flush;
            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Gyration Tensor yz = %1$9.4f Ang^2") % (_tensor_elements(8)*RA2) ) << flush;
            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Gyration Tensor zz = %1$9.4f Ang^2") % (_tensor_elements(9)*RA2) ) << flush;

            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Gyration Tensor D1 = %1$9.4f Ang^2") % (_tensor_diagonal(0)*RA2) ) << flush;
            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Gyration Tensor D2 = %1$9.4f Ang^2") % (_tensor_diagonal(1)*RA2) ) << flush;
            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Gyration Tensor D3 = %1$9.4f Ang^2") % (_tensor_diagonal(2)*RA2) ) << flush;

            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Radius of Gyration = %1$9.4f Ang") % (std::sqrt(_tensor_diagonal(0) + _tensor_diagonal(1) + _tensor_diagonal(2))*tools::conv::bohr2ang )) << flush;

            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Tensor EF Axis 1 1 = %1$9.4f ") % (_tensor_frame(0,0)) ) << flush;
            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Tensor EF Axis 1 2 = %1$9.4f ") % (_tensor_frame(1,0)) ) << flush;
            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Tensor EF Axis 1 3 = %1$9.4f ") % (_tensor_frame(2,0)) ) << flush;
            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Tensor EF Axis 2 1 = %1$9.4f ") % (_tensor_frame(0,1)) ) << flush;
            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Tensor EF Axis 2 2 = %1$9.4f ") % (_tensor_frame(1,1)) ) << flush;
            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Tensor EF Axis 2 3 = %1$9.4f ") % (_tensor_frame(2,1)) ) << flush;
            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Tensor EF Axis 3 1 = %1$9.4f ") % (_tensor_frame(0,2)) ) << flush;
            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Tensor EF Axis 3 2 = %1$9.4f ") % (_tensor_frame(1,2)) ) << flush;
            CTP_LOG(ctp::logINFO,*_log) << (boost::format("  Tensor EF Axis 3 3 = %1$9.4f ") % (_tensor_frame(2,2)) ) << flush;

    }

    ub::vector<double> Density2Gyration::get_quaternion(ub::matrix<double>& eigenframe){

        // second coordinate system is assumed to be Cartesian

        ub::matrix<double> M=ub::zero_matrix<double>(3,3);

        // add all the outer products, if second coordinate system was not Cartesian
        // is anyways, so M=eigenframe
        //M=ub::trans(eigenframe);
        M=eigenframe;

        ub::matrix<double> N = ub::zero_matrix<double>(4,4);

        N(0,0) =  M(0,0) + M(1,1) + M(2,2); // N11=float(M[0][:,0]+M[1][:,1]+M[2][:,2]);
        N(1,1) =  M(0,0) - M(1,1) - M(2,2); // N22=float(M[0][:,0]-M[1][:,1]-M[2][:,2])
        N(2,2) = -M(0,0) + M(1,1) - M(2,2); // N33=float(-M[0][:,0]+M[1][:,1]-M[2][:,2])
        N(3,3) = -M(0,0) - M(1,1) + M(2,2); // N44=float(-M[0][:,0]-M[1][:,1]+M[2][:,2])
        N(0,1) =  M(1,2) - M(2,1);          // N12=float(M[1][:,2]-M[2][:,1])
        N(0,2) =  M(2,0) - M(0,2);          // N13=float(M[2][:,0]-M[0][:,2])
        N(0,3) =  M(0,1) - M(1,0);          // N14=float(M[0][:,1]-M[1][:,0])
        N(1,0) =  N(0,1);                   // N21=float(N12)
        N(1,2) =  M(0,1) + M(1,0);          // N23=float(M[0][:,1]+M[1][:,0])
        N(1,3) =  M(2,0) + M(0,2);          // N24=float(M[2][:,0]+M[0][:,2])
        N(2,0) =  N(0,2);                   // N31=float(N13)
        N(2,1) =  N(1,2);                   // N32=float(N23)
        N(2,3) =  M(1,2) + M(2,1);          // N34=float(M[1][:,2]+M[2][:,1])
        N(3,0) =  N(0,3);                   // N41=float(N14)
        N(3,1) =  N(1,3);                   // N42=float(N24)
        N(3,2) =  N(2,3);                   // N43=float(N34)

        ub::vector<double> eigenvalues;
        ub::matrix<double> eigenvectors;
        linalg_eigenvalues( N, eigenvalues, eigenvectors );

        // find max eigenvalue
        int index=0;
        double maxev = eigenvalues(0);

        for (unsigned i = 1; i < eigenvalues.size(); i++ ) {
            if ( eigenvalues(i) > maxev ) {
                maxev = eigenvalues(i);
                index = i;
            }
        }

        // return this vector
        ub::vector<double> quaternion = ub::zero_vector<double>(4);
        quaternion(0)= eigenvectors(0,index);
        quaternion(1)= eigenvectors(1,index);
        quaternion(2)= eigenvectors(2,index);
        quaternion(3)= eigenvectors(3,index);

        return quaternion;

    }

        void Density2Gyration::Convert2Eigenframe(ub::vector<double> V, ub::vector<double> &_gyration_tensor_diagonal, ub::matrix<double> &_gyration_tensor_eigenframe   ){

                 ub::matrix<double> _gyration_tensor = ub::zero_matrix<double>(3,3);
                _gyration_tensor(0,0) = V(4);
                _gyration_tensor(1,0) = V(5);
                _gyration_tensor(2,0) = V(6);
                _gyration_tensor(0,1) = V(5);
                _gyration_tensor(1,1) = V(7);
                _gyration_tensor(2,1) = V(8);
                _gyration_tensor(0,2) = V(6);
                _gyration_tensor(1,2) = V(8);
                _gyration_tensor(2,2) = V(9);

                linalg_eigenvalues( _gyration_tensor, _gyration_tensor_diagonal, _gyration_tensor_eigenframe );

        }

}}


//=====================================================
// File   :  hand_vec_interface.hh
// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
//=====================================================
//
// This program 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; either version 2
// of the License, or (at your option) any later version.
//
// This program 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 program; if not, write to the Free Software
// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
//
#ifndef HAND_VEC_INTERFACE_HH
#define HAND_VEC_INTERFACE_HH

#include <Eigen/Core>
#include "f77_interface.hh"

using namespace Eigen;

template<class real>
class hand_vec_interface : public f77_interface_base<real> {

public :

  typedef typename internal::packet_traits<real>::type Packet;
  static const int PacketSize = internal::packet_traits<real>::size;

  typedef typename f77_interface_base<real>::stl_matrix stl_matrix;
  typedef typename f77_interface_base<real>::stl_vector stl_vector;
  typedef typename f77_interface_base<real>::gene_matrix gene_matrix;
  typedef typename f77_interface_base<real>::gene_vector gene_vector;

  static void free_matrix(gene_matrix & A, int N){
    internal::aligned_free(A);
  }

  static void free_vector(gene_vector & B){
    internal::aligned_free(B);
  }

  static inline void matrix_from_stl(gene_matrix & A, stl_matrix & A_stl){
    int N = A_stl.size();
    A = (real*)internal::aligned_malloc(N*N*sizeof(real));
    for (int j=0;j<N;j++)
      for (int i=0;i<N;i++)
        A[i+N*j] = A_stl[j][i];
  }

  static inline void vector_from_stl(gene_vector & B, stl_vector & B_stl){
    int N = B_stl.size();
    B = (real*)internal::aligned_malloc(N*sizeof(real));
    for (int i=0;i<N;i++)
      B[i] = B_stl[i];
  }

  static inline std::string name() {
    #ifdef PEELING
    return "hand_vectorized_peeling";
    #else
    return "hand_vectorized";
    #endif
  }

  static inline void matrix_vector_product(const gene_matrix & A, const gene_vector & B, gene_vector & X, int N)
  {
    asm("#begin matrix_vector_product");
    int AN = (N/PacketSize)*PacketSize;
    int ANP = (AN/(2*PacketSize))*2*PacketSize;
    int bound = (N/4)*4;
    for (int i=0;i<N;i++)
      X[i] = 0;

    for (int i=0;i<bound;i+=4)
    {
      register real* __restrict__ A0 = A + i*N;
      register real* __restrict__ A1 = A + (i+1)*N;
      register real* __restrict__ A2 = A + (i+2)*N;
      register real* __restrict__ A3 = A + (i+3)*N;

      Packet ptmp0 = internal::pset1(B[i]);
      Packet ptmp1 = internal::pset1(B[i+1]);
      Packet ptmp2 = internal::pset1(B[i+2]);
      Packet ptmp3 = internal::pset1(B[i+3]);
//       register Packet ptmp0, ptmp1, ptmp2, ptmp3;
//       asm(
//
//           "movss     (%[B],%[j],4), %[ptmp0]  \n\t"
//           "shufps   $0,%[ptmp0],%[ptmp0] \n\t"
//           "movss    4(%[B],%[j],4), %[ptmp1]  \n\t"
//           "shufps   $0,%[ptmp1],%[ptmp1] \n\t"
//           "movss    8(%[B],%[j],4), %[ptmp2]  \n\t"
//           "shufps   $0,%[ptmp2],%[ptmp2] \n\t"
//           "movss   12(%[B],%[j],4), %[ptmp3]  \n\t"
//           "shufps   $0,%[ptmp3],%[ptmp3] \n\t"
//           : [ptmp0] "=x" (ptmp0),
//             [ptmp1] "=x" (ptmp1),
//             [ptmp2] "=x" (ptmp2),
//             [ptmp3] "=x" (ptmp3)
//           : [B] "r" (B),
//             [j] "r" (size_t(i))
//           : );

      if (AN>0)
      {
//         for (size_t j = 0;j<ANP;j+=8)
//         {
//           asm(
//
//           "movaps     (%[A0],%[j],4), %%xmm8  \n\t"
//           "movaps   16(%[A0],%[j],4), %%xmm12 \n\t"
//           "movups     (%[A3],%[j],4), %%xmm11 \n\t"
//           "movups   16(%[A3],%[j],4), %%xmm15 \n\t"
//           "movups     (%[A2],%[j],4), %%xmm10 \n\t"
//           "movups   16(%[A2],%[j],4), %%xmm14 \n\t"
//           "movups     (%[A1],%[j],4), %%xmm9  \n\t"
//           "movups   16(%[A1],%[j],4), %%xmm13 \n\t"
//
//           "mulps %[ptmp0], %%xmm8  \n\t"
//           "addps (%[res0],%[j],4), %%xmm8  \n\t"
//           "mulps %[ptmp3], %%xmm11 \n\t"
//           "addps %%xmm11, %%xmm8  \n\t"
//           "mulps %[ptmp2], %%xmm10 \n\t"
//           "addps %%xmm10, %%xmm8  \n\t"
//           "mulps %[ptmp1], %%xmm9  \n\t"
//           "addps %%xmm9, %%xmm8   \n\t"
//           "movaps %%xmm8, (%[res0],%[j],4)  \n\t"
//
//           "mulps %[ptmp0], %%xmm12 \n\t"
//           "addps 16(%[res0],%[j],4), %%xmm12  \n\t"
//           "mulps %[ptmp3], %%xmm15 \n\t"
//           "addps %%xmm15, %%xmm12  \n\t"
//           "mulps %[ptmp2], %%xmm14 \n\t"
//           "addps %%xmm14, %%xmm12  \n\t"
//           "mulps %[ptmp1], %%xmm13 \n\t"
//           "addps %%xmm13, %%xmm12  \n\t"
//           "movaps %%xmm12, 16(%[res0],%[j],4) \n\t"
//           :
//           : [res0] "r" (X), [j] "r" (j),[A0] "r" (A0),
//             [A1] "r" (A1),
//             [A2] "r" (A2),
//             [A3] "r" (A3),
//             [ptmp0] "x" (ptmp0),
//             [ptmp1] "x" (ptmp1),
//             [ptmp2] "x" (ptmp2),
//             [ptmp3] "x" (ptmp3)
//           : "%xmm8", "%xmm9", "%xmm10", "%xmm11", "%xmm12", "%xmm13", "%xmm14", "%xmm15", "%r14");
//         }
          register Packet A00;
          register Packet A01;
          register Packet A02;
          register Packet A03;
          register Packet A10;
          register Packet A11;
          register Packet A12;
          register Packet A13;
          for (int j = 0;j<ANP;j+=2*PacketSize)
          {
//             A00 = internal::pload(&A0[j]);
//             A01 = internal::ploadu(&A1[j]);
//             A02 = internal::ploadu(&A2[j]);
//             A03 = internal::ploadu(&A3[j]);
//             A10 = internal::pload(&A0[j+PacketSize]);
//             A11 = internal::ploadu(&A1[j+PacketSize]);
//             A12 = internal::ploadu(&A2[j+PacketSize]);
//             A13 = internal::ploadu(&A3[j+PacketSize]);
//
//             A00 = internal::pmul(ptmp0, A00);
//             A01 = internal::pmul(ptmp1, A01);
//             A02 = internal::pmul(ptmp2, A02);
//             A03 = internal::pmul(ptmp3, A03);
//             A10 = internal::pmul(ptmp0, A10);
//             A11 = internal::pmul(ptmp1, A11);
//             A12 = internal::pmul(ptmp2, A12);
//             A13 = internal::pmul(ptmp3, A13);
//
//             A00 = internal::padd(A00,A01);
//             A02 = internal::padd(A02,A03);
//             A00 = internal::padd(A00,internal::pload(&X[j]));
//             A00 = internal::padd(A00,A02);
//             internal::pstore(&X[j],A00);
//
//             A10 = internal::padd(A10,A11);
//             A12 = internal::padd(A12,A13);
//             A10 = internal::padd(A10,internal::pload(&X[j+PacketSize]));
//             A10 = internal::padd(A10,A12);
//             internal::pstore(&X[j+PacketSize],A10);

            internal::pstore(&X[j],
              internal::padd(internal::pload(&X[j]),
                internal::padd(
                  internal::padd(internal::pmul(ptmp0,internal::pload(&A0[j])),internal::pmul(ptmp1,internal::ploadu(&A1[j]))),
                  internal::padd(internal::pmul(ptmp2,internal::ploadu(&A2[j])),internal::pmul(ptmp3,internal::ploadu(&A3[j]))) )));

            internal::pstore(&X[j+PacketSize],
              internal::padd(internal::pload(&X[j+PacketSize]),
                internal::padd(
                  internal::padd(internal::pmul(ptmp0,internal::pload(&A0[j+PacketSize])),internal::pmul(ptmp1,internal::ploadu(&A1[j+PacketSize]))),
                  internal::padd(internal::pmul(ptmp2,internal::ploadu(&A2[j+PacketSize])),internal::pmul(ptmp3,internal::ploadu(&A3[j+PacketSize]))) )));
          }
          for (int j = ANP;j<AN;j+=PacketSize)
            internal::pstore(&X[j],
              internal::padd(internal::pload(&X[j]),
                internal::padd(
                  internal::padd(internal::pmul(ptmp0,internal::pload(&A0[j])),internal::pmul(ptmp1,internal::ploadu(&A1[j]))),
                  internal::padd(internal::pmul(ptmp2,internal::ploadu(&A2[j])),internal::pmul(ptmp3,internal::ploadu(&A3[j]))) )));
      }
      // process remaining scalars
      for (int j=AN;j<N;j++)
        X[j] += internal::pfirst(ptmp0) * A0[j] + internal::pfirst(ptmp1) * A1[j] + internal::pfirst(ptmp2) * A2[j] + internal::pfirst(ptmp3) * A3[j];
    }
    for (int i=bound;i<N;i++)
    {
      real tmp0 = B[i];
      Packet ptmp0 = internal::pset1(tmp0);
      int iN0 = i*N;
      if (AN>0)
      {
        bool aligned0 = (iN0 % PacketSize) == 0;
        if (aligned0)
          for (int j = 0;j<AN;j+=PacketSize)
            internal::pstore(&X[j], internal::padd(internal::pmul(ptmp0,internal::pload(&A[j+iN0])),internal::pload(&X[j])));
        else
          for (int j = 0;j<AN;j+=PacketSize)
            internal::pstore(&X[j], internal::padd(internal::pmul(ptmp0,internal::ploadu(&A[j+iN0])),internal::pload(&X[j])));
      }
      // process remaining scalars
      for (int j=AN;j<N;j++)
        X[j] += tmp0 * A[j+iN0];
    }
    asm("#end matrix_vector_product");
  }

  static inline void symv(const gene_matrix & A, const gene_vector & B, gene_vector & X, int N)
  {

//     int AN = (N/PacketSize)*PacketSize;
//     int ANP = (AN/(2*PacketSize))*2*PacketSize;
//     int bound = (N/4)*4;
    for (int i=0;i<N;i++)
      X[i] = 0;

    int bound = std::max(0,N-8) & 0xfffffffE;

    for (int j=0;j<bound;j+=2)
    {
      register real* __restrict__ A0 = A + j*N;
      register real* __restrict__ A1 = A + (j+1)*N;

      real t0 = B[j];
      Packet ptmp0 = internal::pset1(t0);
      real t1 = B[j+1];
      Packet ptmp1 = internal::pset1(t1);

      real t2 = 0;
      Packet ptmp2 = internal::pset1(t2);
      real t3 = 0;
      Packet ptmp3 = internal::pset1(t3);

      int starti = j+2;
      int alignedEnd = starti;
      int alignedStart = (starti) + internal::first_aligned(&X[starti], N-starti);
      alignedEnd = alignedStart + ((N-alignedStart)/(PacketSize))*(PacketSize);

      X[j]   += t0 * A0[j];
      X[j+1] += t1 * A1[j];

      X[j+1] += t0 * A0[j+1];
      t2 += A0[j+1] * B[j+1];

//       alignedStart = alignedEnd;
      for (int i=starti; i<alignedStart; ++i) {
        X[i] += t0 * A0[i] + t1 * A1[i];
        t2 += A0[i] * B[i];
        t3 += A1[i] * B[i];
      }
      asm("#begin symv");
      for (size_t i=alignedStart; i<alignedEnd; i+=PacketSize) {
        Packet A0i = internal::ploadu(&A0[i]);
        Packet A1i = internal::ploadu(&A1[i]);
//         Packet A0i1 = internal::ploadu(&A0[i+PacketSize]);
        Packet Xi = internal::pload(&X[i]);
        Packet Bi = internal::pload/*u*/(&B[i]);
//         Packet Xi1 = internal::pload(&X[i+PacketSize]);
//         Packet Bi1 = internal::pload/*u*/(&B[i+PacketSize]);
        Xi = internal::padd(internal::padd(Xi, internal::pmul(ptmp0, A0i)), internal::pmul(ptmp1, A1i));
        ptmp2 = internal::padd(ptmp2, internal::pmul(A0i, Bi));
        ptmp3 = internal::padd(ptmp3, internal::pmul(A1i, Bi));
//         Xi1 = internal::padd(Xi1, internal::pmul(ptmp1, A0i1));
//         ptmp2 = internal::padd(ptmp2, internal::pmul(A0i1, Bi1));
//
        internal::pstore(&X[i],Xi);
//         internal::pstore(&X[i+PacketSize],Xi1);
//         asm(
//           "prefetchnta   64(%[A0],%[i],4)   \n\t"
//           //"movups     (%[A0],%[i],4), %%xmm8  \n\t"
//           "movsd       (%[A0],%[i],4), %%xmm8  \n\t"
//           "movhps     8(%[A0],%[i],4), %%xmm8  \n\t"
// //           "movups   16(%[A0],%[i],4), %%xmm9  \n\t"
// //           "movups   64(%[A0],%[i],4), %%xmm15  \n\t"
//           "movaps     (%[B], %[i],4), %%xmm12 \n\t"
// //           "movaps   16(%[B], %[i],4), %%xmm13 \n\t"
//           "movaps     (%[X], %[i],4), %%xmm10 \n\t"
// //           "movaps   16(%[X], %[i],4), %%xmm11 \n\t"
//
//           "mulps %%xmm8, %%xmm12  \n\t"
// //           "mulps %%xmm9, %%xmm13  \n\t"
//
//           "mulps %[ptmp1], %%xmm8  \n\t"
//           "addps %%xmm12, %[ptmp2]  \n\t"
//           "addps %%xmm8, %%xmm10  \n\t"
//
//
//
//
// //           "mulps %[ptmp1], %%xmm9  \n\t"
//
// //           "addps %%xmm9, %%xmm11  \n\t"
// //           "addps %%xmm13, %[ptmp2]  \n\t"
//
//           "movaps %%xmm10,   (%[X],%[i],4) \n\t"
// //           "movaps %%xmm11, 16(%[X],%[i],4) \n\t"
//           :
//           : [X] "r" (X), [i] "r" (i), [A0] "r" (A0),
//             [B] "r" (B),
//             [ptmp1] "x" (ptmp1),
//             [ptmp2] "x" (ptmp2)
//           : "%xmm8", "%xmm9", "%xmm10", "%xmm11", "%xmm12", "%xmm13", "%xmm15");
      }
      asm("#end symv");
      for (int i=alignedEnd; i<N; i++) {
        X[i] += t0 * A0[i] + t1 * A1[i];
        t2 += A0[i] * B[i];
        t3 += A1[i] * B[i];
      }

      X[j]   += t2 + internal::predux(ptmp2);
      X[j+1] += t3 + internal::predux(ptmp3);
    }
    for (int j=bound;j<N;j++)
    {
      register real* __restrict__ A0 = A + j*N;

      real t1 = B[j];
      real t2 = 0;
      X[j] += t1 * A0[j];
      for (int i=j+1; i<N; i+=PacketSize) {
        X[i] += t1 * A0[i];
        t2 += A0[i] * B[i];
      }
      X[j] += t2;
    }

  }

//   static inline void matrix_vector_product(const gene_matrix & A, const gene_vector & B, gene_vector & X, int N)
//   {
//     asm("#begin matrix_vector_product");
//     int AN = (N/PacketSize)*PacketSize;
//     int ANP = (AN/(2*PacketSize))*2*PacketSize;
//     int bound = (N/4)*4;
//     for (int i=0;i<N;i++)
//       X[i] = 0;
//
//     for (int i=0;i<bound;i+=4)
//     {
//       real tmp0 = B[i];
//       Packet ptmp0 = internal::pset1(tmp0);
//       real tmp1 = B[i+1];
//       Packet ptmp1 = internal::pset1(tmp1);
//       real tmp2 = B[i+2];
//       Packet ptmp2 = internal::pset1(tmp2);
//       real tmp3 = B[i+3];
//       Packet ptmp3 = internal::pset1(tmp3);
//       int iN0 = i*N;
//       int iN1 = (i+1)*N;
//       int iN2 = (i+2)*N;
//       int iN3 = (i+3)*N;
//       if (AN>0)
//       {
// //         int aligned0 = (iN0 % PacketSize);
//         int aligned1 = (iN1 % PacketSize);
//
//         if (aligned1==0)
//         {
//           for (int j = 0;j<AN;j+=PacketSize)
//           {
//             internal::pstore(&X[j],
//               internal::padd(internal::pload(&X[j]),
//                 internal::padd(
//                   internal::padd(internal::pmul(ptmp0,internal::pload(&A[j+iN0])),internal::pmul(ptmp1,internal::pload(&A[j+iN1]))),
//                   internal::padd(internal::pmul(ptmp2,internal::pload(&A[j+iN2])),internal::pmul(ptmp3,internal::pload(&A[j+iN3]))) )));
//           }
//         }
//         else if (aligned1==2)
//         {
//           for (int j = 0;j<AN;j+=PacketSize)
//           {
//             internal::pstore(&X[j],
//               internal::padd(internal::pload(&X[j]),
//                 internal::padd(
//                   internal::padd(internal::pmul(ptmp0,internal::pload(&A[j+iN0])),internal::pmul(ptmp1,internal::ploadu(&A[j+iN1]))),
//                   internal::padd(internal::pmul(ptmp2,internal::pload(&A[j+iN2])),internal::pmul(ptmp3,internal::ploadu(&A[j+iN3]))) )));
//           }
//         }
//         else
//         {
//           for (int j = 0;j<ANP;j+=2*PacketSize)
//           {
//             internal::pstore(&X[j],
//               internal::padd(internal::pload(&X[j]),
//                 internal::padd(
//                   internal::padd(internal::pmul(ptmp0,internal::pload(&A[j+iN0])),internal::pmul(ptmp1,internal::ploadu(&A[j+iN1]))),
//                   internal::padd(internal::pmul(ptmp2,internal::ploadu(&A[j+iN2])),internal::pmul(ptmp3,internal::ploadu(&A[j+iN3]))) )));
//
//             internal::pstore(&X[j+PacketSize],
//               internal::padd(internal::pload(&X[j+PacketSize]),
//                 internal::padd(
//                   internal::padd(internal::pmul(ptmp0,internal::pload(&A[j+PacketSize+iN0])),internal::pmul(ptmp1,internal::ploadu(&A[j+PacketSize+iN1]))),
//                   internal::padd(internal::pmul(ptmp2,internal::ploadu(&A[j+PacketSize+iN2])),internal::pmul(ptmp3,internal::ploadu(&A[j+PacketSize+iN3]))) )));
//
// //             internal::pstore(&X[j+2*PacketSize],
// //               internal::padd(internal::pload(&X[j+2*PacketSize]),
// //                 internal::padd(
// //                   internal::padd(internal::pmul(ptmp0,internal::pload(&A[j+2*PacketSize+iN0])),internal::pmul(ptmp1,internal::ploadu(&A[j+2*PacketSize+iN1]))),
// //                   internal::padd(internal::pmul(ptmp2,internal::ploadu(&A[j+2*PacketSize+iN2])),internal::pmul(ptmp3,internal::ploadu(&A[j+2*PacketSize+iN3]))) )));
// //
// //             internal::pstore(&X[j+3*PacketSize],
// //               internal::padd(internal::pload(&X[j+3*PacketSize]),
// //                 internal::padd(
// //                   internal::padd(internal::pmul(ptmp0,internal::pload(&A[j+3*PacketSize+iN0])),internal::pmul(ptmp1,internal::ploadu(&A[j+3*PacketSize+iN1]))),
// //                   internal::padd(internal::pmul(ptmp2,internal::ploadu(&A[j+3*PacketSize+iN2])),internal::pmul(ptmp3,internal::ploadu(&A[j+3*PacketSize+iN3]))) )));
//
//           }
//           for (int j = ANP;j<AN;j+=PacketSize)
//             internal::pstore(&X[j],
//               internal::padd(internal::pload(&X[j]),
//                 internal::padd(
//                   internal::padd(internal::pmul(ptmp0,internal::ploadu(&A[j+iN0])),internal::pmul(ptmp1,internal::ploadu(&A[j+iN1]))),
//                   internal::padd(internal::pmul(ptmp2,internal::ploadu(&A[j+iN2])),internal::pmul(ptmp3,internal::ploadu(&A[j+iN3]))) )));
//         }
//       }
//       // process remaining scalars
//       for (int j=AN;j<N;j++)
//         X[j] += tmp0 * A[j+iN0] + tmp1 * A[j+iN1] + tmp2 * A[j+iN2] + tmp3 * A[j+iN3];
//     }
//     for (int i=bound;i<N;i++)
//     {
//       real tmp0 = B[i];
//       Packet ptmp0 = internal::pset1(tmp0);
//       int iN0 = i*N;
//       if (AN>0)
//       {
//         bool aligned0 = (iN0 % PacketSize) == 0;
//         if (aligned0)
//           for (int j = 0;j<AN;j+=PacketSize)
//             internal::pstore(&X[j], internal::padd(internal::pmul(ptmp0,internal::pload(&A[j+iN0])),internal::pload(&X[j])));
//         else
//           for (int j = 0;j<AN;j+=PacketSize)
//             internal::pstore(&X[j], internal::padd(internal::pmul(ptmp0,internal::ploadu(&A[j+iN0])),internal::pload(&X[j])));
//       }
//       // process remaining scalars
//       for (int j=AN;j<N;j++)
//         X[j] += tmp0 * A[j+iN0];
//     }
//     asm("#end matrix_vector_product");
//   }

//   static inline void matrix_vector_product(const gene_matrix & A, const gene_vector & B, gene_vector & X, int N)
//   {
//     asm("#begin matrix_vector_product");
//     int AN = (N/PacketSize)*PacketSize;
//     for (int i=0;i<N;i++)
//       X[i] = 0;
//
//     for (int i=0;i<N;i+=2)
//     {
//       real tmp0 = B[i];
//       Packet ptmp0 = internal::pset1(tmp0);
//       real tmp1 = B[i+1];
//       Packet ptmp1 = internal::pset1(tmp1);
//       int iN0 = i*N;
//       int iN1 = (i+1)*N;
//       if (AN>0)
//       {
//         bool aligned0 = (iN0 % PacketSize) == 0;
//         bool aligned1 = (iN1 % PacketSize) == 0;
//
//         if (aligned0 && aligned1)
//         {
//           for (int j = 0;j<AN;j+=PacketSize)
//           {
//             internal::pstore(&X[j],
//               internal::padd(internal::pmul(ptmp0,internal::pload(&A[j+iN0])),
//               internal::padd(internal::pmul(ptmp1,internal::pload(&A[j+iN1])),internal::pload(&X[j]))));
//           }
//         }
//         else if (aligned0)
//         {
//           for (int j = 0;j<AN;j+=PacketSize)
//           {
//             internal::pstore(&X[j],
//               internal::padd(internal::pmul(ptmp0,internal::pload(&A[j+iN0])),
//               internal::padd(internal::pmul(ptmp1,internal::ploadu(&A[j+iN1])),internal::pload(&X[j]))));
//           }
//         }
//         else if (aligned1)
//         {
//           for (int j = 0;j<AN;j+=PacketSize)
//           {
//             internal::pstore(&X[j],
//               internal::padd(internal::pmul(ptmp0,internal::ploadu(&A[j+iN0])),
//               internal::padd(internal::pmul(ptmp1,internal::pload(&A[j+iN1])),internal::pload(&X[j]))));
//           }
//         }
//         else
//         {
//           int ANP = (AN/(4*PacketSize))*4*PacketSize;
//           for (int j = 0;j<ANP;j+=4*PacketSize)
//           {
//             internal::pstore(&X[j],
//               internal::padd(internal::pmul(ptmp0,internal::ploadu(&A[j+iN0])),
//               internal::padd(internal::pmul(ptmp1,internal::ploadu(&A[j+iN1])),internal::pload(&X[j]))));
//
//             internal::pstore(&X[j+PacketSize],
//               internal::padd(internal::pmul(ptmp0,internal::ploadu(&A[j+PacketSize+iN0])),
//               internal::padd(internal::pmul(ptmp1,internal::ploadu(&A[j+PacketSize+iN1])),internal::pload(&X[j+PacketSize]))));
//
//             internal::pstore(&X[j+2*PacketSize],
//               internal::padd(internal::pmul(ptmp0,internal::ploadu(&A[j+2*PacketSize+iN0])),
//               internal::padd(internal::pmul(ptmp1,internal::ploadu(&A[j+2*PacketSize+iN1])),internal::pload(&X[j+2*PacketSize]))));
//
//             internal::pstore(&X[j+3*PacketSize],
//               internal::padd(internal::pmul(ptmp0,internal::ploadu(&A[j+3*PacketSize+iN0])),
//               internal::padd(internal::pmul(ptmp1,internal::ploadu(&A[j+3*PacketSize+iN1])),internal::pload(&X[j+3*PacketSize]))));
//           }
//           for (int j = ANP;j<AN;j+=PacketSize)
//             internal::pstore(&X[j],
//               internal::padd(internal::pmul(ptmp0,internal::ploadu(&A[j+iN0])),
//               internal::padd(internal::pmul(ptmp1,internal::ploadu(&A[j+iN1])),internal::pload(&X[j]))));
//         }
//       }
//       // process remaining scalars
//       for (int j=AN;j<N;j++)
//         X[j] += tmp0 * A[j+iN0] + tmp1 * A[j+iN1];
//     }
//     int remaining = (N/2)*2;
//     for (int i=remaining;i<N;i++)
//     {
//       real tmp0 = B[i];
//       Packet ptmp0 = internal::pset1(tmp0);
//       int iN0 = i*N;
//       if (AN>0)
//       {
//         bool aligned0 = (iN0 % PacketSize) == 0;
//         if (aligned0)
//           for (int j = 0;j<AN;j+=PacketSize)
//             internal::pstore(&X[j], internal::padd(internal::pmul(ptmp0,internal::pload(&A[j+iN0])),internal::pload(&X[j])));
//         else
//           for (int j = 0;j<AN;j+=PacketSize)
//             internal::pstore(&X[j], internal::padd(internal::pmul(ptmp0,internal::ploadu(&A[j+iN0])),internal::pload(&X[j])));
//       }
//       // process remaining scalars
//       for (int j=AN;j<N;j++)
//         X[j] += tmp0 * A[j+iN0];
//     }
//     asm("#end matrix_vector_product");
//   }

//   static inline void matrix_vector_product(const gene_matrix & A, const gene_vector & B, gene_vector & X, int N)
//   {
//     asm("#begin matrix_vector_product");
//     int AN = (N/PacketSize)*PacketSize;
//     for (int i=0;i<N;i++)
//       X[i] = 0;
//     for (int i=0;i<N;i++)
//     {
//       real tmp = B[i];
//       Packet ptmp = internal::pset1(tmp);
//       int iN = i*N;
//       if (AN>0)
//       {
//         bool aligned = (iN % PacketSize) == 0;
//         if (aligned)
//         {
//           #ifdef PEELING
//           Packet A0, A1, A2, X0, X1, X2;
//           int ANP = (AN/(8*PacketSize))*8*PacketSize;
//           for (int j = 0;j<ANP;j+=PacketSize*8)
//           {
//             A0 = internal::pload(&A[j+iN]);
//             X0 = internal::pload(&X[j]);
//             A1 = internal::pload(&A[j+PacketSize+iN]);
//             X1 = internal::pload(&X[j+PacketSize]);
//             A2 = internal::pload(&A[j+2*PacketSize+iN]);
//             X2 = internal::pload(&X[j+2*PacketSize]);
//             internal::pstore(&X[j], internal::padd(X0, internal::pmul(ptmp,A0)));
//             A0 = internal::pload(&A[j+3*PacketSize+iN]);
//             X0 = internal::pload(&X[j+3*PacketSize]);
//             internal::pstore(&X[j+PacketSize], internal::padd(internal::pload(&X1), internal::pmul(ptmp,A1)));
//             A1 = internal::pload(&A[j+4*PacketSize+iN]);
//             X1 = internal::pload(&X[j+4*PacketSize]);
//             internal::pstore(&X[j+2*PacketSize], internal::padd(internal::pload(&X2), internal::pmul(ptmp,A2)));
//             A2 = internal::pload(&A[j+5*PacketSize+iN]);
//             X2 = internal::pload(&X[j+5*PacketSize]);
//             internal::pstore(&X[j+3*PacketSize], internal::padd(internal::pload(&X0), internal::pmul(ptmp,A0)));
//             A0 = internal::pload(&A[j+6*PacketSize+iN]);
//             X0 = internal::pload(&X[j+6*PacketSize]);
//             internal::pstore(&X[j+4*PacketSize], internal::padd(internal::pload(&X1), internal::pmul(ptmp,A1)));
//             A1 = internal::pload(&A[j+7*PacketSize+iN]);
//             X1 = internal::pload(&X[j+7*PacketSize]);
//             internal::pstore(&X[j+5*PacketSize], internal::padd(internal::pload(&X2), internal::pmul(ptmp,A2)));
//             internal::pstore(&X[j+6*PacketSize], internal::padd(internal::pload(&X0), internal::pmul(ptmp,A0)));
//             internal::pstore(&X[j+7*PacketSize], internal::padd(internal::pload(&X1), internal::pmul(ptmp,A1)));
// //
// //             internal::pstore(&X[j], internal::padd(internal::pload(&X[j]), internal::pmul(ptmp,internal::pload(&A[j+iN]))));
// //             internal::pstore(&X[j+PacketSize], internal::padd(internal::pload(&X[j+PacketSize]), internal::pmul(ptmp,internal::pload(&A[j+PacketSize+iN]))));
// //             internal::pstore(&X[j+2*PacketSize], internal::padd(internal::pload(&X[j+2*PacketSize]), internal::pmul(ptmp,internal::pload(&A[j+2*PacketSize+iN]))));
// //             internal::pstore(&X[j+3*PacketSize], internal::padd(internal::pload(&X[j+3*PacketSize]), internal::pmul(ptmp,internal::pload(&A[j+3*PacketSize+iN]))));
// //             internal::pstore(&X[j+4*PacketSize], internal::padd(internal::pload(&X[j+4*PacketSize]), internal::pmul(ptmp,internal::pload(&A[j+4*PacketSize+iN]))));
// //             internal::pstore(&X[j+5*PacketSize], internal::padd(internal::pload(&X[j+5*PacketSize]), internal::pmul(ptmp,internal::pload(&A[j+5*PacketSize+iN]))));
// //             internal::pstore(&X[j+6*PacketSize], internal::padd(internal::pload(&X[j+6*PacketSize]), internal::pmul(ptmp,internal::pload(&A[j+6*PacketSize+iN]))));
// //             internal::pstore(&X[j+7*PacketSize], internal::padd(internal::pload(&X[j+7*PacketSize]), internal::pmul(ptmp,internal::pload(&A[j+7*PacketSize+iN]))));
//           }
//           for (int j = ANP;j<AN;j+=PacketSize)
//             internal::pstore(&X[j], internal::padd(internal::pload(&X[j]), internal::pmul(ptmp,internal::pload(&A[j+iN]))));
//           #else
//           for (int j = 0;j<AN;j+=PacketSize)
//             internal::pstore(&X[j], internal::padd(internal::pload(&X[j]), internal::pmul(ptmp,internal::pload(&A[j+iN]))));
//           #endif
//         }
//         else
//         {
//           #ifdef PEELING
//           int ANP = (AN/(8*PacketSize))*8*PacketSize;
//           for (int j = 0;j<ANP;j+=PacketSize*8)
//           {
//             internal::pstore(&X[j], internal::padd(internal::pload(&X[j]), internal::pmul(ptmp,internal::ploadu(&A[j+iN]))));
//             internal::pstore(&X[j+PacketSize], internal::padd(internal::pload(&X[j+PacketSize]), internal::pmul(ptmp,internal::ploadu(&A[j+PacketSize+iN]))));
//             internal::pstore(&X[j+2*PacketSize], internal::padd(internal::pload(&X[j+2*PacketSize]), internal::pmul(ptmp,internal::ploadu(&A[j+2*PacketSize+iN]))));
//             internal::pstore(&X[j+3*PacketSize], internal::padd(internal::pload(&X[j+3*PacketSize]), internal::pmul(ptmp,internal::ploadu(&A[j+3*PacketSize+iN]))));
//             internal::pstore(&X[j+4*PacketSize], internal::padd(internal::pload(&X[j+4*PacketSize]), internal::pmul(ptmp,internal::ploadu(&A[j+4*PacketSize+iN]))));
//             internal::pstore(&X[j+5*PacketSize], internal::padd(internal::pload(&X[j+5*PacketSize]), internal::pmul(ptmp,internal::ploadu(&A[j+5*PacketSize+iN]))));
//             internal::pstore(&X[j+6*PacketSize], internal::padd(internal::pload(&X[j+6*PacketSize]), internal::pmul(ptmp,internal::ploadu(&A[j+6*PacketSize+iN]))));
//             internal::pstore(&X[j+7*PacketSize], internal::padd(internal::pload(&X[j+7*PacketSize]), internal::pmul(ptmp,internal::ploadu(&A[j+7*PacketSize+iN]))));
//           }
//           for (int j = ANP;j<AN;j+=PacketSize)
//             internal::pstore(&X[j], internal::padd(internal::pload(&X[j]), internal::pmul(ptmp,internal::ploadu(&A[j+iN]))));
//           #else
//           for (int j = 0;j<AN;j+=PacketSize)
//             internal::pstore(&X[j], internal::padd(internal::pload(&X[j]), internal::pmul(ptmp,internal::ploadu(&A[j+iN]))));
//           #endif
//         }
//       }
//       // process remaining scalars
//       for (int j=AN;j<N;j++)
//         X[j] += tmp * A[j+iN];
//     }
//     asm("#end matrix_vector_product");
//   }

    static inline void atv_product(const gene_matrix & A, const gene_vector & B, gene_vector & X, int N)
  {
    int AN = (N/PacketSize)*PacketSize;
    int bound = (N/4)*4;
    for (int i=0;i<bound;i+=4)
    {
      real tmp0 = 0;
      Packet ptmp0 = internal::pset1(real(0));
      real tmp1 = 0;
      Packet ptmp1 = internal::pset1(real(0));
      real tmp2 = 0;
      Packet ptmp2 = internal::pset1(real(0));
      real tmp3 = 0;
      Packet ptmp3 = internal::pset1(real(0));
      int iN0 = i*N;
      int iN1 = (i+1)*N;
      int iN2 = (i+2)*N;
      int iN3 = (i+3)*N;
      if (AN>0)
      {
        int align1 = (iN1 % PacketSize);
        if (align1==0)
        {
          for (int j = 0;j<AN;j+=PacketSize)
          {
            Packet b = internal::pload(&B[j]);
            ptmp0 = internal::padd(ptmp0, internal::pmul(b, internal::pload(&A[j+iN0])));
            ptmp1 = internal::padd(ptmp1, internal::pmul(b, internal::pload(&A[j+iN1])));
            ptmp2 = internal::padd(ptmp2, internal::pmul(b, internal::pload(&A[j+iN2])));
            ptmp3 = internal::padd(ptmp3, internal::pmul(b, internal::pload(&A[j+iN3])));
          }
        }
        else if (align1==2)
        {
          for (int j = 0;j<AN;j+=PacketSize)
          {
            Packet b = internal::pload(&B[j]);
            ptmp0 = internal::padd(ptmp0, internal::pmul(b, internal::pload(&A[j+iN0])));
            ptmp1 = internal::padd(ptmp1, internal::pmul(b, internal::ploadu(&A[j+iN1])));
            ptmp2 = internal::padd(ptmp2, internal::pmul(b, internal::pload(&A[j+iN2])));
            ptmp3 = internal::padd(ptmp3, internal::pmul(b, internal::ploadu(&A[j+iN3])));
          }
        }
        else
        {
          for (int j = 0;j<AN;j+=PacketSize)
          {
            Packet b = internal::pload(&B[j]);
            ptmp0 = internal::padd(ptmp0, internal::pmul(b, internal::pload(&A[j+iN0])));
            ptmp1 = internal::padd(ptmp1, internal::pmul(b, internal::ploadu(&A[j+iN1])));
            ptmp2 = internal::padd(ptmp2, internal::pmul(b, internal::ploadu(&A[j+iN2])));
            ptmp3 = internal::padd(ptmp3, internal::pmul(b, internal::ploadu(&A[j+iN3])));
          }
        }
        tmp0 = internal::predux(ptmp0);
        tmp1 = internal::predux(ptmp1);
        tmp2 = internal::predux(ptmp2);
        tmp3 = internal::predux(ptmp3);
      }
      // process remaining scalars
      for (int j=AN;j<N;j++)
      {
        tmp0 += B[j] * A[j+iN0];
        tmp1 += B[j] * A[j+iN1];
        tmp2 += B[j] * A[j+iN2];
        tmp3 += B[j] * A[j+iN3];
      }
      X[i+0] = tmp0;
      X[i+1] = tmp1;
      X[i+2] = tmp2;
      X[i+3] = tmp3;
    }

    for (int i=bound;i<N;i++)
    {
      real tmp0 = 0;
      Packet ptmp0 = internal::pset1(real(0));
      int iN0 = i*N;
      if (AN>0)
      {
        if (iN0 % PacketSize==0)
          for (int j = 0;j<AN;j+=PacketSize)
            ptmp0 = internal::padd(ptmp0, internal::pmul(internal::pload(&B[j]), internal::pload(&A[j+iN0])));
        else
          for (int j = 0;j<AN;j+=PacketSize)
            ptmp0 = internal::padd(ptmp0, internal::pmul(internal::pload(&B[j]), internal::ploadu(&A[j+iN0])));
        tmp0 = internal::predux(ptmp0);
      }
      // process remaining scalars
      for (int j=AN;j<N;j++)
        tmp0 += B[j] * A[j+iN0];
      X[i+0] = tmp0;
    }
  }

//   static inline void atv_product(const gene_matrix & A, const gene_vector & B, gene_vector & X, int N)
//   {
//     int AN = (N/PacketSize)*PacketSize;
//     for (int i=0;i<N;i++)
//       X[i] = 0;
//     for (int i=0;i<N;i++)
//     {
//       real tmp = 0;
//       Packet ptmp = internal::pset1(real(0));
//       int iN = i*N;
//       if (AN>0)
//       {
//         bool aligned = (iN % PacketSize) == 0;
//         if (aligned)
//         {
//           #ifdef PEELING
//           int ANP = (AN/(8*PacketSize))*8*PacketSize;
//           for (int j = 0;j<ANP;j+=PacketSize*8)
//           {
//             ptmp =
//               internal::padd(internal::pmul(internal::pload(&B[j]), internal::pload(&A[j+iN])),
//               internal::padd(internal::pmul(internal::pload(&B[j+PacketSize]), internal::pload(&A[j+PacketSize+iN])),
//               internal::padd(internal::pmul(internal::pload(&B[j+2*PacketSize]), internal::pload(&A[j+2*PacketSize+iN])),
//               internal::padd(internal::pmul(internal::pload(&B[j+3*PacketSize]), internal::pload(&A[j+3*PacketSize+iN])),
//               internal::padd(internal::pmul(internal::pload(&B[j+4*PacketSize]), internal::pload(&A[j+4*PacketSize+iN])),
//               internal::padd(internal::pmul(internal::pload(&B[j+5*PacketSize]), internal::pload(&A[j+5*PacketSize+iN])),
//               internal::padd(internal::pmul(internal::pload(&B[j+6*PacketSize]), internal::pload(&A[j+6*PacketSize+iN])),
//               internal::padd(internal::pmul(internal::pload(&B[j+7*PacketSize]), internal::pload(&A[j+7*PacketSize+iN])),
//               ptmp))))))));
//           }
//           for (int j = ANP;j<AN;j+=PacketSize)
//             ptmp = internal::padd(ptmp, internal::pmul(internal::pload(&B[j]), internal::pload(&A[j+iN])));
//           #else
//           for (int j = 0;j<AN;j+=PacketSize)
//             ptmp = internal::padd(ptmp, internal::pmul(internal::pload(&B[j]), internal::pload(&A[j+iN])));
//           #endif
//         }
//         else
//         {
//           #ifdef PEELING
//           int ANP = (AN/(8*PacketSize))*8*PacketSize;
//           for (int j = 0;j<ANP;j+=PacketSize*8)
//           {
//             ptmp =
//               internal::padd(internal::pmul(internal::pload(&B[j]), internal::ploadu(&A[j+iN])),
//               internal::padd(internal::pmul(internal::pload(&B[j+PacketSize]), internal::ploadu(&A[j+PacketSize+iN])),
//               internal::padd(internal::pmul(internal::pload(&B[j+2*PacketSize]), internal::ploadu(&A[j+2*PacketSize+iN])),
//               internal::padd(internal::pmul(internal::pload(&B[j+3*PacketSize]), internal::ploadu(&A[j+3*PacketSize+iN])),
//               internal::padd(internal::pmul(internal::pload(&B[j+4*PacketSize]), internal::ploadu(&A[j+4*PacketSize+iN])),
//               internal::padd(internal::pmul(internal::pload(&B[j+5*PacketSize]), internal::ploadu(&A[j+5*PacketSize+iN])),
//               internal::padd(internal::pmul(internal::pload(&B[j+6*PacketSize]), internal::ploadu(&A[j+6*PacketSize+iN])),
//               internal::padd(internal::pmul(internal::pload(&B[j+7*PacketSize]), internal::ploadu(&A[j+7*PacketSize+iN])),
//               ptmp))))))));
//           }
//           for (int j = ANP;j<AN;j+=PacketSize)
//             ptmp = internal::padd(ptmp, internal::pmul(internal::pload(&B[j]), internal::ploadu(&A[j+iN])));
//           #else
//           for (int j = 0;j<AN;j+=PacketSize)
//             ptmp = internal::padd(ptmp, internal::pmul(internal::pload(&B[j]), internal::ploadu(&A[j+iN])));
//           #endif
//         }
//         tmp = internal::predux(ptmp);
//       }
//       // process remaining scalars
//       for (int j=AN;j<N;j++)
//         tmp += B[j] * A[j+iN];
//       X[i] = tmp;
//     }
//   }

  static inline void axpy(real coef, const gene_vector & X, gene_vector & Y, int N){
    int AN = (N/PacketSize)*PacketSize;
    if (AN>0)
    {
      Packet pcoef = internal::pset1(coef);
      #ifdef PEELING
      const int peelSize = 3;
      int ANP = (AN/(peelSize*PacketSize))*peelSize*PacketSize;
      float* X1 = X + PacketSize;
      float* Y1 = Y + PacketSize;
      float* X2 = X + 2*PacketSize;
      float* Y2 = Y + 2*PacketSize;
      Packet x0,x1,x2,y0,y1,y2;
      for (int j = 0;j<ANP;j+=PacketSize*peelSize)
      {
        x0 = internal::pload(X+j);
        x1 = internal::pload(X1+j);
        x2 = internal::pload(X2+j);

        y0 = internal::pload(Y+j);
        y1 = internal::pload(Y1+j);
        y2 = internal::pload(Y2+j);

        y0 = internal::pmadd(pcoef, x0, y0);
        y1 = internal::pmadd(pcoef, x1, y1);
        y2 = internal::pmadd(pcoef, x2, y2);

        internal::pstore(Y+j,  y0);
        internal::pstore(Y1+j, y1);
        internal::pstore(Y2+j, y2);
//         internal::pstore(&Y[j+2*PacketSize], internal::padd(internal::pload(&Y[j+2*PacketSize]), internal::pmul(pcoef,internal::pload(&X[j+2*PacketSize]))));
//         internal::pstore(&Y[j+3*PacketSize], internal::padd(internal::pload(&Y[j+3*PacketSize]), internal::pmul(pcoef,internal::pload(&X[j+3*PacketSize]))));
//         internal::pstore(&Y[j+4*PacketSize], internal::padd(internal::pload(&Y[j+4*PacketSize]), internal::pmul(pcoef,internal::pload(&X[j+4*PacketSize]))));
//         internal::pstore(&Y[j+5*PacketSize], internal::padd(internal::pload(&Y[j+5*PacketSize]), internal::pmul(pcoef,internal::pload(&X[j+5*PacketSize]))));
//         internal::pstore(&Y[j+6*PacketSize], internal::padd(internal::pload(&Y[j+6*PacketSize]), internal::pmul(pcoef,internal::pload(&X[j+6*PacketSize]))));
//         internal::pstore(&Y[j+7*PacketSize], internal::padd(internal::pload(&Y[j+7*PacketSize]), internal::pmul(pcoef,internal::pload(&X[j+7*PacketSize]))));
      }
      for (int j = ANP;j<AN;j+=PacketSize)
        internal::pstore(&Y[j], internal::padd(internal::pload(&Y[j]), internal::pmul(pcoef,internal::pload(&X[j]))));
      #else
      for (int j = 0;j<AN;j+=PacketSize)
        internal::pstore(&Y[j], internal::padd(internal::pload(&Y[j]), internal::pmul(pcoef,internal::pload(&X[j]))));
      #endif
    }
    // process remaining scalars
    for (int i=AN;i<N;i++)
      Y[i] += coef * X[i];
  }

};

#endif


/*****************************************************************************
 *
 * This file is part of Mapnik (c++ mapping toolkit)
 *
 * Copyright (C) 2013 Artem Pavlenko
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * 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
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 */

#ifndef MAPNIK_POLYGON_CLIPPER_HPP
#define MAPNIK_POLYGON_CLIPPER_HPP

// stl
#include <iostream>
#include <deque>

// mapnik
#include <mapnik/box2d.hpp>
#include <mapnik/geometry.hpp>

// boost
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/register/point.hpp>

BOOST_GEOMETRY_REGISTER_POINT_2D(mapnik::coord2d, double, cs::cartesian, x, y)

// register mapnik::box2d<double>
namespace boost { namespace geometry { namespace traits
{

template<> struct tag<mapnik::box2d<double> > { typedef box_tag type; };

template<> struct point_type<mapnik::box2d<double> > { typedef mapnik::coord2d type; };

template <>
struct indexed_access<mapnik::box2d<double>, min_corner, 0>
{
    typedef coordinate_type<mapnik::coord2d>::type ct;
    static inline ct get(mapnik::box2d<double> const& b) { return b.minx();}
    static inline void set(mapnik::box2d<double> &b, ct const& value) { b.set_minx(value); }
};

template <>
struct indexed_access<mapnik::box2d<double>, min_corner, 1>
{
    typedef coordinate_type<mapnik::coord2d>::type ct;
    static inline ct get(mapnik::box2d<double> const& b) { return b.miny();}
    static inline void set(mapnik::box2d<double> &b, ct const& value) { b.set_miny(value); }
};

template <>
struct indexed_access<mapnik::box2d<double>, max_corner, 0>
{
    typedef coordinate_type<mapnik::coord2d>::type ct;
    static inline ct get(mapnik::box2d<double> const& b) { return b.maxx();}
    static inline void set(mapnik::box2d<double> &b, ct const& value) { b.set_maxx(value); }
};

template <>
struct indexed_access<mapnik::box2d<double>, max_corner, 1>
{
    typedef coordinate_type<mapnik::coord2d>::type ct;
    static inline ct get(mapnik::box2d<double> const& b) { return b.maxy();}
    static inline void set(mapnik::box2d<double> &b , ct const& value) { b.set_maxy(value); }
};

}}}

namespace mapnik {

using namespace boost::geometry;

template <typename Geometry>
struct polygon_clipper
{
    typedef mapnik::coord2d point_2d;
    typedef model::polygon<mapnik::coord2d> polygon_2d;
    typedef std::deque<polygon_2d> polygon_list;

    polygon_clipper(Geometry & geom)
        : clip_box_(),
          geom_(geom)
    {

    }

    polygon_clipper( box2d<double> const& clip_box,Geometry & geom)
        : clip_box_(clip_box),
          geom_(geom)
    {
        init();
    }

    void set_clip_box(box2d<double> const& clip_box)
    {
        clip_box_ = clip_box;
        init();
    }

    unsigned type() const
    {
        return geom_.type();
    }

    void rewind(unsigned path_id)
    {
        output_.rewind(path_id);
    }

    unsigned vertex (double * x, double * y)
    {
        return output_.vertex(x,y);
    }

private:

    void init()
    {
        polygon_2d subject_poly;
        double x,y;
        double prev_x, prev_y;
        geom_.rewind(0);
        unsigned ring_count = 0;
        while (true)
        {
            unsigned cmd = geom_.vertex(&x,&y);
            if (cmd == SEG_END) break;
            if (cmd == SEG_MOVETO)
            {
                prev_x = x;
                prev_y = y;
                if (ring_count == 0)
                {
                    append(subject_poly, make<point_2d>(x,y));
                }
                else
                {
                    subject_poly.inners().push_back(polygon_2d::inner_container_type::value_type());
                    append(subject_poly.inners().back(),make<point_2d>(x,y));
                }
                ++ring_count;
            }
            else if (cmd == SEG_LINETO)
            {
                if (std::abs(x - prev_x) < 1e-12 && std::abs(y - prev_y) < 1e-12)
                {
                    std::cerr << std::setprecision(12) << "coincident vertices:(" << prev_x << ","
                              <<  prev_y << ") , (" << x << "," << y <<  ")" << std::endl;
                    continue;
                }
                prev_x = x;
                prev_x = y;
                if (ring_count == 1)
                {
                    append(subject_poly, make<point_2d>(x,y));
                }
                else
                {
                    append(subject_poly.inners().back(),make<point_2d>(x,y));
                }
            }
        }

        polygon_list clipped_polygons;

        try
        {
            boost::geometry::intersection(clip_box_, subject_poly, clipped_polygons);
        }
        catch (boost::geometry::exception const& ex)
        {
            std::cerr << ex.what() << std::endl;
        }

        BOOST_FOREACH(polygon_2d const& poly, clipped_polygons)
        {
            bool move_to = true;
            BOOST_FOREACH(point_2d const& c, boost::geometry::exterior_ring(poly))
            {
                if (move_to)
                {
                    move_to = false;
                    output_.move_to(c.x,c.y);
                }
                else
                {
                    output_.line_to(c.x,c.y);
                }
            }
            output_.close_path();
            // interior rings
            BOOST_FOREACH(polygon_2d::inner_container_type::value_type const& ring, boost::geometry::interior_rings(poly))
            {
                move_to = true;
                BOOST_FOREACH(point_2d const& c, ring)
                {
                    if (move_to)
                    {
                        move_to = false;
                        output_.move_to(c.x,c.y);
                    }
                    else
                    {
                        output_.line_to(c.x,c.y);
                    }
                }
                output_.close_path();
            }
        }
    }

    box2d<double> clip_box_;
    Geometry & geom_;
    mapnik::geometry_type output_;

};

}

#endif //MAPNIK_POLYGON_CLIPPER_HPP