// This file is part of the dune-gdt project:
//   http://users.dune-project.org/projects/dune-gdt
// Copyright holders: Felix Schindler
// License: BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)

#ifndef DUNE_GDT_OPERATORS_PROLONGATIONS_HH
#define DUNE_GDT_OPERATORS_PROLONGATIONS_HH

#include <vector>
#include <limits>

#include <boost/numeric/conversion/cast.hpp>

#include <dune/common/dynmatrix.hh>

#include <dune/stuff/common/type_utils.hh>
#include <dune/stuff/common/vector.hh>
#include <dune/stuff/functions/interfaces.hh>
#include <dune/stuff/grid/boundaryinfo.hh>
#include <dune/stuff/grid/intersection.hh>
#include <dune/stuff/la/container.hh>
#include <dune/stuff/la/solver.hh>

#include <dune/geometry/quadraturerules.hh>

#include <dune/stuff/grid/search.hh>

#include <dune/gdt/exceptions.hh>
#include <dune/gdt/discretefunction/default.hh>
#include <dune/gdt/spaces/cg/fem.hh>
#include <dune/gdt/spaces/cg/pdelab.hh>

namespace Dune {
namespace GDT {

namespace Spaces {
namespace DG {
template<class GridPartImp, int polynomialOrder, class RangeFieldImp, size_t rangeDim, size_t rangeDimCols>
class FemBased;
}
template<class SpaceImp>
class Block;
}

namespace Operators {

/**
 *  \note The automatic detection of the right integration order might fail, so you might want to specify
 *        over_integrate. The reason is that in order to locally evaluate the source we first have to create a
 *        quadrature, the correct order of wich we guess by taking the sources order on the first entity.
 *  \note We would have liked to do something like this and match on implementations of SpaceInterface:\code
template< class T, class VS, class GPR, int pR, class RR, size_t rR, size_t rCR, class VR >
void apply(const ConstDiscreteFunction< SpaceInterface< T >, VS >& source,
           DiscreteFunction< Spaces::DG::FemBased< GPR, pR, RR, rR, rCR >, VR >& range) const
{
  static_assert(Dune::AlwaysFalse< T >::value, "Not implemented for this combination of source and range!");
}\endcode
 *        but that gave compile errors (the compiler just could not match the first argument for whatever reason). This
 *        is why we need all combinations of spaces below which are just compile time checks and forwards.
 *
 *  \todo Create a Redirect class templatized with space/vector to check that its a const/discrete function with a
 *        method which extracts the correct space if needed. This should give better compile errors.
 */
template< class GridViewType >
class L2Prolongation
{
  typedef typename GridViewType::template Codim< 0 >::Entity EntityType;
  typedef typename GridViewType::ctype DomainFieldType;
  static const size_t dimDomain = GridViewType::dimension;

public:
  L2Prolongation(const GridViewType& grid_view)
    : grid_view_(grid_view)
  {}

  // Source: Spaces::CG::FemBased
  // Range:  Spaces::DG::FemBased

  template< class GPS, int pS, class RS, size_t rS, size_t rCS, class VS,
            class GPR, int pR, class RR, size_t rR, size_t rCR, class VR >
  void apply(const ConstDiscreteFunction< Spaces::CG::FemBased< GPS, pS, RS, rS, rCS >, VS >& /*source*/,
             DiscreteFunction< Spaces::DG::FemBased< GPR, pR, RR, rR, rCR >, VR >& /*range*/) const
  {
    static_assert(Dune::AlwaysFalse< GPS >::value, "Not implemented for this combination of source and range!");
  }

  template< class GPS, int pS, class R, size_t r, size_t rC, class VS, class GPR, int pR, class VR >
  inline void apply(const ConstDiscreteFunction< Spaces::CG::FemBased< GPS, pS, R, r, rC >, VS >& source,
                    DiscreteFunction< Spaces::DG::FemBased< GPR, pR, R, r, rC >, VR >&
                      range) const
  {
    prolong_onto_dg_fem_localfunctions_wrapper(source, range);
  }

  // Source: Spaces::DG::FemBased
  // Range:  Spaces::DG::FemBased

  template< class GPS, int pS, class RS, size_t rS, size_t rCS, class VS,
            class GPR, int pR, class RR, size_t rR, size_t rCR, class VR >
  void apply(const ConstDiscreteFunction< Spaces::DG::FemBased< GPS, pS, RS, rS, rCS >, VS >& /*source*/,
             DiscreteFunction< Spaces::DG::FemBased< GPR, pR, RR, rR, rCR >, VR >& /*range*/) const
  {
    static_assert(Dune::AlwaysFalse< GPS >::value, "Not implemented for this combination of source and range!");
  }

  template< class GPS, int pS, class R, size_t r, size_t rC, class VS, class GPR, int pR, class VR >
  inline void apply(const ConstDiscreteFunction
                      < Spaces::DG::FemBased< GPS, pS, R, r, rC >, VS >& source,
                    DiscreteFunction< Spaces::DG::FemBased< GPR, pR, R, r, rC >, VR >&
                      range) const
  {
    prolong_onto_dg_fem_localfunctions_wrapper(source, range);
  }

  template< class GPS, int pS, class R, size_t r, size_t rC, class VS, class GPR, int pR, class VR >
  inline void apply(const ConstDiscreteFunction
                      < Spaces::Block< Spaces::DG::FemBased< GPS, pS, R, r, rC > >, VS >& source,
                    DiscreteFunction< Spaces::Block< Spaces::DG::FemBased< GPR, pR, R, r, rC > >, VR >&
                      range) const
  {
    prolong_onto_dg_fem_localfunctions_wrapper(source, range);
  }

  template< class GPS, int pS, class R, size_t r, size_t rC, class VS, class GPR, int pR, class VR >
  inline void apply(const ConstDiscreteFunction
                      < Spaces::Block< Spaces::DG::FemBased< GPS, pS, R, r, rC > >, VS >& source,
                    DiscreteFunction< Spaces::DG::FemBased< GPR, pR, R, r, rC >, VR >& range) const
  {
    prolong_onto_dg_fem_localfunctions_wrapper(source, range);
  }

private:
  template< class SourceFunctionType, class RangeFunctionType >
  void prolong_onto_dg_fem_localfunctions_wrapper(const SourceFunctionType& source, RangeFunctionType& range) const
  {
    typedef typename RangeFunctionType::DomainType DomainType;
    typedef typename RangeFunctionType::RangeType RangeType;
    typedef typename RangeFunctionType::RangeFieldType RangeFieldType;
    typedef typename Stuff::LA::Container< RangeFieldType, Stuff::LA::default_dense_backend >::MatrixType
        LocalMatrixType;
    typedef typename Stuff::LA::Container< RangeFieldType, Stuff::LA::default_dense_backend >::VectorType
        LocalVectorType;
    // clear
    range.vector() *= 0.0;
    // create search in the source grid part
    typedef typename SourceFunctionType::SpaceType::GridViewType SourceGridViewType;
    typedef Stuff::Grid::EntityInlevelSearch< SourceGridViewType > EntitySearch;
    EntitySearch entity_search(source.space().grid_view());
    // guess the polynomial order of the source by hoping that they are the same for all entities
    const size_t source_order = source.local_function(*source.space().grid_view().template begin< 0 >())->order();
    // walk the grid
    RangeType source_value(0);
    std::vector< RangeType > basis_values(range.space().mapper().maxNumDofs());
    const auto entity_it_end = grid_view_.template end< 0 >();
    for (auto entity_it = grid_view_.template begin< 0 >(); entity_it != entity_it_end; ++entity_it) {
      // prepare
      const auto& entity = *entity_it;
      const auto local_basis = range.space().base_function_set(entity);
      auto local_range = range.local_discrete_function(entity);
      LocalMatrixType local_matrix(local_basis.size(), local_basis.size(), RangeFieldType(0));
      LocalVectorType local_vector(local_basis.size(), RangeFieldType(0));
      LocalVectorType local_DoFs(local_basis.size(), RangeFieldType(0));
      // create quadrature
      const auto integrand_order = std::max(source_order, local_basis.order()) + local_basis.order();
      const auto& quadrature = QuadratureRules< DomainFieldType, dimDomain >::rule(entity.type(),
                                                                                boost::numeric_cast< int >(integrand_order));
      // get global quadrature points
      std::vector< DomainType > quadrature_points;
      for (const auto& quadrature_point : quadrature)
        quadrature_points.emplace_back(entity.geometry().global(quadrature_point.position()));
      // get source entities
      const auto source_entity_ptr_unique_ptrs = entity_search(quadrature_points);
      assert(source_entity_ptr_unique_ptrs.size() >= quadrature_points.size());
      // loop over all quadrature points
      size_t pp = 0;
      for (const auto& quadrature_point : quadrature) {
        const auto local_point = quadrature_point.position();
        const auto quadrature_weight = quadrature_point.weight();
        const auto integration_element = entity.geometry().integrationElement(local_point);
        // evaluate source
        const auto& source_entity_ptr_unique_ptr = source_entity_ptr_unique_ptrs[pp];
        if (source_entity_ptr_unique_ptr) {
          const auto source_entity_ptr = *source_entity_ptr_unique_ptr;
          const auto& source_entity = *source_entity_ptr;
          const auto local_source = source.local_function(source_entity);
          local_source->evaluate(source_entity.geometry().local(entity.geometry().global(local_point)), source_value);
        } else
          source_value *= 0.0;
        // evaluate
        local_basis.evaluate(local_point, basis_values);
        // compute integrals
        for (size_t ii = 0; ii < local_basis.size(); ++ii) {
          local_vector[ii] += integration_element * quadrature_weight * (source_value * basis_values[ii]);
          for (size_t jj = 0; jj < local_basis.size(); ++jj) {
            local_matrix.add_to_entry(ii,
                                      jj,
                                      integration_element * quadrature_weight * (basis_values[ii] * basis_values[jj]));
          }
        }
        ++pp;
      } // loop over all quadrature points
      // compute local DoFs
      try {
        Stuff::LA::Solver< LocalMatrixType >(local_matrix).apply(local_vector, local_DoFs);
      } catch (Stuff::Exceptions::linear_solver_failed& ee) {
        DUNE_THROW(Exceptions::prolongation_error,
                   "L2 prolongation failed because a local matrix could not be inverted!\n\n"
                   << "This was the original error: " << ee.what());
      }
      // set local DoFs
      auto local_range_vector = local_range->vector();
      assert(local_range_vector.size() == local_DoFs.size());
      for (size_t ii = 0; ii < local_range_vector.size(); ++ii)
        local_range_vector.set(ii, local_DoFs.get_entry(ii));
    } // walk the grid
  } // ... prolong_onto_dg_fem_localfunctions_wrapper(...)

  const GridViewType& grid_view_;
}; // class L2Prolongation

/**
 *  \note We would have liked to do something like this and match on implementations of SpaceInterface:\code
template< class T, class VS, class GPR, int pR, class RR, size_t rR, size_t rCR, class VR >
void apply(const ConstDiscreteFunction< SpaceInterface< T >, VS >& source,
           DiscreteFunction< Spaces::CG::FemBased< GPR, pR, RR, rR, rCR >, VR >& range) const
{
  static_assert(Dune::AlwaysFalse< T >::value, "Not implemented for this combination of source and range!");
}\endcode
 *        but that gave compile errors (the compiler just could not match the first argument for whatever reason). This
 *        is why we need all combinations of spaces below which are just compile time checks and forwards.
 */
template< class GridViewType >
class LagrangeProlongation
{
public:
  typedef typename GridViewType::ctype DomainFieldType;
  static const size_t dimDomain = GridViewType::dimension;

  LagrangeProlongation(const GridViewType& grid_view)
    : grid_view_(grid_view)
  {}

  // Source: Spaces::CG::FemBased
  // Range:  Spaces::CG::FemBased

  template< class GPS, int pS, class RS, size_t rS, size_t rCS, class VS,
            class GPR, int pR, class RR, size_t rR, size_t rCR, class VR >
  void apply(const ConstDiscreteFunction< Spaces::CG::FemBased< GPS, pS, RS, rS, rCS >, VS >& /*source*/,
             DiscreteFunction< Spaces::CG::FemBased< GPR, pR, RR, rR, rCR >, VR >& /*range*/) const
  {
    static_assert(Dune::AlwaysFalse< GPS >::value, "Not implemented for this combination of source and range!");
  }

  template< class GPS, int pS, class R, size_t r, class VS, class GPR, int pR, class VR >
  inline void apply(const ConstDiscreteFunction< Spaces::CG::FemBased< GPS, pS, R, r, 1 >, VS >& source,
                    DiscreteFunction< Spaces::CG::FemBased< GPR, pR, R, r, 1 >, VR >& range) const
  {
    redirect_to_appropriate_apply(source, range);
  }

  // Source: Spaces::DG::FemBased
  // Range:  Spaces::CG::FemBased

  template< class GPS, int pS, class RS, size_t rS, size_t rCS, class VS,
            class GPR, int pR, class RR, size_t rR, size_t rCR, class VR >
  void apply(const ConstDiscreteFunction< Spaces::DG::FemBased< GPS, pS, RS, rS, rCS >, VS >& /*source*/,
             DiscreteFunction< Spaces::CG::FemBased< GPR, pR, RR, rR, rCR >, VR >& /*range*/) const
  {
    static_assert(Dune::AlwaysFalse< GPS >::value, "Not implemented for this combination of source and range!");
  }

  template< class GPS, int pS, class R, size_t r, class VS, class GPR, int pR, class VR >
  inline void apply(const ConstDiscreteFunction
                      < Spaces::DG::FemBased< GPS, pS, R, r, 1 >, VS >& source,
                    DiscreteFunction< Spaces::CG::FemBased< GPR, pR, R, r, 1 >, VR >& range) const
  {
    redirect_to_appropriate_apply(source, range);
  }

  // Source: Spaces::CG::PdelabBased
  // Range:  Spaces::CG::PdelabBased

  template< class GPS, int pS, class RS, size_t rS, size_t rCS, class VS,
            class GPR, int pR, class RR, size_t rR, size_t rCR, class VR >
  void apply(const ConstDiscreteFunction< Spaces::CG::PdelabBased< GPS, pS, RS, rS, rCS >, VS >& /*source*/,
             DiscreteFunction< Spaces::CG::PdelabBased< GPR, pR, RR, rR, rCR >, VR >& /*range*/) const
  {
    static_assert(Dune::AlwaysFalse< GPS >::value, "Not implemented for this combination of source and range!");
  }

  template< class GPS, int pS, class R, size_t r, size_t rC, class VS, class GPR, class VR >
  inline void apply(const ConstDiscreteFunction < Spaces::CG::PdelabBased< GPS, pS, R, r, rC >, VS >&
                      source,
                    DiscreteFunction< Spaces::CG::PdelabBased< GPR, 1, R, r, rC >, VR >& range) const
  {
    redirect_to_appropriate_apply(source, range);
  }

private:
  template< class SourceType, class RangeType >
  void redirect_to_appropriate_apply(const SourceType& source, RangeType& range) const
  {
    // create search in the source grid part
    typedef typename SourceType::SpaceType::GridViewType SourceGridViewType;
    typedef Stuff::Grid::EntityInlevelSearch< SourceGridViewType > EntitySearch;
    EntitySearch entity_search(source.space().grid_view());
    // set all range dofs to infinity
    const auto infinity = std::numeric_limits< typename RangeType::RangeFieldType >::infinity();
    for (size_t ii = 0; ii < range.vector().size(); ++ii)
      range.vector().set_entry(ii, infinity);
    // walk the grid
    const auto entity_it_end = grid_view_.template end< 0 >();
    for (auto entity_it = grid_view_.template begin< 0 >();
         entity_it != entity_it_end;
         ++entity_it) {
      const auto& entity = *entity_it;
      // get global lagrange point coordinates
      const auto lagrange_point_set = range.space().lagrange_points(entity);
      typedef FieldVector< typename SourceGridViewType::ctype, SourceGridViewType::dimension > DomainType;
      std::vector< DomainType > lagrange_points(lagrange_point_set.size());
      for (size_t ii = 0; ii < lagrange_point_set.size(); ++ii)
        lagrange_points[ii] = entity.geometry().global(lagrange_point_set[ii]);
      // get source entities
      const auto source_entity_ptrs = entity_search(lagrange_points);
      assert(source_entity_ptrs.size() == lagrange_points.size());
      // get range
      auto local_range = range.local_discrete_function(entity);
      auto local_range_DoF_vector = local_range->vector();
      // do the actual work (see below)
      apply_local(source, lagrange_points, source_entity_ptrs, local_range_DoF_vector);
    } // walk the grid
  } // ... redirect_to_appropriate_apply(...)

  template< class SourceType, class LagrangePointsType, class EntityPointers, class LocalDoFVectorType >
  void apply_local(const SourceType& source,
                   const LagrangePointsType& lagrange_points,
                   const EntityPointers& source_entity_ptr_unique_ptrs,
                   LocalDoFVectorType& range_DoF_vector) const
  {
    static const size_t dimRange = SourceType::dimRange;
    size_t kk = 0;
    assert(source_entity_ptr_unique_ptrs.size() >= lagrange_points.size());
    for (size_t ii = 0; ii < lagrange_points.size(); ++ii) {
      if (std::isinf(range_DoF_vector.get(kk))) {
        const auto& global_point = lagrange_points[ii];
        // evaluate source function
        const auto& source_entity_ptr_unique_ptr = source_entity_ptr_unique_ptrs[ii];
        if (source_entity_ptr_unique_ptr) {
          const auto source_entity_ptr = *source_entity_ptr_unique_ptr;
          const auto& source_entity = *source_entity_ptr;
          const auto local_source_point = source_entity.geometry().local(global_point);
          const auto local_source = source.local_function(source_entity);
          const auto source_value = local_source->evaluate(local_source_point);
          for (size_t jj = 0; jj < dimRange; ++jj, ++kk)
            range_DoF_vector.set(kk, source_value[jj]);
        } else
          for (size_t jj = 0; jj < dimRange; ++jj, ++kk)
            range_DoF_vector.set(kk, 0.0);
      }
      else
        kk += dimRange;
    }
  } // ... apply_local(...)

  const GridViewType& grid_view_;
}; // class LagrangeProlongation

template< class GridViewType >
class Prolongation
{
public:
  typedef typename GridViewType::ctype DomainFieldType;
  static const size_t dimDomain = GridViewType::dimension;

  Prolongation(const GridViewType& grid_view)
    : l2_prolongation_operator_(grid_view)
    , lagrange_prolongation_operator_(grid_view)
  {}

  template< class SourceType, class RangeType >
  void apply(const SourceType& source, RangeType& range) const
  {
    redirect_to_appropriate_operator(source, range);
  }

private:
  template< class GPS, int pS, class RS, size_t rS, size_t rCS, class VS,
            class GPR, int pR, class RR, size_t rR, size_t rCR, class VR >
  inline void redirect_to_appropriate_operator(const ConstDiscreteFunction< Spaces::CG::FemBased
                                                  < GPS, pS, RS, rS, rCS >, VS >& source,
                                               DiscreteFunction< Spaces::DG::FemBased
                                                  < GPR, pR, RR, rR, rCR >, VR >& range) const
  {
    l2_prolongation_operator_.apply(source, range);
  }

  template< class GPS, int pS, class RS, size_t rS, size_t rCS, class VS,
            class GPR, int pR, class RR, size_t rR, size_t rCR, class VR >
  inline void redirect_to_appropriate_operator(const ConstDiscreteFunction
                                                  < Spaces::DG::FemBased
                                                    < GPS, pS, RS, rS, rCS >, VS >& source,
                                               DiscreteFunction
                                                  < Spaces::DG::FemBased
                                                    < GPR, pR, RR, rR, rCR >, VR >& range) const
  {
    l2_prolongation_operator_.apply(source, range);
  }

  template< class GPS, int pS, class RS, size_t rS, size_t rCS, class VS,
            class GPR, int pR, class RR, size_t rR, size_t rCR, class VR >
  inline void redirect_to_appropriate_operator(const ConstDiscreteFunction
                                                  < Spaces::Block< Spaces::DG::FemBased
                                                    < GPS, pS, RS, rS, rCS > >, VS >& source,
                                               DiscreteFunction
                                                  < Spaces::Block< Spaces::DG::FemBased
                                                    < GPR, pR, RR, rR, rCR > >, VR >& range) const
  {
    l2_prolongation_operator_.apply(source, range);
  }

  template< class GPS, int pS, class RS, size_t rS, size_t rCS, class VS,
            class GPR, int pR, class RR, size_t rR, size_t rCR, class VR >
  inline void redirect_to_appropriate_operator(const ConstDiscreteFunction
                                                  < Spaces::Block< Spaces::DG::FemBased
                                                    < GPS, pS, RS, rS, rCS > >, VS >& source,
                                               DiscreteFunction
                                                  < Spaces::DG::FemBased
                                                    < GPR, pR, RR, rR, rCR >, VR >& range) const
  {
    l2_prolongation_operator_.apply(source, range);
  }

  template< class GPS, int pS, class RS, size_t rS, size_t rCS, class VS,
            class GPR, int pR, class RR, size_t rR, size_t rCR, class VR >
  inline void redirect_to_appropriate_operator(const ConstDiscreteFunction
                                                  < Spaces::CG::FemBased
                                                    < GPS, pS, RS, rS, rCS >, VS >& source,
                                               DiscreteFunction
                                                  < Spaces::CG::FemBased
                                                    < GPR, pR, RR, rR, rCR >, VR >& range) const
  {
    lagrange_prolongation_operator_.apply(source, range);
  }

  template< class GPS, int pS, class RS, size_t rS, size_t rCS, class VS,
            class GPR, int pR, class RR, size_t rR, size_t rCR, class VR >
  inline void redirect_to_appropriate_operator(const ConstDiscreteFunction
                                                  < Spaces::DG::FemBased
                                                    < GPS, pS, RS, rS, rCS >, VS >& source,
                                               DiscreteFunction
                                                  < Spaces::CG::FemBased
                                                    < GPR, pR, RR, rR, rCR >, VR >& range) const
  {
    lagrange_prolongation_operator_.apply(source, range);
  }

  template< class GPS, int pS, class RS, size_t rS, size_t rCS, class VS,
            class GPR, int pR, class RR, size_t rR, size_t rCR, class VR >
  inline void redirect_to_appropriate_operator(const ConstDiscreteFunction
                                                  < Spaces::CG::PdelabBased
                                                    < GPS, pS, RS, rS, rCS >, VS >& source,
                                               DiscreteFunction
                                                  < Spaces::CG::PdelabBased
                                                    < GPR, pR, RR, rR, rCR >, VR >& range) const
  {
    lagrange_prolongation_operator_.apply(source, range);
  }

  const L2Prolongation< GridViewType > l2_prolongation_operator_;
  const LagrangeProlongation< GridViewType > lagrange_prolongation_operator_;
}; // class Prolongation

template< class GridViewType, class SourceType, class RangeType >
void prolong(const GridViewType& grid_view, const SourceType& source, RangeType& range)
{
  const Prolongation< GridViewType > prolongation_operator(grid_view);
  prolongation_operator.apply(source, range);
}

template< class SourceType, class RangeType >
void prolong(const SourceType& source, RangeType& range)
{
  const Prolongation< typename RangeType::SpaceType::GridViewType > prolongation_operator(range.space().grid_view());
  prolongation_operator.apply(source, range);
}

} // namespace Operators
} // namespace GDT
} // namespace Dune

#endif // DUNE_GDT_OPERATORS_PROLONGATIONS_HH


//Tencent is pleased to support the open source community by making FeatherCNN available.

//Copyright (C) 2019 THL A29 Limited, a Tencent company. All rights reserved.

//Licensed under the BSD 3-Clause License (the "License"); you may not use this file except
//in compliance with the License. You may obtain a copy of the License at
//
//https://opensource.org/licenses/BSD-3-Clause
//
//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 <booster/sgemv.h>
#include <booster/helper.h>

#include <assert.h>
#include <immintrin.h>
#include <string.h>
#if 0
void fully_connected_inference_direct(const int input_size, const int output_size, const float *x, const float *y, float *z, const int num_threads)
{
    #pragma omp parallel for schedule(static) num_threads(num_threads)
    for (int i = 0; i < output_size; i++)
    {
        float sum = 0.f;
        for (int j = 0; j < input_size; j++)
            sum += x[j] * y[i * input_size + j];
        z[i] = sum;
    }
}

void fully_connected_transpose_inference_sse8(const int input_size, const int output_size, const float *x, const float *y, float *z, const int num_threads)
{
    assert(input_size % 8 == 0);
    assert(output_size % 8 == 0);
    #pragma omp parallel for schedule(static) num_threads(num_threads)
    for (int k = 0; k < output_size / 8; k++)
    {
        const float *yPtr = y + k * 8 * input_size;
        __m128 res = {0.0, 0.0, 0.0, 0.0};
        __m128 res1 = {0.0, 0.0, 0.0, 0.0};
        __m128 va0, va1, va2, va3, vb0, vb1, vb2, vb3, vb4, vb5, vb6, vb7;
        for (int i = 0; i < input_size; i += 4)
        {
            //          __m128 v1, v2;
            //va = _mm_load_ps(x + i);

            vb0 = _mm_load_ps(yPtr);
            vb1 = _mm_load_ps(yPtr + 4);
            vb2 = _mm_load_ps(yPtr + 8);
            vb3 = _mm_load_ps(yPtr + 12);
            vb4 = _mm_load_ps(yPtr + 16);
            vb5 = _mm_load_ps(yPtr + 20);
            vb6 = _mm_load_ps(yPtr + 24);
            vb7 = _mm_load_ps(yPtr + 28);

            va0 = _mm_broadcast_ss(x + i);
            va1 = _mm_broadcast_ss(x + i + 1);
            va2 = _mm_broadcast_ss(x + i + 2);
            va3 = _mm_broadcast_ss(x + i + 3);

            res = _mm_fmadd_ps(vb0, va0, res);
            res1 = _mm_fmadd_ps(vb1, va0, res1);
            res = _mm_fmadd_ps(vb2, va1, res);
            res1 = _mm_fmadd_ps(vb3, va1, res1);
            res = _mm_fmadd_ps(vb4, va2, res);
            res1 = _mm_fmadd_ps(vb5, va2, res1);
            res = _mm_fmadd_ps(vb6, va3, res);
            res1 = _mm_fmadd_ps(vb7, va3, res1);

            yPtr += 32;
        }
        _mm_store_ps((z + 8 * k), res);
        _mm_store_ps((z + 8 * k + 4), res1);
    }
}

#include <stdio.h>
#include <stdlib.h>

//For fully connected layers, the weights matrix is transposed.
//To reduce memory allocation, a KxSTRIDE packed matrix is sufficient.
template<int STRIDE>
void packed_sgemv_transposed_init(const int N, const int K, float* matrix, float* packed_buffer)
{
    size_t N_aligned = N - N % STRIDE;
    for (int j = 0; j < N_aligned; j += STRIDE)
    {
        float* pMatrix = matrix + j * K;
        float* pPacked = packed_buffer;
        for (int k = 0; k < K; ++k)
        {
            for (int i = 0; i < STRIDE; ++i)
                pPacked[i] = pMatrix[i * K + k];
            pPacked += STRIDE;
        }
        memcpy(pMatrix, packed_buffer, STRIDE * K * sizeof(float));
    }
    int rem = N % STRIDE;
    if (rem > 0)
    {
        float* pMatrix = matrix + N_aligned * K;
        float* pPacked = packed_buffer;
        for (int k = 0; k < K; ++k)
        {
            for (int i = 0; i < rem; ++i)
                pPacked[i] = pMatrix[i * K + k];
            pPacked += rem;
        }
        memcpy(pMatrix, packed_buffer, rem * K * sizeof(float));
    }
}
template void packed_sgemv_transposed_init<8>(const int N, const int K, float* matrix, float* packed_matrix);
template void packed_sgemv_transposed_init<16>(const int N, const int K, float* matrix, float* packed_matrix);

//The packed matrix will only be scanned once. Cache miss would only occur with very large K, which is not a very common case.

template<bool fuseBias, bool fuseRelu>
void packed_sgemv(const int N, const int K, const float* A, const float* B, float* C, const float* bias_data, const int num_threads)
{
    assert(K % 4 == 0);
    int N_aligned = N - N % 16;
    __m256 vZero = _mm256_set1_ps(0.f);
    #pragma omp parallel for
    for (int j = 0; j < N_aligned; j += 16)
    {
        const float *pB = B + j * K;
        float *pC = C + j;
        __m256 acc0 = _mm256_set1_ps(0.f);
        __m256 acc1 = _mm256_set1_ps(0.f);
        if (fuseBias)
        {
            acc0 = _mm256_load_ps(bias_data + j);
            acc1 = _mm256_load_ps(bias_data + j + 8);
        }
        __m256 va0, va1, va2, va3;
        __m256 vb0, vb1, vb2, vb3, vb4, vb5, vb6, vb7;
        for (int k = 0; k < K; k += 4)
        {
            vb0 = _mm256_load_ps(pB);
            vb1 = _mm256_load_ps(pB + 8);
            vb2 = _mm256_load_ps(pB + 16);
            vb3 = _mm256_load_ps(pB + 24);
            vb4 = _mm256_load_ps(pB + 32);
            vb5 = _mm256_load_ps(pB + 40);
            vb6 = _mm256_load_ps(pB + 48);
            vb7 = _mm256_load_ps(pB + 56);

            //print_floats(pB, 8);
            va0 = _mm256_broadcast_ss(A + k);
            va1 = _mm256_broadcast_ss(A + k + 1);
            va2 = _mm256_broadcast_ss(A + k + 2);
            va3 = _mm256_broadcast_ss(A + k + 3);

            acc0 = _mm256_fmadd_ps(vb0, va0, acc0);
            acc1 = _mm256_fmadd_ps(vb1, va0, acc1);
            acc0 = _mm256_fmadd_ps(vb2, va1, acc0);
            acc1 = _mm256_fmadd_ps(vb3, va1, acc1);
            acc0 = _mm256_fmadd_ps(vb4, va2, acc0);
            acc1 = _mm256_fmadd_ps(vb5, va2, acc1);
            acc0 = _mm256_fmadd_ps(vb6, va3, acc0);
            acc1 = _mm256_fmadd_ps(vb7, va3, acc1);
            pB += 64;
        }
        if (fuseRelu)
        {
            acc0 = _mm256_max_ps(vZero, acc0);
            acc1 = _mm256_max_ps(vZero, acc1);
        }
        _mm256_store_ps(pC, acc0);
        _mm256_store_ps(pC + 8, acc1);
    }

    int rem = N % 16;
    if (rem == 8)
    {
        const float *pB = B + N_aligned * K;
        __m256 vacc = _mm256_set1_ps(0.f);
        if (fuseBias)
            vacc = _mm256_load_ps(bias_data + N_aligned);
        __m256 va, vb;
        float acc[4];
        for (int k = 0; k < K; ++k)
        {
            vb  = _mm256_loadu_ps(pB);
            va  = _mm256_broadcast_ss(A + k);
            vacc = _mm256_fmadd_ps(vb, va, vacc);
            pB += rem;
        }
        if (fuseRelu)
            vacc = _mm256_max_ps(vZero, vacc);
        _mm256_store_ps(C + N_aligned, vacc);
    }
    else if (rem > 0)
    {
        const float *pB = B + N_aligned * K;
        float *pC = C + N_aligned;
        __m256 vacc = _mm256_set1_ps(0.f);
        if (fuseBias && rem >= 8)
            vacc = _mm256_load_ps(bias_data + N_aligned);

        __m256 va, vb;
        float acc[4];
        for (int i = 0; i < rem - 8; ++i)
        {
            if (fuseBias)
                acc[i] = bias_data[N_aligned + i];
            else
                acc[i] = 0;
        }
        for (int k = 0; k < K; ++k)
        {
            if (rem >= 8)
            {
                vb  = _mm256_loadu_ps(pB);
                va  = _mm256_broadcast_ss(A + k);
                vacc = _mm256_fmadd_ps(vb, va, vacc);
            }
            if (rem - 8 > 0)
            {
                for (int i = 0; i < rem - 8; ++i)
                    acc[i] = A[k] * pB[i + 8];
            }
            pB += rem;
        }
        if (fuseRelu)
        {
            vacc = _mm256_max_ps(vZero, vacc);
        }
        _mm256_store_ps(pC, vacc);
        for (int i = 0; i < rem - 8; ++i)
        {
            if (fuseRelu)
                pC[i + 8] = ((acc[i] > 0.f) ? acc[i] : 0.f);
            else
                pC[i + 8] = acc[i];
        }
    }
}

template void packed_sgemv<false, false>(const int N, const int K, const float* A, const float* B, float* C, const float* bias_data, const int num_threads);
template void packed_sgemv<true, false>(const int N, const int K, const float* A, const float* B, float* C, const float* bias_data, const int num_threads);
template void packed_sgemv<false, true>(const int N, const int K, const float* A, const float* B, float* C, const float* bias_data, const int num_threads);
template void packed_sgemv<true, true>(const int N, const int K, const float* A, const float* B, float* C, const float* bias_data, const int num_threads);

void fully_connected_inference_direct_BiasReLU(int input_size, int output_size, float *x, float *y, float *z, float* biasArr, int num_threads)
{
    #pragma omp parallel for schedule(static) num_threads(num_threads)
    for (int i = 0; i < output_size; i++)
    {
        float sum = 0.f;
        for (int j = 0; j < input_size; j++)
            sum += x[j] * y[i * input_size + j];

        sum += biasArr[i];
        if (sum < 0.f) sum = 0.f;
        z[i] = sum;
    }
}

void fully_connected_transpose_inference_sse8_BiasReLU(int input_size, int output_size, float *x, float *y, float *z, float* biasArr, int num_threads)
{
    assert(input_size % 8 == 0);
    assert(output_size % 8 == 0);
    #pragma omp parallel for schedule(static) num_threads(num_threads)
    for (int k = 0; k < output_size / 8; k++)
    {
        float *yPtr = y + k * 8 * input_size;
        const __m128 vzero = _mm_set1_ps(0.f);

        __m128 res  = _mm_load_ps(biasArr + k * 8);
        __m128 res1 = _mm_load_ps(biasArr + k * 8 + 4);

        __m128 va0, va1, va2, va3, vb0, vb1, vb2, vb3, vb4, vb5, vb6, vb7;
        for (int i = 0; i < input_size; i += 4)
        {
            vb0 = _mm_load_ps(yPtr);
            vb1 = _mm_load_ps(yPtr + 4);
            vb2 = _mm_load_ps(yPtr + 8);
            vb3 = _mm_load_ps(yPtr + 12);
            vb4 = _mm_load_ps(yPtr + 16);
            vb5 = _mm_load_ps(yPtr + 20);
            vb6 = _mm_load_ps(yPtr + 24);
            vb7 = _mm_load_ps(yPtr + 28);

            va0 = _mm_broadcast_ss(x + i);
            va1 = _mm_broadcast_ss(x + i + 1);
            va2 = _mm_broadcast_ss(x + i + 2);
            va3 = _mm_broadcast_ss(x + i + 3);

            res = _mm_fmadd_ps(vb0, va0, res);
            res1 = _mm_fmadd_ps(vb1, va0, res1);
            res = _mm_fmadd_ps(vb2, va1, res);
            res1 = _mm_fmadd_ps(vb3, va1, res1);
            res = _mm_fmadd_ps(vb4, va2, res);
            res1 = _mm_fmadd_ps(vb5, va2, res1);
            res = _mm_fmadd_ps(vb6, va3, res);
            res1 = _mm_fmadd_ps(vb7, va3, res1);

            yPtr += 32;
        }

        res  = _mm_max_ps(res, vzero);
        res1 = _mm_max_ps(res1, vzero);

        _mm_store_ps((z + 8 * k), res);
        _mm_store_ps((z + 8 * k + 4), res1);
    }
}

#else

template <bool fuseBias, bool fuseRelu>
void fully_connected_inference_direct(const int input_size, const int output_size, const float *x, const float *y, float *z, const int num_threads, float *bias_arr)
{
    #pragma omp parallel for schedule(static) num_threads(num_threads)
    for (int i = 0; i < output_size; i++)
    {
        float sum = 0;
        for (int j = 0; j < input_size; j++)
            sum += x[j] * y[i * input_size + j];
        if (fuseBias)
            sum += bias_arr[i];
        if (fuseRelu)
            sum = (sum > 0.f) ? sum : 0.f;
        z[i] = sum;
    }
}

template <bool fuseBias, bool fuseRelu>
void fully_connected_transpose_inference(const int input_size, const int output_size, const float *x, const float *y, float *z, const int num_threads, float *bias_arr)
{
    assert(input_size % 8 == 0);
    assert(output_size % 8 == 0);
    #pragma omp parallel for schedule(static) num_threads(num_threads)
    for (int k = 0; k < output_size / 8; k++)
    {
        __m128 vBias, vBias1;
        const __m128 vZero = _mm_set1_ps(0.f);
        __m128 res = _mm_set1_ps(0.f);
        __m128 res1 = _mm_set1_ps(0.f);

        if (fuseBias)
        {
            vBias = _mm_load_ps(bias_arr + k * 8);
            vBias1 = _mm_load_ps(bias_arr + k * 8 + 4);
        }
        const float *yPtr = y + k * 8 * input_size;
        __m128 va0, va1, va2, va3, vb0, vb1, vb2, vb3, vb4, vb5, vb6, vb7;
        for (int i = 0; i < input_size; i += 4)
        {
            vb0 = _mm_load_ps(yPtr);
            vb1 = _mm_load_ps(yPtr + 4);
            vb2 = _mm_load_ps(yPtr + 8);
            vb3 = _mm_load_ps(yPtr + 12);
            vb4 = _mm_load_ps(yPtr + 16);
            vb5 = _mm_load_ps(yPtr + 20);
            vb6 = _mm_load_ps(yPtr + 24);
            vb7 = _mm_load_ps(yPtr + 28);

            va0 = _mm_broadcast_ss(x + i);
            va1 = _mm_broadcast_ss(x + i + 1);
            va2 = _mm_broadcast_ss(x + i + 2);
            va3 = _mm_broadcast_ss(x + i + 3);

            res = _mm_fmadd_ps(vb0, va0, res);
            res1 = _mm_fmadd_ps(vb1, va0, res1);
            res = _mm_fmadd_ps(vb2, va1, res);
            res1 = _mm_fmadd_ps(vb3, va1, res1);
            res = _mm_fmadd_ps(vb4, va2, res);
            res1 = _mm_fmadd_ps(vb5, va2, res1);
            res = _mm_fmadd_ps(vb6, va3, res);
            res1 = _mm_fmadd_ps(vb7, va3, res1);

            yPtr += 32;
        }

        if (fuseBias)
        {
            res  = _mm_add_ps(res, vBias);
            res1 = _mm_add_ps(res1, vBias1);
        }
        if (fuseRelu)
        {
            res = _mm_max_ps(res, vZero);
            res1 = _mm_max_ps(res1, vZero);
        }
        _mm_store_ps((z + 8 * k), res);
        _mm_store_ps((z + 8 * k + 4), res1);
    }
}

template void fully_connected_inference_direct<false, false>(const int, const int, const float *, const float *, float *, const int, float *);
template void fully_connected_inference_direct<false, true>(const int, const int, const float *, const float *, float *, const int, float *);
template void fully_connected_inference_direct<true, false>(const int, const int, const float *, const float *, float *, const int, float *);
template void fully_connected_inference_direct<true, true>(const int, const int, const float *, const float *, float *, const int, float *);

template void fully_connected_transpose_inference<false, false>(const int, const int, const float *, const float *, float *, const int, float *);
template void fully_connected_transpose_inference<false, true>(const int, const int, const float *, const float *, float *, const int, float *);
template void fully_connected_transpose_inference<true, false>(const int, const int, const float *, const float *, float *, const int, float *);
template void fully_connected_transpose_inference<true, true>(const int, const int, const float *, const float *, float *, const int, float *);

#endif

void matrixTranspose(float *array, size_t m, size_t n, float *buffer) //  A[m][n] -> A[n][m]
{
    for (int i = 0; i < m; i++)
        for (int j = 0; j < n; j++)
            buffer[j * m + i] = array[i * n + j];
    memcpy(array, buffer, m * n * sizeof(float));
}


//
//  main.cpp
//  PhyloAcc
//
//  Created by hzr on 3/8/16.
//  Copyright © 2016 hzr. All rights reserved.
//
#include <dirent.h>
#include <stdio.h>
#include <iostream>
#include <fstream>
#include <sstream>
#include <string>
#include <algorithm>
#include <iomanip>
#include <omp.h>
#include <string>
#include <armadillo>
#include "profile.h"
#include "newick.h"
#include "bpp.hpp"
#include "bpp_c.hpp"
#include "utils.h"
#include <math.h>
#include <gsl/gsl_errno.h>

using namespace std;
using namespace arma;

// parameters, input and output files paths
string params_path;
string phytree_path;
string align_path;
string output_path="";
string output_path2="";
string segment_path;
string id_path="";
string result_prefix="test";

//string refspecies;
string outgroup;
string targetspecies;
string conservegroup; // can't be missing in more than 50%
double conserve_prop = 0.8;

int num_thread = 1;

// running parameters
int num_burn = 200;         // num of burn-in updates
int num_mcmc = 800;         // num of MCMC updates,10
int num_thin = 500;    // num of updates between two samples, adaptive frequency
int num_chain; // outer loop of updates Q matrix and hyperparameter of substitution rates

double prep_lrate = 0.3;
double prep_lrate2 = 0.0;//0.1
double prep_grate = 0.5; // initalization

double prior_lrate2_a = 0,prior_lrate2_b = 1 ; // beta prior for lrate2, 0.5
double prior_lrate_a = 1 ,prior_lrate_b = 9 ; // beta prior for lrate, 2
double prior_grate_a = 3,prior_grate_b = 1 ; // beta prior for grate

double ratio0 = 0.5; //initial conserved rate
double ratio1 = 1; // initial accelerated rate
double missing_thres = 0.8;

double nprior_a = 10, nprior_b = 0.2;  //around 1
double cprior_a = 5, cprior_b = 0.04;  //around ratio
int ropt = 1;
double cub = 1;
double nlb = 0.6;

int batch = -1 ;
int seed = 5;
double indel = 0;
double indel2;
bool sample_indel = 0;
bool sample_hyper;
char gapchar = '-';
bool verbose = 0;
double consToMis = 0.01;
bool prune=0;
double revgap=1;
int min_length = 50;

// load the program parameters
void LoadParams(int argc, char* argv[])
{
    cout << "Loading input data and running parameters......" << endl;

    if (argc > 1)
        params_path = string(argv[1]);
    else
        params_path = "/Users/hzr/Phd_2/Bird/PhyloAcc_init3-3/params2s.txt"; //params2

    cout << "Loading program configurations from " << params_path << "......" <<endl;

    const int BUFF_SIZE = 1024;
    char line_buff[BUFF_SIZE];

    ifstream in_params(params_path.c_str());
    if (!in_params)
    {
        cerr << "Cannot open the parameters file: " << params_path.c_str() << endl;
        exit(1);
    }
    while(!in_params.eof())
    {
        in_params.getline(line_buff, BUFF_SIZE);
        istringstream line_stream(line_buff);
        string tmp; line_stream >> tmp;

        // input and output file paths
        if (tmp=="PHYTREE_FILE")
            line_stream >> phytree_path;
        else if (tmp=="ALIGN_FILE")
            line_stream >> align_path;
        else if (tmp=="SEG_FILE")
            line_stream >> segment_path;
        else if (tmp=="ID_FILE")
            line_stream >> id_path;
        else if (tmp=="BATCH")
            line_stream >> batch;
        else if (tmp=="RESULT_FOLDER")
            line_stream >> output_path;
        else if (tmp=="PREFIX")
            line_stream >> result_prefix;

        //else if (tmp=="RESULT_INDIV")
        //    line_stream >> output_path2;

        else if (tmp=="SEED")
            line_stream >> seed;
        else if (tmp=="INIT_CONSERVE_RATE")
            line_stream >> ratio0;
        else if (tmp=="INIT_ACCE_RATE")
            line_stream >> ratio1;
        else if (tmp=="CONSERVE_PRIOR_A")
            line_stream >> cprior_a;
        else if (tmp=="CONSERVE_PRIOR_B")
            line_stream >> cprior_b;
        else if (tmp=="ACCE_PRIOR_A")
            line_stream >> nprior_a;
        else if (tmp=="ACCE_PRIOR_B")
            line_stream >> nprior_b;
        else if (tmp=="ROPT")
            line_stream >> ropt;
        else if (tmp=="CUB")
            line_stream >> cub;
        else if (tmp=="NLB")
            line_stream >> nlb;

        // running parameters
        else if (tmp=="BURNIN")
            line_stream >> num_burn;
        else if (tmp=="MCMC")
            line_stream >> num_mcmc;
        else if (tmp=="ADAPT_FREQ")
            line_stream >> num_thin;
        else if (tmp=="INIT_LRATE")
            line_stream >> prep_lrate;
        else if (tmp=="INIT_GRATE")
            line_stream >> prep_grate;
        else if (tmp=="HYPER_LRATE_A")
            line_stream >> prior_lrate_a;
        else if (tmp=="HYPER_LRATE_B")
            line_stream >> prior_lrate_b;
        else if (tmp=="HYPER_GRATE_A")
            line_stream >> prior_grate_a;
        else if (tmp=="HYPER_GRATE_B")
            line_stream >> prior_grate_b;
        else if (tmp=="CHAIN")
            line_stream >> num_chain;

        // constraint
        else if (tmp == "OUTGROUP")
            line_stream >> outgroup;
        else if (tmp == "TARGETSPECIES")
            line_stream >> targetspecies;
        else if (tmp == "CONSERVE")
            line_stream >> conservegroup;
        else if (tmp == "CONSERVE_PROP")
            line_stream >> conserve_prop;
        else if (tmp == "GAP_PROP")
            line_stream >> missing_thres;
        //else if (tmp == "REF")
         //   line_stream >> refspecies;
        else if (tmp == "CONSTOMIS")
            line_stream >> consToMis;

        // treat indel as additional character
        else if (tmp == "GAPCHAR")
            line_stream >> gapchar;
        else if (tmp == "PRUNE_TREE")
            line_stream >> prune;
        else if (tmp == "TRIM_GAP_PERCENT")
            line_stream >> revgap;
        else if (tmp == "MIN_LEN")
            line_stream >> min_length;
        else if (tmp == "INDEL") // not used
            line_stream >> indel;
        else if (tmp == "INDEL2") // not used
            line_stream >> indel2;
        else if(tmp == "SAMPLE_INDEL")  // not used
            line_stream >> sample_indel;
        else if(tmp == "SAMPLE_HYPER")
            line_stream >> sample_hyper;
        else if(tmp == "VERBOSE")
            line_stream >> verbose;
        else if(tmp == "NUM_THREAD")
            line_stream >> num_thread;
        else if(tmp != "")
            cout << "Unknown parameter: " << tmp <<endl;

    }

    // trimming file names
    phytree_path = strutils::trim(phytree_path, " \"\t\n");
    align_path = strutils::trim(align_path, " \"\t\n");
    output_path  = strutils::trim(output_path,  " \"\t\n");
    //output_path2  = strutils::trim(output_path2,  " \"\t\n");
    segment_path = strutils::trim(segment_path, " \"\t\n");

}

bool DirectoryExists( string pzPath )
{
    if ( pzPath == "") return false;

    DIR *pDir;
    bool bExists = false;

    pDir = opendir (pzPath.c_str());

    if (pDir != NULL)
    {
        bExists = true;
        (void) closedir (pDir);
    }

    return bExists;
}

void DispParams(PhyloProf profile, int seed)
{
    double mean_seg_size = 0;
    for(unsigned int c=0; c<profile.C; c++)
        mean_seg_size += (double)(profile.element_pos[c][1] - profile.element_pos[c][0]) / profile.C;
    cout << "  # total length = " << profile.G << " (" << profile.C << ")" << ". # Species = " << profile.S << ". # elements = " << profile.C << ". Mean gene set size = " << mean_seg_size << "." << endl;
    cout << "# Burn-ins = " << num_burn << ". # MCMC Updates = " << num_mcmc << ". # adaptive frequency = " << num_thin << ".  RND SEED = " << seed << "." << endl ; //
    cout << "# Threads = " << num_thread << endl << endl;
}

int main(int argc, char* argv[])
{
    time_t start = time(NULL);

    cout << std::fixed << setprecision(4);

    // load the program parameters
    LoadParams(argc, argv);

    // check output path
    if(! DirectoryExists(output_path))
    {
    	cout << "output path doesn't exist or empty!" << endl;
    	return 1;
    }

    // load the phylogenetic profile
    PhyloProf profile = LoadPhyloProfiles(align_path,segment_path);

    // load the phylogenetic tree
    PhyloTree phytree = LoadPhyloTree(phytree_path);

    // init and display the running parameters
    //InitParams(profile.G);
    DispParams(profile, seed);

    // create and init the BPP object
    //int pC = 500;  // only read in some elements for testing
    BPP bpp(0, profile, phytree, output_path, targetspecies, outgroup, conserve_prop, conservegroup, ratio0, ratio1, ropt, cub, nlb, nprior_a, nprior_b, cprior_a, cprior_b, seed, prep_grate, prep_lrate, prep_lrate2, prior_grate_a, prior_grate_b,prior_lrate_a, prior_lrate_b,prior_lrate2_a, prior_lrate2_b,  indel, indel2, missing_thres, sample_indel);  //c=1 test run first element

    // remove profile?
    //profile.~PhyloProf();

    //initialize the MCMC sampling
    bpp.InitMCMC(num_burn, num_mcmc, num_thin);

    output_path = output_path + "/" + result_prefix ;
    output_path2 = output_path;
    string outpath_Z0 = output_path + "_rate_postZ_M" +to_string(0) +".txt";  //null
    string outpath_Z1 = output_path + "_rate_postZ_M" +to_string(2) +".txt";  //full
    string outpath_Z2 = output_path + "_rate_postZ_M" +to_string(1) +".txt";  // M1

    string outpath_hyper = output_path+"_hyper.txt";
    ofstream out_hyper(outpath_hyper.c_str());
    out_hyper << "iter\tnprior_a\tnprior_b\tcprior_a\tcprior_b\tprior_l_a\tprior_l_b\tprior_g_a\tprior_g_b\n";
    out_hyper << 0 << "\t"<< nprior_a<< "\t"<< nprior_b <<"\t"<< cprior_a << "\t"<< cprior_b << "\t"<< prior_lrate_a << "\t"<< prior_lrate_b << "\t"<< prior_grate_a << "\t"<< prior_grate_b <<endl;

    ofstream out_lik;
    if(sample_hyper) {
        string outpath_elem = output_path+ "_elem_lik.txt";
        out_lik.open(outpath_elem.c_str());
        out_lik.precision(8);
        out_lik << "No.\tID\tloglik_all\tloglik_Max"<<endl;
    }

    ofstream out_Z0(outpath_Z0.c_str());
    ofstream out_Z1(outpath_Z1.c_str());
    ofstream out_Z2(outpath_Z2.c_str());

    // output species name
    string species_name = output_path+"_species_names.txt";
    ofstream out_species(species_name.c_str());

    out_Z0 << "No.\tn_rate\tc_rate\tg_rate\tl_rate\tl2_rate"; out_Z1 << "No.\tn_rate\tc_rate\tg_rate\tl_rate\tl2_rate"; out_Z2 << "No.\tn_rate\tc_rate\tg_rate\tl_rate\tl2_rate";
    for(int s=0; s<bpp.N;s++){
         for(int k=0;k<4;k++){
            out_Z0 <<"\t"<<bpp.nodes_names[s]<<"_"<<k;
            out_Z1 <<"\t"<<bpp.nodes_names[s]<<"_"<<k;
            out_Z2 <<"\t"<<bpp.nodes_names[s]<<"_"<<k;
         }
         out_species << bpp.nodes_names[s] << endl;
    }
	out_Z0 <<endl; out_Z1 <<endl; out_Z2 <<endl;

    out_species.close();

    double lrate_prop = 0.5, grate_prop = 0.5;

    vector<int> ids;
    if(id_path=="")
    {
        if(batch==-1)
        {
          for(int c =0;c<bpp.C;c++)
          {
            ids.push_back(c);
          }
        }else{
          int temp = ceil(bpp.C/3);
          for(int c =batch*temp ;c< (batch+1)*temp;c++)
          {
            if(c >= bpp.C) break;
            ids.push_back(c);
          }

        }

    }else{
        ifstream in_params(id_path.c_str());
        if (!in_params)
        {
            cerr << "Cannot open the id file: " << id_path.c_str() << endl;
            exit(1);
        }
        while(!in_params.eof())
        {
            const int BUFF_SIZE = 1024;
            char line_buff[BUFF_SIZE];
            in_params.getline(line_buff, BUFF_SIZE);
            istringstream line_stream(line_buff);
            string tmp; line_stream >> tmp;
            tmp = strutils::trim(tmp);
            if(tmp=="") continue;
            ids.push_back(atoi(tmp.c_str()));

        }

    }

    cout << ids.size() << " of elements to be computed" << endl;
    //gsl_set_error_handler_off();
    for(int iter =0; iter<num_chain; iter++)
    {
        cout << "Running MCMC chain " << iter +1 << " ..." << endl;
        // generate next indel parameter, nprior_a, nprior_b, etc...
        //bpp.sample_proposal(iter, lrate_prop, grate_prop,out_hyper);

        // Gibbs sampling
        #pragma omp parallel for schedule (guided) num_threads(num_thread)
        for(std::size_t i = 0; i < ids.size(); i++ )
        {
            int c = ids[i];
            bool filter = false;

            // null model
            try{
            BPP_C bppc(c, profile, bpp, gapchar, missing_thres, filter, verbose, consToMis, prune, revgap, min_length);  // for individual element
            if(filter) {
              if(verbose) cerr << "filter: "<< c <<endl;
              continue;
            }

            if(!sample_hyper)
            {
                bppc.initMCMC(iter,bpp,0);  //not constrain log_prob_back
                bppc.Gibbs(iter,bpp,out_Z0,output_path,output_path2,0,true,sample_hyper, lrate_prop, grate_prop);  // Gibbs run to get Z for each element
                //if(!bppc.failure)
                //{
                bppc.Eval2(bpp,0);
                if(bppc.verbose || bppc.failure) bppc.Output_sampling(iter, output_path2, bpp, 0);
                bppc.Output_init(output_path,output_path2,bpp,out_Z0, 0); //sort rates!!, posterior median of nrate and crate; posterior mean of Z
               // }
                // res model, crate by null model
                bppc.initMCMC(iter,bpp,2);  //not constrain log_prob_back
                bppc.Gibbs(iter,bpp,out_Z2,output_path,output_path2,2,true,sample_hyper, lrate_prop, grate_prop);  // Gibbs run to get Z for each element
                //if(!bppc.failure)
                //{
                bppc.Eval2(bpp,2);
                if(bppc.verbose || bppc.failure) bppc.Output_sampling(iter, output_path2, bpp, 1);
                bppc.Output_init(output_path,output_path2,bpp,out_Z2, 2); //sort rates!!
                //}
            }

            // full model, nrate, crate by res model
            bppc.initMCMC(iter,bpp,1);  //not constrain log_prob_back
            bppc.Gibbs(iter, bpp,out_Z1,output_path,output_path2,1, true, sample_hyper, lrate_prop, grate_prop);  // Gibbs run to get Z for each element
            //if(!bppc.failure)
            //{
            bppc.Eval2(bpp,1);
            if(bppc.verbose || bppc.failure) bppc.Output_sampling(iter, output_path2, bpp, 2);
            bppc.Output_init(output_path,output_path2,bpp,out_Z1, 1); //sort rates!!
            //}

            }catch (exception& e){
              cout << c << " Standard exception: " << e.what() << endl;
            }
        }

        // sample indel, nprior_a, nprior_b, etc...
        try{
          if(sample_hyper) {
            bpp.sample_hyperparam(iter, ids, out_hyper);
            bpp.Output_init0(profile,out_lik, ids);
          }else{
            bpp.Output_init(profile,output_path, ids);
          }
        }catch (exception& e){
              cout << " Standard exception: " << e.what() << endl;
        }
    }

        out_Z0.close();
        out_Z1.close();
        out_Z2.close();
        out_hyper.close();
        out_lik.close();

    cout << endl << endl << "time used:  " << (time(NULL)-start)/60 << " min." << endl << endl;

    return 0;
}


/* //////////////////////////////////////////////////////////////////////

    Recurrent Neural Network Language Model
    v0.95

    一部のコードは "Recurrent neural network based statistical language modeling toolkit Version 0.4a" を元にしています.

////////////////////////////////////////////////////////////////////// */

#include <stdio.h>
#define __STDC_FORMAT_MACROS
#include <inttypes.h>
//#include <stddef.h>
#include <stdlib.h>
#include <string.h>

#include <iostream>
#include <string>
#include <math.h>

#include <time.h>
#include <cfloat>

#include <unordered_map>
#include <boost/unordered_map.hpp>
#include "rnnlmlib_dynamic.h"

#ifndef exp10
    #define exp10(x) pow((double)10, (x))
#endif

namespace RNNLM{

    inline void FreadAllOrDie_dyn(void* ptr, size_t size, size_t count, FILE* fo, const char* message) {/*{{{*/
        size_t read = fread(ptr, size, count, fo);
        if (read != count) {
            fprintf(
                    stderr, "ERROR: expected to read %zu elements, but read %zu elements (%s)\n",
                    count, read, message);
            exit(1);
        }
    }/*}}}*/

    void CRnnLM_dyn::ReadFRHeader(FILE* file) {/*{{{*/
        const uint64_t kVersionStepSize = 10000;
        const int kCurrentVersion = 6;
        const unsigned kMaxLayerTypeName = 64; // maximum size of layer name type in bytes (including \0)
        const std::string kDefaultLayerType = "sigmoid";
        const char* error_message = "error@reading config";

        // READ ...

        // leyer1 のサイズ
        uint64_t quazi_layer_size; //10000 *version + layer size が入っている
        FreadAllOrDie_dyn(&quazi_layer_size, sizeof(int64_t), 1, file, "failed to read layer size.");
        unsigned int layer_size = quazi_layer_size % kVersionStepSize; //
        int version = quazi_layer_size / kVersionStepSize;
        if(version > kCurrentVersion || version < kCurrentVersion){
            fprintf(stderr, "unknown version: %" PRIu64, quazi_layer_size / kVersionStepSize );
            exit(1);
        }

        // direct
        unsigned long int maxent_hash_size;
        FreadAllOrDie_dyn(&maxent_hash_size, sizeof(int64_t), 1, file, error_message);
        unsigned int maxent_order;
        FreadAllOrDie_dyn(&maxent_order, sizeof(int), 1, file, error_message);

        // nce
        real nce_lnz_ = 9; // default value
        bool use_nce_;
        FreadAllOrDie_dyn(&use_nce_, sizeof(bool), 1, file, error_message);
        use_nce = use_nce_;
        FreadAllOrDie_dyn(&nce_lnz_, sizeof(real), 1, file, error_message);
        nce_lnz = nce_lnz_;

        // 読み込むだけのパラメタ (RNNLM toolkit に関係の無いパラメタ)
        bool reverse_sentence;
        char buffer[kMaxLayerTypeName];
        int layer_count;
        int hs_arity;
        FreadAllOrDie_dyn(&reverse_sentence, sizeof(bool), 1, file, error_message);
        FreadAllOrDie_dyn(&buffer, sizeof(char), kMaxLayerTypeName, file, error_message); // 固定長
        FreadAllOrDie_dyn(&layer_count, sizeof(int), 1, file, error_message);
        FreadAllOrDie_dyn(&hs_arity, sizeof(int), 1, file, error_message);

        // 決め打ちでセットするパラメタ (faster-rnnlm には無いパラメタ)
        independent = true; // 文は独立

        // RNNLM 用のパラメタを設定
        layer0_size = vocab_size + layer_size;
        layer1_size = layer_size;
        layerc_size = 0;
        layer2_size = vocab_size;
        direct_size = maxent_hash_size;
        direct_order = maxent_order;

    }/*}}}*/

    real CRnnLM_dyn::calc_direct_score(int word, context* c){/*{{{*/
        unsigned long long hash[MAX_NGRAM_ORDER] = {};
        unsigned long long hash_max = direct_size - vocab_size;
        unsigned int a,b;
        unsigned int d_o = direct_order;
        double direct_score = 0;

        for (a=0; a<d_o; ++a) {
            b=0;
            hash[a]=PRIMES[0]*PRIMES[1];

            for (b=1; b<=a; b++){
                hash[a]+=PRIMES[(a*PRIMES[b]+b)%PRIMES_SIZE]*(unsigned long long)(c->history[b-1]+1);
            }
            hash[a]=(hash[a]%(hash_max));
        }

        for (a=0; a<d_o; ++a) {
            direct_score += (*syn_d)[(hash[a] + word)%hash_max];
            if(a>0 && c->history[a-1] == 0) break;
        }
        return direct_score;
    }/*}}}*/

    real CRnnLM_dyn::random(real min, real max)/*{{{*/
    {
        return rand()/(real)RAND_MAX*(max-min)+min;
    }/*}}}*/

    void CRnnLM_dyn::setRnnLMFile(const char *str)/*{{{*/
    {
        strcpy(rnnlm_file, str);
    }/*}}}*/

    int CRnnLM_dyn::searchVocab(char *word)
    {//{{{
        //*mmfstr = word;
        //std::string stdkey = word;
        //auto vitr = vocab_map->find(*mmfstr);
        auto vitr = vocab_map->find(shash(std::string(word)));
        if( vitr == vocab_map->end()){
            return -1;
        }else{
            return vitr->second;
        }

        return -1; //return OOV if not found
    }//}}}

    void CRnnLM_dyn::saveFullContext(context *dest)
    {//{{{
        int a;

        dest->l1_neuron.resize(layer1_size);
        dest->history.resize(MAX_NGRAM_ORDER);
        dest->last_word = history[0];
        dest->have_recurrent = false;

        //vector<real> l1_neuron = layer1_size;
        //vector<cahr> history = layer1_size;
        for (a=0; a<layer1_size; a++) dest->l1_neuron[a] = neu1[a].ac;
        for (a=0; a<MAX_NGRAM_ORDER; a++) dest->history[a] = history[a];
    }//}}}

    void CRnnLM_dyn::CacheRecurrent(context *dest)
    {//{{{
        dest->have_recurrent = true;
        for (int a=0; a<layer1_size; a++) dest->l1_neuron[a] = neu1[a].ac;
    }//}}}

    void CRnnLM_dyn::restoreFullContext(const context *dest)
    {//{{{
        //std::cerr << "restoreFullContext history_size:" << dest->history.size() << " " << (int)dest->last_word << " ," << (int)dest->history[0] << "," << (int)dest->history[1] << std::endl; //H?
        int a;
        for (a=0; a<layer1_size; a++) neu1[a].ac = dest->l1_neuron[a];
        for (a=0; a<MAX_NGRAM_ORDER; a++) history[a] = dest->history[a];
        if(dest->have_recurrent){
            for (a=0; a<layer1_size; a++) neu1[a].ac = dest->l1_neuron[a];
        }else{
            // TODO: 直接 neu0に 移したほうが速い... 下を後で試す
            for (a=0; a<layer1_size; a++) neu0[a+vocab_size].ac = dest->l1_neuron[a];

            //for (a=0; a<layer1_size; a++) neu1[a].ac = dest->l1_neuron[a];
            //copyHiddenLayerToInput(); // neu0 に移す
        }
    }//}}}

    void CRnnLM_dyn::initNetFR()
    {//{{{

        neu0=(struct neuron *)calloc(layer0_size, sizeof(struct neuron));
        neu1=(struct neuron *)calloc(layer1_size, sizeof(struct neuron));
        neu2=(struct neuron *)calloc(layer2_size, sizeof(struct neuron));

    }//}}}

    // RNNLM モデル，語彙の読み込みを行う(faster-rnnlm 対応版)
    void CRnnLM_dyn::restoreNet_FR()    //will read whole network structure
    {//{{{
        int a, b;
        float fl;

        // Filename
        const std::string model_vocab_file = rnnlm_file;
        const std::string model_weight_file = model_vocab_file + ".nnet"; // ネットワークの重み

        // 語彙 (vocab, vocab_map)
        FILE *vocab_file = fopen(model_vocab_file.c_str(), "rb");
        if (vocab_file == NULL){
            fprintf(stderr, "Error: vocaburary file %s not found. \n", model_vocab_file.c_str());
            exit(1);
        }

        std::string MapVocabFilePath= model_vocab_file+".map";
        if( access( MapVocabFilePath.c_str(), F_OK ) != -1 ){ //メモリマップからの読み込み
            if (debug_mode>0)
                std::cerr << "read mapped file" << std::endl;
            p_file_vocab = new bip::managed_mapped_file(bip::open_read_only, MapVocabFilePath.c_str());
            vocab_map = p_file_vocab->find<umap_vocab>("map_vocab").first;
        }else{ //モデルファイルの読み込み
            if (debug_mode>0)
                std::cerr << "read original file" << std::endl;

            // 大きめに1GB (10M文で85MB程度)とる(あとでshrink)
            unsigned long long map_vocab_size = 1024 * 1024 * 1024;
            // メモリマップファイルを作成
            p_file_vocab = new bip::managed_mapped_file(bip::create_only, MapVocabFilePath.c_str(), map_vocab_size );
            // メモリマップ内にvocablary 用の領域を確保
            vocab_map = p_file_vocab->construct<umap_vocab>("map_vocab")( 0, boost::hash<uint64_t>(), std::equal_to<uint64_t>(), p_file_vocab->get_allocator<hashPair>());

            // 語彙の vocab, と vocab_map への登録
            for (int line_number = 0; !feof(vocab_file); ++line_number) {
                char buffer[MAX_STRING];
                uint64_t count;
                if (fscanf(vocab_file, "%s %" PRIu64 " ", buffer, &count) != 2) {
                    fprintf(stderr, "WARNING: Skipping ill-formed line #%d in the vocabulary\n", line_number);
                    continue;
                }

                int wid = line_number;
                auto hash_key = shash(std::string(buffer));
                if(vocab_map->find(hash_key) == vocab_map->end())
                    (*vocab_map)[shash(std::string(buffer))] = wid;
                else
                    std::cerr << "Collision:" << buffer << ", " << wid << std::endl;

                if(debug_mode>1)
                    std::cerr << buffer << ", " << wid << std::endl;
            }
            // メモリマップへの書き込みを flush
            p_file_vocab->flush();
            // メモリマップのサイズ shrink
            bip::managed_mapped_file::shrink_to_fit(MapVocabFilePath.c_str());
        }
        fclose(vocab_file);

        // vocab_map を全て読み込んでから，vocab に逆向きのmap を作成(デバッグ用)
        vocab_size = vocab_map->size();
        if (debug_mode>0) {
            vocab_file = fopen(model_vocab_file.c_str(), "rb");

            if (vocab!=NULL) free(vocab);
            vocab_max_size=vocab_size+1000;
            vocab=(struct vocab_word *)calloc(vocab_max_size, sizeof(struct vocab_word));    //initialize memory for vocabulary

            for (int line_number = 0; !feof(vocab_file); ++line_number) {
                char buffer[MAX_STRING];
                uint64_t count;
                if (fscanf(vocab_file, "%s %" PRIu64 " ", buffer, &count) != 2) {
                    fprintf(stderr, "WARNING: Skipping ill-formed line #%d in the vocabulary\n", line_number);
                    continue;
                }

                auto wid = searchVocab(buffer);
                // Debug に必要であれば vocab ファイルに登録
                if(wid != -1)
                    strncpy(vocab[wid].word, buffer, strlen(buffer));
            }

            fclose(vocab_file);
        }

        // ニューラルネットワークの重み 読み出し
        FILE *model_file = fopen(model_weight_file.c_str(), "rb");
        if (model_file == NULL){
            fprintf(stderr, "Error: model file %s not found. \n", model_weight_file.c_str());
            exit(1);
        }

        ReadFRHeader(model_file);

        if (debug_mode>0)
            std::cerr << "reading model file" << std::endl;

        if (neu0==NULL) initNetFR(); //memory allocation (neu0, neu1, neu2)

        if (use_nce != true){
            std::cerr << "err:use nce" << std::endl;
            exit(1); //DEB
        }

        const std::string MapFilePath( model_vocab_file + ".nnet.map" );
        if( access( MapFilePath.c_str(), F_OK ) != -1 ){ //読み込みのみ
            p_file_syn = new bip::managed_mapped_file(bip::open_read_only, MapFilePath.c_str());
            if(debug_mode>0)
                std::cerr << "reading RNNLM model" << std::endl;
            syn_vocab_l1 = p_file_syn->find<vector_syn>("syn_vocab_l1").first;
            syn_rec      = p_file_syn->find<vector_syn>("syn_rec").first;
            syn_l1_l2    = p_file_syn->find<vector_syn>("syn_l1_l2").first;
        }else{ //通常読み込み
            unsigned long long syn_size = sizeof(real)*(vocab_size*layer1_size + layer1_size*layer1_size + layer2_size*layer1_size) + 4096;
            p_file_syn = new bip::managed_mapped_file(bip::create_only, MapFilePath.c_str(), syn_size );
            syn_vocab_l1 = p_file_syn->construct<vector_syn>("syn_vocab_l1")(p_file_syn->get_segment_manager());
            syn_rec      = p_file_syn->construct<vector_syn>("syn_rec")(p_file_syn->get_segment_manager());
            syn_l1_l2    = p_file_syn->construct<vector_syn>("syn_l1_l2")(p_file_syn->get_segment_manager());

            if(debug_mode>0)
                std::cerr << "reading embedding" << std::endl;
            syn_vocab_l1->resize(vocab_size*layer1_size);
            for (a=0; a<vocab_size; a++) { //vocab_size < layer_0.size
                for (b=0; b<layer1_size; b++) {
                    fread(&fl, sizeof(fl), 1, model_file); //real
                    (*syn_vocab_l1)[b+a*layer1_size]=fl;
                }
            }

            // TODO: 初期化．どうせ使う時初期化するはずなので，不要かも
            for (a=0; a<layer1_size; a++) {
                neu1[a].ac=0;
            }

            if(debug_mode>0)
                std::cerr << "reading NCE layer " << std::endl;
            // W:layer1 -> output (word embedding => layer1
            syn_l1_l2->resize(layer1_size*layer2_size);
            for (b=0; b<layer2_size; b++) {
                for (a=0; a<layer1_size; a++) {
                    fread(&fl, sizeof(fl), 1, model_file);
                    (*syn_l1_l2)[a+b*layer1_size]=fl;
                }
            }

            if(debug_mode>0)
                std::cerr << "reading reccurent weight" << std::endl;
            // Recurrent weight
            syn_rec->resize(layer1_size*layer1_size);
            for (b=0; b<layer1_size; b++) {
                for (a=0; a<layer1_size; a++) {
                    fread(&fl, sizeof(fl), 1, model_file);
                    (*syn_rec)[a+b*layer1_size]=fl;
                }
            }
            p_file_syn->flush();
        }

        if(debug_mode>0)
            std::cerr << "reading direct weight" << std::endl;

        const std::string FilePath( model_vocab_file + ".direct" );
        if( access( FilePath.c_str(), F_OK ) != -1 ){ //読み込みのみ
            p_file_direct = new bip::managed_mapped_file( bip::open_read_only, FilePath.c_str() );
            syn_d = p_file_direct->find<vector_syn>("MyVector").first;
            if(debug_mode>0)
                std::cerr << "read finished"<< std::endl;
        }else{
            // 初回起動時は Memory mapped file に読み込み
            p_file_direct = new bip::managed_mapped_file( bip::create_only, FilePath.c_str(), direct_size*sizeof(real)+4096 );
            syn_d = p_file_direct->construct<vector_syn>("MyVector")(p_file_direct->get_segment_manager());
            if(debug_mode>0)
                std::cerr << "Creating memory mapped file:" << std::endl;

            real fl;
            (*syn_d).resize(direct_size);
            for (unsigned long long b=0; b<direct_size; b++) {
                fread(&fl, sizeof(real), 1, model_file); //real
                (*syn_d)[b]=fl;
            }
            p_file_direct->flush();
            if(debug_mode>0)
                std::cerr << "Creating memory mapped file: finished."<< std::endl;
        }

        fclose(model_file);
        return;
    }//}}}

    void CRnnLM_dyn::matrixXvector(struct neuron *dest, struct neuron *srcvec, vector_syn *srcmatrix, int matrix_width, int from, int to, int from2, int to2, int type)
    {//{{{
        int a, b;
        real val1, val2, val3, val4;
        real val5, val6, val7, val8;

        if (type==0) {		//ac mod
            for (b=0; b<(to-from)/8; b++) {
                val1=0;
                val2=0;
                val3=0;
                val4=0;

                val5=0;
                val6=0;
                val7=0;
                val8=0;

                for (a=from2; a<to2; a++) {
                    val1 += srcvec[a].ac * (*srcmatrix)[a+(b*8+from+0)*matrix_width];
                    val2 += srcvec[a].ac * (*srcmatrix)[a+(b*8+from+1)*matrix_width];
                    val3 += srcvec[a].ac * (*srcmatrix)[a+(b*8+from+2)*matrix_width];
                    val4 += srcvec[a].ac * (*srcmatrix)[a+(b*8+from+3)*matrix_width];

                    val5 += srcvec[a].ac * (*srcmatrix)[a+(b*8+from+4)*matrix_width];
                    val6 += srcvec[a].ac * (*srcmatrix)[a+(b*8+from+5)*matrix_width];
                    val7 += srcvec[a].ac * (*srcmatrix)[a+(b*8+from+6)*matrix_width];
                    val8 += srcvec[a].ac * (*srcmatrix)[a+(b*8+from+7)*matrix_width];
                }
                dest[b*8+from+0].ac += val1;
                dest[b*8+from+1].ac += val2;
                dest[b*8+from+2].ac += val3;
                dest[b*8+from+3].ac += val4;

                dest[b*8+from+4].ac += val5;
                dest[b*8+from+5].ac += val6;
                dest[b*8+from+6].ac += val7;
                dest[b*8+from+7].ac += val8;
            }

            for (b=b*8; b<to-from; b++) {
                for (a=from2; a<to2; a++) {
                    dest[b+from].ac += srcvec[a].ac * (*srcmatrix)[a+(b+from)*matrix_width];
                }
            }
        }
        else {		//er mod
            for (a=0; a<(to2-from2)/8; a++) {
                val1=0;
                val2=0;
                val3=0;
                val4=0;

                val5=0;
                val6=0;
                val7=0;
                val8=0;

                for (b=from; b<to; b++) {
                    val1 += srcvec[b].er * (*srcmatrix)[a*8+from2+0+b*matrix_width];
                    val2 += srcvec[b].er * (*srcmatrix)[a*8+from2+1+b*matrix_width];
                    val3 += srcvec[b].er * (*srcmatrix)[a*8+from2+2+b*matrix_width];
                    val4 += srcvec[b].er * (*srcmatrix)[a*8+from2+3+b*matrix_width];

                    val5 += srcvec[b].er * (*srcmatrix)[a*8+from2+4+b*matrix_width];
                    val6 += srcvec[b].er * (*srcmatrix)[a*8+from2+5+b*matrix_width];
                    val7 += srcvec[b].er * (*srcmatrix)[a*8+from2+6+b*matrix_width];
                    val8 += srcvec[b].er * (*srcmatrix)[a*8+from2+7+b*matrix_width];
                }
                dest[a*8+from2+0].er += val1;
                dest[a*8+from2+1].er += val2;
                dest[a*8+from2+2].er += val3;
                dest[a*8+from2+3].er += val4;

                dest[a*8+from2+4].er += val5;
                dest[a*8+from2+5].er += val6;
                dest[a*8+from2+6].er += val7;
                dest[a*8+from2+7].er += val8;
            }

            for (a=a*8; a<to2-from2; a++) {
                for (b=from; b<to; b++) {
                    dest[a+from2].er += srcvec[b].er * (*srcmatrix)[a+from2+b*matrix_width];
                }
            }

            if (gradient_cutoff>0)
                for (a=from2; a<to2; a++) {
                    if (dest[a].er>gradient_cutoff) dest[a].er=gradient_cutoff;
                    if (dest[a].er<-gradient_cutoff) dest[a].er=-gradient_cutoff;
                }
        }

        //this is normal implementation (about 3x slower):

        /*if (type==0) {		//ac mod
          for (b=from; b<to; b++) {
          for (a=from2; a<to2; a++) {
          dest[b].ac += srcvec[a].ac * srcmatrix[a+b*matrix_width].weight;
          }
          }
          }
          else 		//er mod
          if (type==1) {
          for (a=from2; a<to2; a++) {
          for (b=from; b<to; b++) {
          dest[a].er += srcvec[b].er * srcmatrix[a+b*matrix_width].weight;
          }
          }
          }*/
    }//}}}

    // 今までの状態を上書きし，破壊することに注意
    void CRnnLM_dyn::computeNet_selfnm(int word, context *context)
    {//{{{
        int a, b;
        int last_word_local = context->history[0];

        // 語彙数次元のベクトル
        if (last_word_local!=-1) neu0[last_word_local].ac=1;

        // 0-(vocab)-(layer1)-layer0_size
        // 0...vocab(layer0_size-layer1_size -1) が語彙の次元
        // layer0_size-layer1_size > layer0_size がコンテクストの次元
        restoreFullContext(context); // コンテクストをneu0 のコンテクストへコピー，history をセット

        // 未知語に対しては計算しない
        if(word == -1) return;

        if(context->have_recurrent){
            // context が recurrent に含まれるなら，それを読み込むだけに留める.

        }else{
            //propagate 0->1
            for (a=0; a<layer1_size; a++) neu1[a].ac=0; //初期化

            // コンテクストvectorについての計算 context vector X reccurrent weight
            auto *syn_r = syn_rec; //syn0 +vocab_size*layer1_size;
            auto *neu_r = neu0 +vocab_size;
            matrixXvector(neu1, neu_r, syn_r, layer1_size, 0, layer1_size, 0, layer1_size, 0);

            CacheRecurrent(context);
        }

        // last_word について計算(one hot)
        if(last_word_local != -1)
            for (b=0; b<layer1_size; b++) {
                a=last_word_local;
                neu1[b].ac += (*syn_vocab_l1)[b+a*layer1_size];
            }

        // layer 1 の activation(sigmoid)
        for (a=0; a<layer1_size; a++) {
            neu1[a].ac=1/(1+std::exp(-neu1[a].ac));
        }

        //1->2 class
        double direct_score = 0.0;
        if (direct_size>0) { //RNNME を使う場合
            direct_score = calc_direct_score(word, context);
        }

        double rnn_score = 0.0;
        for (a=0; a<layer1_size; a++) {
            rnn_score += neu1[a].ac * (*syn_l1_l2)[a + word*layer1_size];
        }

        // exp 前 スコア
        neu2[word].ac = std::exp( rnn_score + direct_score - nce_lnz );

        if(debug_mode > 0)
            std::cerr << "p(" << vocab[word].word << "|" << vocab[context->history[0]].word << ", " << vocab[context->history[1]].word << ", ...) = " << neu2[word].ac << " (nn_score:" << rnn_score << " direct_score:" << direct_score << ")"<< std::endl;

    }//}}}

    void CRnnLM_dyn::copyHiddenLayerToInput()
    {//{{{
        //std::cout << "copyHiddenLayerToInput"<< std::endl;
        int a;

        for (a=0; a<layer1_size; a++) {
            neu0[a+layer0_size-layer1_size].ac=neu1[a].ac;
        }
    }//}}}

    void CRnnLM_dyn::get_initial_context_FR(context *c)
    {//{{{
        if (debug_mode>0) std::cerr << "initializing RNNLM FR" << std::endl;
        restoreNet_FR(); // initialize モデル読込  重い
        //computeNet(0, 0); // initialize
        copyHiddenLayerToInput();
        //saveContext();
        //saveContext2(); //必要？
        for (int a=0; a<MAX_NGRAM_ORDER; a++) history[a]=-1;
        history[0]=0;
        saveFullContext(c); //文頭としてInitial context を作成
    }//}}}

    real CRnnLM_dyn::test_word_selfnm(context *c, context *new_c, std::string next_word, size_t word_length)
    {//{{{
        int last_word;
        last_word = c->last_word;
        real senp;

        real lambda=1;
        real logp=0;
        senp=0; //

        int word = searchVocab((char*)next_word.c_str());

        // 文区切りを0に対応させる アドホックな対処
        if(next_word == "<EOS>" || next_word == "<BOS>")
            word = 0;

        // RNN の実行 (結果はneu2[word].ac に書き込まれる
        computeNet_selfnm(word,c);

        double ln_score = neu2[word].ac;

        if (word!=-1) { //OOVでない
            logp+=log10(ln_score);
            senp+=log10(ln_score*lambda);
        } else {
            //assign to OOVs some score to correctly rescore nbest lists, reasonable value can be less than 1/|V| or backoff LM score (in case it is trained on more data)
            //this means that PPL results from nbest list rescoring are not true probabilities anymore (as in open vocabulary LMs)

            // 文字の長さに対してlinear に設定する
            real oov_penalty=-5; //log penalty
            if(lpenalty){ //penalty を文字の長さに対して線形に与える.
               oov_penalty -= lweight * word_length;
            }

            logp+=oov_penalty;
            senp+=oov_penalty;
        }

        copyHiddenLayerToInput(); //必要？
        if (last_word!=-1) neu0[last_word].ac=0;  //delete previous activation

        // history の更新
        for (int a=MAX_NGRAM_ORDER-1; a>0; a--)
            history[a] = c->history[a-1];
        history[0] = word;

        // context の保存 // history の渡し方が暗黙的
        saveFullContext(new_c);

        return senp;
    }//}}}

}


/*
 * (C) Copyright 2021 Met Office UK
 *
 * This software is licensed under the terms of the Apache Licence Version 2.0
 * which can be obtained at http://www.apache.org/licenses/LICENSE-2.0.
 */

#include <Eigen/Dense>
#include <algorithm>
#include <set>

#include "ioda/ObsDataVector.h"
#include "oops/util/IntSetParser.h"
#include "oops/util/missingValues.h"
#include "ufo/filters/obsfunctions/CloudCostFunction.h"
#include "ufo/filters/Variable.h"
#include "ufo/utils/ufo_utils.interface.h"

namespace ufo {

static ObsFunctionMaker<CloudCostFunction> makerCloudCostFunction_("CloudCostFunction");

CloudCostFunction::CloudCostFunction(const eckit::LocalConfiguration & conf)
  : invars_() {
  // Initialize options
  options_.deserialize(conf);

  // List of field names for B matrix
  fields_ = options_.field_names.value();

  // Get channels for computing scattering index from options
  std::set<int> chanset = oops::parseIntSet(options_.chanlist.value());
  channels_.assign(chanset.begin(), chanset.end());

  // List of required data
  for (size_t i = 0; i < fields_.size(); ++i) {
    invars_ += Variable("brightness_temperature_jacobian_"+fields_[i]+"@ObsDiag", channels_);
  }
  invars_ += Variable("brightness_temperature@ObsValue", channels_);
  invars_ += Variable("brightness_temperature@"+options_.HofXGroup.value(), channels_);
  invars_ += Variable("latitude@MetaData");
  if (options_.qtotal_lnq_gkg.value()) {
    invars_ += Variable("specific_humidity@GeoVaLs");
    invars_ += Variable("mass_content_of_cloud_liquid_water_in_atmosphere_layer@GeoVaLs");
    invars_ += Variable("mass_content_of_cloud_ice_in_atmosphere_layer@GeoVaLs");
    invars_ += Variable("air_pressure@GeoVaLs");
    invars_ += Variable("air_temperature@GeoVaLs");
    invars_ += Variable("surface_pressure@GeoVaLs");
    invars_ += Variable("surface_temperature@GeoVaLs");
    invars_ += Variable("specific_humidity_at_two_meters_above_surface@GeoVaLs");
  }
}

// -----------------------------------------------------------------------------

void CloudCostFunction::compute(const ObsFilterData & in,
                                    ioda::ObsDataVector<float> & out) const {
  // Get dimensions
  const size_t nlocs = in.nlocs();
  const size_t nchans = channels_.size();
  ASSERT(nchans > 0);
  ASSERT(out.nvars() == 1);

  // B, R error covariance objects
  eckit::LocalConfiguration bMatrixConf;
  bMatrixConf.set("BMatrix", options_.bmatrix_filepath.value());
  bMatrixConf.set("background fields", options_.field_names.value());
  bMatrixConf.set("qtotal", options_.qtotal_lnq_gkg.value());
  MetOfficeBMatrixStatic staticB(bMatrixConf);

  eckit::LocalConfiguration rMatrixConf;
  rMatrixConf.set("RMatrix", options_.rmatrix_filepath.value());
  MetOfficeRMatrixRadiance staticR(rMatrixConf);

  bool split_rain = options_.qtotal_split_rain.value();

  const std::string clw_name = "mass_content_of_cloud_liquid_water_in_atmosphere_layer";
  const std::string ciw_name = "mass_content_of_cloud_ice_in_atmosphere_layer";
  std::vector<float> gv_pres(nlocs), gv_temp(nlocs), gv_qgas(nlocs), gv_clw(nlocs), gv_ciw(nlocs),
                     humidity_total(nlocs);

  // Determine if pressure is ascending or descending (B-matrix assumption)
  size_t np = in.nlevs(Variable("air_pressure@GeoVaLs"));
  std::vector<float> gv_pres_1(nlocs), gv_pres_N(nlocs);
  in.get(Variable("air_pressure@GeoVaLs"), 1, gv_pres_1);
  in.get(Variable("air_pressure@GeoVaLs"), np, gv_pres_N);
  const float missing = util::missingValue(missing);
  ASSERT(gv_pres_1[0] != missing);
  ASSERT(gv_pres_N[0] != missing);
  bool p_ascending = (gv_pres_N[0] > gv_pres_1[0]);

  // Assemble combined Jacobian from component fields
  std::vector<std::vector<std::vector<float>>>
       jac_vec(nlocs, std::vector<std::vector<float>>(nchans, std::vector<float>()));
  for (size_t ifield = 0; ifield < fields_.size(); ++ifield) {
    if (options_.qtotal_lnq_gkg.value() &&
            (fields_[ifield] == clw_name || fields_[ifield] == ciw_name)) {
      // qtotal ln(g/kg) Jacobian calculated when "specific_humidity" is reached in field list
      continue;
    }
    std::string jac_name = "brightness_temperature_jacobian_"+fields_[ifield]+"@ObsDiag";
    size_t nlevs = in.nlevs(Variable(jac_name, channels_)[0]);
    std::vector<float> jac_store(nlocs);
    for (size_t ilev = 0; ilev < nlevs; ++ilev) {
      int level_gv = (p_ascending ? ilev+1 : nlevs-ilev);
      int level_jac = (options_.reverse_Jacobian.value() ? nlevs-level_gv+1 : level_gv);
      if (fields_[ifield] == "specific_humidity" && options_.qtotal_lnq_gkg.value()) {
        in.get(Variable("air_pressure@GeoVaLs"), level_gv, gv_pres);
        in.get(Variable("air_temperature@GeoVaLs"), level_gv, gv_temp);
        in.get(Variable("specific_humidity@GeoVaLs"), level_gv, gv_qgas);
        in.get(Variable(clw_name+"@GeoVaLs"), level_gv, gv_clw);
        in.get(Variable(ciw_name+"@GeoVaLs"), level_gv, gv_ciw);
        std::vector<float> qsaturated(nlocs);
        ufo_ops_satrad_qsatwat_f90(qsaturated.data(), gv_temp.data(), gv_pres.data(),
                                   static_cast<int>(nlocs));
        for (size_t iloc = 0; iloc < nlocs; ++iloc) {
          // Ensure specific humidity is within limits
          gv_qgas[iloc] = std::max(gv_qgas[iloc], options_.min_q.value());
          gv_qgas[iloc] = std::min(gv_qgas[iloc], qsaturated[iloc]);
          humidity_total[iloc] = gv_qgas[iloc] + gv_clw[iloc] + gv_ciw[iloc];
        }
      }

      for (size_t ichan = 0; ichan < nchans; ++ichan) {
        in.get(Variable(jac_name, channels_)[ichan], level_jac, jac_store);

        if (fields_[ifield] == "specific_humidity" && options_.qtotal_lnq_gkg.value()) {
          std::vector<float> jac_clw(nlocs), jac_ciw(nlocs);
          in.get(Variable("brightness_temperature_jacobian_"+clw_name+"@ObsDiag", channels_)[ichan],
                 level_jac, jac_clw);
          in.get(Variable("brightness_temperature_jacobian_"+ciw_name+"@ObsDiag", channels_)[ichan],
                 level_jac, jac_ciw);
          std::vector<float> dq_dqtotal(nlocs), dql_dqtotal(nlocs), dqi_dqtotal(nlocs);
          int qsplit_mode = 2;  // compute derivatives
          ufo_ops_satrad_qsplit_f90(qsplit_mode, static_cast<int>(nlocs), gv_pres.data(),
                                    gv_temp.data(), humidity_total.data(), dq_dqtotal.data(),
                                    dql_dqtotal.data(), dqi_dqtotal.data(), split_rain);
          // Jacobian dy/dx for observation y, humdity x in units kg/kg
          // For alternative units of B-matrix humidity z in ln(g/kg)
          // chain rule gives dy/dz = x.(dy/dx)
          // Gradient due to ice is ignored unless we are using scattering radiative transfer
          // dTb/dln(qt) = qt*(dTb/dq*dq/dqt + dTb/dql*dql/dqt [+ dTb/dqi*dqi/dqt])
          for (size_t iloc = 0; iloc < nlocs; ++iloc) {
            jac_store[iloc] *= dq_dqtotal[iloc];
            jac_store[iloc] += jac_clw[iloc]*dql_dqtotal[iloc];
            if (options_.scattering_switch.value()) {
              jac_store[iloc] += jac_ciw[iloc]*dqi_dqtotal[iloc];
            }
            jac_store[iloc] *= humidity_total[iloc];
          }
        }

        if (fields_[ifield] == "specific_humidity_at_two_meters_above_surface"
            && options_.qtotal_lnq_gkg.value()) {
          in.get(Variable("surface_pressure@GeoVaLs"), level_gv, gv_pres);
          in.get(Variable("surface_temperature@GeoVaLs"), level_gv, gv_temp);
          in.get(Variable("specific_humidity_at_two_meters_above_surface@GeoVaLs"),
                 level_gv, gv_qgas);
          std::vector<float> qsaturated(nlocs);
          ufo_ops_satrad_qsatwat_f90(qsaturated.data(), gv_temp.data(), gv_pres.data(),
                                     static_cast<int>(nlocs));
          for (size_t iloc = 0; iloc < nlocs; ++iloc) {
            gv_qgas[iloc] = std::max(gv_qgas[iloc], options_.min_q.value());
            gv_qgas[iloc] = std::min(gv_qgas[iloc], qsaturated[iloc]);
            // dTb/dln(q2m) = q2m*(dTb/dq2m)
            jac_store[iloc] *= gv_qgas[iloc];
          }
        }

        for (size_t iloc = 0; iloc < nlocs; ++iloc) {
          jac_vec[iloc][ichan].push_back(jac_store[iloc]);
        }
      }
    }
  }

  const size_t sizeB = staticB.getsize();
  for (size_t iloc = 0; iloc < nlocs; ++iloc) {
    for (size_t ichan = 0; ichan < nchans; ++ichan) {
      ASSERT(jac_vec[iloc][ichan].size() == sizeB);
    }
  }

  // Get departures = ObsValue - HofX (where HofX is bias corrected)
  std::vector<std::vector<float>> departures(nlocs, std::vector<float>(nchans));
  std::vector<float> obsvalues(nlocs);
  std::vector<float> bgvalues(nlocs);
  std::vector<bool> is_out_of_bounds(nlocs, false);
  for (size_t ichan = 0; ichan < nchans; ++ichan) {
    in.get(Variable("brightness_temperature@ObsValue", channels_)[ichan], obsvalues);
    in.get(Variable("brightness_temperature@"+options_.HofXGroup.value(), channels_)[ichan],
           bgvalues);
    for (size_t iloc = 0; iloc < nlocs; ++iloc) {
      departures[iloc][ichan] = obsvalues[iloc] - bgvalues[iloc];
      // Flag observations outside expected bounds
      if (obsvalues[iloc] < options_.minTb.value() || obsvalues[iloc] > options_.maxTb.value()) {
        is_out_of_bounds[iloc] = true;
      }
    }
  }

  std::vector<float> latitude(nlocs);
  in.get(Variable("latitude@MetaData"), latitude);
  Eigen::MatrixXf Hmatrix(nchans, sizeB);

  for (size_t iloc = 0; iloc < nlocs; ++iloc) {
    if (is_out_of_bounds[iloc]) {
      out[0][iloc] = options_.maxCost.value();
      continue;
    }

    for (size_t ichan = 0; ichan < nchans; ++ichan) {
      Hmatrix.row(ichan) = Eigen::Map<Eigen::VectorXf>(jac_vec[iloc][ichan].data(), sizeB);
    }

    // Matrix of departures dy
    Eigen::Map<Eigen::VectorXf> dy(departures[iloc].data(), nchans);

    // Calculate Scratch_matrix = H.B.H^T + R
    Eigen::MatrixXf BHT;
    staticB.multiply(latitude[iloc], Hmatrix.transpose(), BHT);
    Eigen::MatrixXf HBHT = Hmatrix*BHT;
    Eigen::MatrixXf Scratch_matrix;
    staticR.add(channels_, HBHT, Scratch_matrix);

    // Calculate Scratch_matrix2 = Scratch_matrix^-1.dy using Cholesky decomposition
    Eigen::LLT<Eigen::MatrixXf> decomposition(Scratch_matrix);
    if (decomposition.info() == Eigen::NumericalIssue) {
      oops::Log::warning() <<
        "CloudCostFunction Scratch_matrix appears not to be positive definite" << std::endl;
      out[0][iloc] = options_.maxCost.value();
      continue;
    }
    Eigen::VectorXf Scratch_matrix2 = decomposition.solve(dy);

    // Final cost
    float Cost_final = 0.5*dy.transpose()*Scratch_matrix2;
    Cost_final /= static_cast<float>(nchans);  // normalise by number of channels
    out[0][iloc] = std::min(Cost_final, options_.maxCost.value());
  }
}

// -----------------------------------------------------------------------------

const ufo::Variables & CloudCostFunction::requiredVariables() const {
  return invars_;
}

// -----------------------------------------------------------------------------

}  // namespace ufo