//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_LINALG_FUNCTIONS_PINV_HPP_INCLUDED
#define NT2_LINALG_FUNCTIONS_PINV_HPP_INCLUDED
#include <nt2/include/functor.hpp>
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>
#include <nt2/sdk/memory/container.hpp>
#include <nt2/sdk/meta/size_as.hpp>
#include <nt2/sdk/meta/value_as.hpp>
#include <nt2/core/container/dsl/value_type.hpp>
#include <nt2/core/container/dsl/size.hpp>
#include <nt2/include/functions/issquare.hpp>

/*!
 * @brief inv  pseudo-inverse of matrix.
 * pinv(x) is the pseudo-inverse of the matrix expression x.
 *
 * x = pinv(a) produces a matrix x of the same dimensions
 * as trans(a) so that a*x*a = a, x*a*x = x and a*x and x*a
 * are hermitian. the computation is based on svd(a) and any
 * singular values less than a tolerance are treated as zero.
 * the default tolerance is max(size(a)) * norm(a) * eps<class(a)> .
 *
 * pinv(a,tol) uses the tolerance tol instead of the default.
 *
 **/

namespace nt2 { namespace tag
  {
    /*!
     * \brief Define the tag pinv_ of functor pinv
     *        in namespace nt2::tag for toolbox algebra
    **/
    struct pinv_ :  ext::unspecified_<pinv_> { typedef ext::unspecified_<pinv_> parent; };
  }

  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::pinv_, pinv, 1)
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::pinv_, pinv, 2)

}

namespace nt2 { namespace ext
{
  template<class Domain, class Expr,  int N>
  struct  size_of<tag::pinv_, Domain, N, Expr>
  {
    typedef typename boost::proto::result_of::child_c<Expr&,0>::value_type  c0_t;
    typedef typename c0_t::extent_type                               result_type;
    BOOST_FORCEINLINE result_type operator()(Expr& e) const
    {
      result_type sizee = boost::proto::child_c<0>(e).extent();
      std::swap(sizee[0], sizee[1]);
      return sizee;
    }
  };

 template <class Domain, class Expr,  int N>
 struct value_type < tag::pinv_, Domain,N,Expr>
  : meta::value_as<Expr,0>
 {};
} }

#endif


//
// $Id$
//
//
// Original author: Darren Kessner <darren@proteowizard.org>
//
// Copyright 2006 Louis Warschaw Prostate Cancer Center
//   Cedars Sinai Medical Center, Los Angeles, California  90048
//
// 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 "TransientData.hpp"
#include "pwiz/data/misc/FrequencyData.hpp"
#include "pwiz/utility/misc/unit.hpp"
#include <boost/filesystem/operations.hpp>
#include <iostream>
#include <cstring>
#include <fstream>
#include <stdexcept>
#include <iterator>

using namespace std;
using namespace pwiz::data;
using namespace pwiz::util;

ostream* os_ = 0;

const double startTime_ = 0.116800;
const double observationDuration_ = .768;
const double A_ = 1.075339687500000e+008;
const double B_ = -3.454602661132810e+008;
const unsigned int sampleCount_ = 100;

string filename_ = "TransientDataTest_test.dat";
string filenameText_ = "TransientDataTest_test.dat.txt";

void createTestTransients()
{
    TransientData td;
    td.startTime(startTime_);
    td.observationDuration(observationDuration_);
    td.A(A_);
    td.B(B_);

    td.data().resize(sampleCount_);
    fill(td.data().begin(), td.data().end(), 1);

    td.write(filename_);
    td.write(filenameText_, TransientData::Text);
}

void testBasic(const TransientData& td)
{
    unit_assert(td.startTime() == startTime_);
    unit_assert(td.data().size() == sampleCount_);
    unit_assert_equal(td.observationDuration(), observationDuration_, 1e-12);
    unit_assert_equal(td.A(), A_, 1e-12);
    unit_assert_equal(td.B(), B_, 1e-3);
    unit_assert_equal(td.bandwidth(), sampleCount_/observationDuration_/2, 1e-10);

    unit_assert(td.data().size() == sampleCount_);
    for (unsigned int i=0; i<sampleCount_; i++)
        unit_assert(td.data()[i] == 1);

    if (os_) *os_
        << setprecision(12)
        << "Start time: " << td.startTime() << endl
        << "Observation duration: " << td.observationDuration() << endl
        << "Calibration parameter A: " << td.A() << endl
        << "Calibration parameter B: " << td.B() << endl
        << "Number of samples: " << td.data().size() << endl
        << "Bandwidth: " << td.bandwidth() << endl
        << "Magnetic field = " << td.magneticField() << endl << endl;
}

void test()
{
    createTestTransients();

    TransientData td(filename_);
    testBasic(td);

    TransientData tdText(filenameText_);
    testBasic(tdText);

    boost::filesystem::remove(filename_);
    boost::filesystem::remove(filenameText_);
}

class TestSignal : public TransientData::Signal
{
    public:

    virtual double operator()(double t) const
    {
        // sum of two decaying sinusoids at frequencies 100000 and 101000
        return exp(-t)*(cos(100000*2*M_PI*t) + cos(101000*2*M_PI*t));
    }
};

void testAdd()
{
    if (os_) *os_ << "testAdd()\n";

    if (os_) *os_ << "creating signal with two peaks\n";

    TransientData td;
    td.observationDuration(.768);
    td.A(A_);
    td.B(B_);

    td.data().resize(1048576);
    td.add(TestSignal());

    if (os_) *os_ << "computing fft\n";

    FrequencyData fd;
    td.computeFFT(1, fd);

    // "peak detection"

    double threshold = 100000 * fd.noiseFloor();
    vector<FrequencyDatum> peaks;

    for (FrequencyData::iterator it=fd.data().begin(); it!=fd.data().end(); ++it)
        if (abs(it->y) > threshold)
            peaks.push_back(*it);

    if (os_)
    {
        *os_ << "found peaks: " << peaks.size() << endl;
        copy(peaks.begin(), peaks.end(), ostream_iterator<FrequencyDatum>(*os_, "\n"));
    }

    unit_assert(peaks.size() == 2);
    unit_assert(peaks[0].x == 100000);
    unit_assert(peaks[1].x == 101000);
}

int main(int argc, char* argv[])
{
    try
    {
        if (argc>1 && !strcmp(argv[1],"-v")) os_ = &cout;
        if (os_) *os_ << "TransientDataTest\n";
        test();
        testAdd();
        return 0;
    }
    catch (exception& e)
    {
        cerr << e.what() << endl;
        return 1;
    }
}


//
//  Jacobian.cpp
//  Eigen_test
//
//  Created by Emil Iliev on 18.10.19.
//  Copyright © 2019 Emil Iliev. All rights reserved.
//

#include "Jacobian.hpp"
#include <iostream>
#include <Eigen/SVD>

Jacobian::Jacobian() : mtxf(MatrixFactory::getInstance()){
}

void Jacobian::setJacobianConfiguration( unsigned int row , unsigned int col ) {
    //We don't know configuration
    _jacobian.resize(row , col);
}

Eigen::MatrixXf& Jacobian::getJacobian() {
    //Get calculated Jacobian
    return _jacobian;
}

// Transformation matrix representation
//
//|r11 r12 r13 d14|
//|r21 r22 r23 d24|
//|r31 r32 r33 d34|
//|  0   0   0   1|
//

/************************************************************************/
/* Main routine for J calculation                                       */
/************************************************************************/
void Jacobian::calculateJacobian( HomMatrixHolder& hom_matrix_handler , JointHandler& jhandler , Eigen::Matrix4f& full) {
    unsigned int col_num = jhandler.size();

    if (!col_num) {
        //Zero size not allowed
        return;
    }

    unsigned int row_num = NUMBEROFSETPARAMETERS;
    unsigned int size = hom_matrix_handler.size();
    //Set J confiruration
    setJacobianConfiguration(row_num , col_num);

    std::vector<Eigen::Matrix4f> trans_matrix_holder;
    for (int index = 0; index < size + 1; ++index) {
        trans_matrix_holder.push_back(Eigen::Matrix4f::Identity());
    }

    for (int index = 1; index < size + 1; ++index) {
        trans_matrix_holder[index] = trans_matrix_holder[index - 1] * hom_matrix_handler[index - 1];
    }

    for (unsigned int i = 1 ; i < col_num + 1 ; ++i) {
        calculateColumnOfJacobian_New(trans_matrix_holder[i - 1],DHINDEX(i),jhandler[DHINDEX(i)].getJointType(),full);
    }
}

Jacobian* Jacobian::_instance = NULL;

Jacobian* Jacobian::getInstance() {
    if (!_instance)
        _instance = new Jacobian();

    return _instance;
}

Eigen::MatrixXf Jacobian::psevdoInverse() {
    Eigen::MatrixXf inv;
    Eigen::JacobiSVD<Eigen::MatrixXf> svd;
    svd.compute(_jacobian , Eigen::ComputeThinU | Eigen::ComputeThinV);
//    svd.pinv(inv);
    double epsilon = std::numeric_limits<double>::epsilon();
    double tolerance = epsilon * std::max(_jacobian.cols(), _jacobian.rows()) *svd.singularValues().array().abs()(0);
    inv = svd.matrixV() * (svd.singularValues().array().abs() > tolerance).select(svd.singularValues().array().inverse(), 0).matrix().asDiagonal() * svd.matrixU().adjoint();
    return inv;
}

void Jacobian::calculateColumnOfJacobian_New(Eigen::Matrix4f& hom_matrix_handler, unsigned int ind, JointT jt, Eigen::Matrix4f& fullm) {
    Eigen::Vector3f z0(0.0f , 0.0f , 1.0f);
    Eigen::Vector3f zi;
    Eigen::Matrix4f transf_matrix = hom_matrix_handler;
    Eigen::Matrix3f rot_m;

    Eigen::Vector3f p_end_effector;
    Eigen::Vector3f pi;

    //Position of end effector
    p_end_effector<< fullm(0,3), fullm(1,3), fullm(2,3);
    rot_m = transf_matrix.block(0,0,3,3);

    //
    //  Zi-1
    //
    zi = rot_m * z0;
    pi << transf_matrix(0,3) , transf_matrix(1,3) , transf_matrix(2,3);

    //
    //  (Pe - Pi-1)
    //
    Eigen::Vector3f delta_vec = p_end_effector - pi;

    //
    //  Zi x (Pe - Pi-1)
    //
    Eigen::Vector3f d_rev = zi.cross(delta_vec);

    //We should get type of joint and go further
    switch(jt) {
    case PRISMATIC:
        //For prismatic joint everything is simple :
        //        | z | <--- calculated vector z
        // Cind = |   |
        //        | 0 | <--- zero vector3f
        _jacobian(0,ind) = zi(0);
        _jacobian(1,ind) = zi(1);
        _jacobian(2,ind) = zi(2);
        _jacobian(3,ind) = 0.0f;
        _jacobian(4,ind) = 0.0f;
        _jacobian(5,ind) = 0.0f;
        break;
    case REVOLUTE:
        //For revolute joint everything is harder :
        //        | z * d | <--- calculated vector z * vector d
        // Cind = |       |
        //        |   z   | <--- calculated vector z
        _jacobian(0,ind) = d_rev(0);
        _jacobian(1,ind) = d_rev(1);
        _jacobian(2,ind) = d_rev(2);
        _jacobian(3,ind) = zi(0);
        _jacobian(4,ind) = zi(1);
        _jacobian(5,ind) = zi(2);
        break;
    default:
        break;
    }
}


#include <gauxc/xc_integrator/xc_host_util.hpp>

#include "host_weights.hpp"
#include "host_collocation.hpp"
#include "host_zmat.hpp"
#include "integrator_common.hpp"
#include "blas.hpp"
#include "util.hpp"

#include <Eigen/Core>

namespace GauXC  {
namespace integrator::host {

template <typename F, size_t n_deriv>
void process_batches_host_replicated_p(
  XCIntegratorState      integrator_state,
  XCWeightAlg            weight_alg,
  const functional_type& func,
  const BasisSet<F>&     basis,
  const Molecule   &     mol,
  const MolMeta    &     meta,
  XCHostData<F>    &     host_data,
  std::vector< XCTask >& tasks,
  const F*               P,
  F*                     VXC,
  F*                     exc,
  F*                     n_el
) {

  const int32_t nbf = basis.nbf();

  auto task_comparator = []( const XCTask& a, const XCTask& b ) {
    return (a.points.size() * a.nbe) > (b.points.size() * b.nbe);
  };
  std::sort( tasks.begin(), tasks.end(), task_comparator );

  if( not integrator_state.modified_weights_are_stored )
    partition_weights_host( weight_alg, mol, meta, tasks );

  std::fill( VXC, VXC + size_t(nbf)*nbf, F(0.) );
  *exc = 0.;

  size_t ntasks = tasks.size();
  for( size_t iT = 0; iT < ntasks; ++iT ) {

    auto& task = tasks[iT];

    const int32_t  npts    = task.points.size();
    const int32_t  nbe     = task.nbe;
    const int32_t  nshells = task.shell_list.size();

    const F* points      = task.points.data()->data();
    const F* weights     = task.weights.data();
    const int32_t* shell_list = task.shell_list.data();

    F* basis_eval = host_data.basis_eval.data();
    F* den_eval   = host_data.den_scr.data();
    F* nbe_scr    = host_data.nbe_scr.data();
    F* zmat       = host_data.zmat.data();

    F* eps        = host_data.eps.data();
    F* gamma      = host_data.gamma.data();
    F* vrho       = host_data.vrho.data();
    F* vgamma     = host_data.vgamma.data();

    F* dbasis_x_eval = nullptr;
    F* dbasis_y_eval = nullptr;
    F* dbasis_z_eval = nullptr;
    F* dden_x_eval = nullptr;
    F* dden_y_eval = nullptr;
    F* dden_z_eval = nullptr;

    if( n_deriv > 0 ) {
      dbasis_x_eval = basis_eval    + npts * nbe;
      dbasis_y_eval = dbasis_x_eval + npts * nbe;
      dbasis_z_eval = dbasis_y_eval + npts * nbe;
      dden_x_eval   = den_eval    + npts;
      dden_y_eval   = dden_x_eval + npts;
      dden_z_eval   = dden_y_eval + npts;
    }

    // Get the submatrix map for batch
    auto submat_map = gen_compressed_submat_map( basis, task.shell_list );

    // Evaluate Collocation Matrix
    if( n_deriv == 1 )
      eval_collocation_deriv1( npts, nshells, nbe, points, basis, shell_list,
                               basis_eval, dbasis_x_eval, dbasis_y_eval,
                               dbasis_z_eval );
    else
      eval_collocation( npts, nshells, nbe, points, basis, shell_list, basis_eval );

    // Extrat Submatrix
    const F* den_ptr_use = P;
    if( nbe != nbf ) {
      detail::submat_set( nbf, nbf, nbe, nbe, P, nbf, nbe_scr, nbe, submat_map );
      den_ptr_use = nbe_scr;
    }

    // Z = P * BF
    GauXC::blas::gemm( 'N', 'N', nbe, npts, nbe, 1., den_ptr_use, nbe,
                       basis_eval, nbe, 0., zmat, nbe );

    // Evaluate the density
    for( int32_t i = 0; i < npts; ++i ) {

      const size_t ioff = size_t(i) * nbe;
      const F*     zmat_i = zmat + ioff;

      den_eval[i] =
        2. * GauXC::blas::dot( nbe, basis_eval + ioff, 1, zmat_i, 1 );

      if( n_deriv > 0 ) {
        const F dx =
          4. * GauXC::blas::dot( nbe, dbasis_x_eval + ioff, 1, zmat_i, 1 );
        const F dy =
          4. * GauXC::blas::dot( nbe, dbasis_y_eval + ioff, 1, zmat_i, 1 );
        const F dz =
          4. * GauXC::blas::dot( nbe, dbasis_z_eval + ioff, 1, zmat_i, 1 );

        dden_x_eval[i] = dx;
        dden_y_eval[i] = dy;
        dden_z_eval[i] = dz;

        gamma[i] = dx*dx + dy*dy + dz*dz;
      }

    }

    // Evaluate XC functional
    if( func.is_gga() )
      func.eval_exc_vxc( npts, den_eval, gamma, eps, vrho, vgamma );
    else
      func.eval_exc_vxc( npts, den_eval, eps, vrho );

    // Factor weights into XC results
    for( int32_t i = 0; i < npts; ++i ) {
      eps[i]  *= weights[i];
      vrho[i] *= weights[i];
    }

    if( func.is_gga() )
      for( int32_t i = 0; i < npts; ++i ) vgamma[i] *= weights[i];

    // Scalar integrations
    if( n_el )
      for( int32_t i = 0; i < npts; ++i ) *n_el += weights[i] * den_eval[i];

    for( int32_t i = 0; i < npts; ++i ) *exc += eps[i] * den_eval[i];

    // Assemble Z
    if( func.is_gga() )
      zmat_gga_host( npts, nbe, vrho, vgamma, basis_eval, dbasis_x_eval,
                     dbasis_y_eval, dbasis_z_eval, dden_x_eval, dden_y_eval,
                     dden_z_eval, zmat );
    else
      zmat_lda_host( npts, nbe, vrho, basis_eval, zmat );

    // Update VXC XXX: Only LT
    GauXC::blas::syr2k( 'L', 'N', nbe, npts, F(1.), basis_eval,
                        nbe, zmat, nbe, F(0.), nbe_scr, nbe );

    detail::inc_by_submat( nbf, nbf, nbe, nbe, VXC, nbf, nbe_scr, nbe,
                           submat_map );
  }

  // Symmetrize VXC
  for( int32_t j = 0;   j < nbf; ++j )
  for( int32_t i = j+1; i < nbf; ++i )
    VXC[ j + i*nbf ] = VXC[ i + j*nbf ];

}

#define HOST_IMPL( F, ND ) \
template \
void process_batches_host_replicated_p<F, ND>(\
  XCIntegratorState      integrator_state, \
  XCWeightAlg            weight_alg,\
  const functional_type& func,\
  const BasisSet<F>&     basis,\
  const Molecule   &     mol,\
  const MolMeta    &     meta,\
  XCHostData<F>    &     host_data,\
  std::vector< XCTask >& local_work,\
  const F*               P,\
  F*                     VXC,\
  F*                     exc,\
  F*                     n_el\
)

HOST_IMPL( double, 0 );
HOST_IMPL( double, 1 );

}
}


#include <Eigen/Dense>
#include <vtkDelaunay3D.h>
#include <vtkDataSetSurfaceFilter.h>
#include <vtkPolyDataReader.h>
#include <vtkPolyDataWriter.h>
#include <vtkPolyData.h>
#include <vtkPointData.h>
#include <vtkCellArray.h>
#include <vtkDoubleArray.h>
#include <vtkIdFilter.h>
#include <vtkSmartPointer.h>

typedef Eigen::Vector3d Vector3d;
typedef Eigen::VectorXd VectorXd;
typedef Eigen::Matrix3d Matrix3d;
typedef Eigen::Matrix3Xd Matrix3Xd;
typedef Eigen::Map<Matrix3Xd> Map3Xd;

int main(){
    clock_t t1;
    t1 = clock();
    for(auto i=0; i < 10000; ++i){
        vtkNew<vtkPolyDataReader> reader;
        reader->SetFileName("T7.vtk");
        reader->Update();
        auto poly = reader->GetOutput();
        auto N = poly->GetNumberOfPoints();
        auto pts = (double*) poly->GetPoints()->GetData()->GetVoidPointer(0);
        Map3Xd points(pts,3,N);

        // Project points to unit sphere
        points.colwise().normalize();

        // Create a 3D tessellation
        auto idf = vtkSmartPointer<vtkIdFilter>::New();
        idf->PointIdsOn();
        idf->SetIdsArrayName("OrigIds");
        idf->SetInputData(poly);
        auto d3d = vtkSmartPointer<vtkDelaunay3D>::New();
        d3d->SetInputConnection(idf->GetOutputPort());
        auto dssf = vtkSmartPointer<vtkDataSetSurfaceFilter>::New();
        dssf->SetInputConnection(d3d->GetOutputPort());
        dssf->Update();
        auto final = dssf->GetOutput();
        auto interim = final->GetPolys();
        interim->InitTraversal();
        auto origIds = vtkIdTypeArray::SafeDownCast(
                                final->GetPointData()->GetArray("OrigIds"));
        auto pointIds = vtkSmartPointer<vtkIdList>::New();
        auto finalCells = vtkSmartPointer<vtkCellArray>::New();
        while(interim->GetNextCell(pointIds)){
            int numIds = pointIds->GetNumberOfIds();
            finalCells->InsertNextCell(numIds);
            for(auto j=0; j < numIds; j++ ){
                int id = (int)origIds->GetTuple1( pointIds->GetId(j) );
                finalCells->InsertCellPoint(id);
            }
        }
        poly->SetPolys(finalCells);
        auto writer = vtkSmartPointer<vtkPolyDataWriter>::New();
        writer->SetFileName("Mesh.vtk");
        writer->SetInputData(poly);
        writer->Write();

    }

    float diff((float)clock() - (float)t1);
    std::cout << "Time elapsed : " << diff / CLOCKS_PER_SEC
              << " seconds" << std::endl;
    return 0;
}


#include "Product.h"
#include "Config.h"
#include "State.h"
#include <boost/lexical_cast.hpp>
#include <ctpp2/CDT.hpp>
#include <macgyver/TimeParser.h>
#include <macgyver/Exception.h>
#include <spine/HTTP.h>

namespace
{
// ----------------------------------------------------------------------
/*!
 * \brief Distance between two points along earth surface
 *
 * \param theLon1 Longitude of point 1
 * \param theLat1 Latitude of point 1
 * \param theLon2 Longitude of point 2
 * \param theLat2 Latitude of point 2
 * \return The distance in kilometers
 *
 *  Haversine Formula (from R.W. Sinnott, "Virtues of the Haversine",
 *  Sky and Telescope, vol. 68, no. 2, 1984, p. 159)
 *  will give mathematically and computationally exact results. The
 *  intermediate result c is the great circle distance in radians. The
 *  great circle distance d will be in the same units as R.
 *
 *  When the two points are antipodal (on opposite sides of the Earth),
 *  the Haversine Formula is ill-conditioned, but the error, perhaps
 *  as large as 2 km (1 mi), is in the context of a distance near
 *  20,000 km (12,000 mi). Further, there is a possibility that roundoff
 *  errors might cause the value of sqrt(a) to exceed 1.0, which would
 *  cause the inverse sine to crash without the bulletproofing provided by
 *  the min() function.
 *
 * The code was taken from NFmiLocation::Distance
 */
// ----------------------------------------------------------------------

double torad(double theValue)
{
  return theValue * 3.14159265358979323846 / 180.0;
}
double geodistance(double theLon1, double theLat1, double theLon2, double theLat2)
{
  double lo1 = torad(theLon1);
  double la1 = torad(theLat1);

  double lo2 = torad(theLon2);
  double la2 = torad(theLat2);

  double dlon = lo2 - lo1;
  double dlat = la2 - la1;
  double sindlat = sin(dlat / 2);
  double sindlon = sin(dlon / 2);

  double a = sindlat * sindlat + cos(la1) * cos(la2) * sindlon * sindlon;
  double help1 = sqrt(a);
  double c = 2. * asin(std::min(1., help1));

  return 6371.220 * c;
}
}  // namespace

namespace SmartMet
{
namespace Plugin
{
namespace CrossSection
{
// ----------------------------------------------------------------------
/*!
 * \brief Initialize the product from JSON
 */
// ----------------------------------------------------------------------

void Product::init(const Json::Value& theJson, const Config& theConfig)
{
  try
  {
    if (!theJson.isObject())
      throw Fmi::Exception(BCP, "Product JSON is not a JSON object (name-value pairs)");

    // Iterate through all the members

    const auto members = theJson.getMemberNames();
    for (const auto& name : members)
    {
      const Json::Value& json = theJson[name];

      if (name == "layers")
        layers.init(json, theConfig);
      else
        throw Fmi::Exception(BCP,
                                         "Product does not have a setting named '" + name + "'");
    }
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Generate the product into the template hash tables
 *
 */
// ----------------------------------------------------------------------

void Product::generate(CTPP::CDT& theGlobals,
                       State& theState,
                       const SmartMet::Spine::TimeSeriesGenerator::LocalTimeList& theTimes)
{
  try
  {
    // Initialize the structure

    theGlobals["layers"] = CTPP::CDT(CTPP::CDT::HASH_VAL);

    // Process all times

    for (const auto& time : theTimes)
    {
      theState.time(time);
      layers.generate(theGlobals, theState);
    }

    // Generate bounding box

    const auto& env = theState.envelope();
    if (env.IsInit() != 0)
    {
      theGlobals["bbox"] = CTPP::CDT(CTPP::CDT::HASH_VAL);
      theGlobals["bbox"]["xmin"] = env.MinX;
      theGlobals["bbox"]["xmax"] = env.MaxX;
      theGlobals["bbox"]["ymin"] = env.MinY;
      theGlobals["bbox"]["ymax"] = env.MaxY;
    }

    // Distance between the two points

    theGlobals["distance"] = geodistance(theState.query().longitude1,
                                         theState.query().latitude1,
                                         theState.query().longitude2,
                                         theState.query().latitude2);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

}  // namespace CrossSection
}  // namespace Plugin
}  // namespace SmartMet


#include <fstream>
#include <iostream>
#include <string>
#include <stdlib.h>
#include <vector>
//#include <random>

#include <bits/stdc++.h>
#include <Eigen/Dense>
#include <Eigen/Core>
#include <Eigen/LU>
#include <Eigen/SVD>
#include <Eigen/QR>
#include "time.h"

#include "genotype.h"
#include "mailman.h"
#include "arguments.h"
//#include "helper.h"
#include "storage.h"

#if SSE_SUPPORT==1
	#define fastmultiply fastmultiply_sse
	#define fastmultiply_pre fastmultiply_pre_sse
#else
	#define fastmultiply fastmultiply_normal
	#define fastmultiply_pre fastmultiply_pre_normal
#endif

using namespace Eigen;
using namespace std;

// Storing in RowMajor Form
typedef Matrix<double, Dynamic, Dynamic, RowMajor> MatrixXdr;
//Intermediate Variables
int blocksize;
int hsegsize;
double *partialsums;
double *sum_op;
double *yint_e;
double *yint_m;
double **y_e;
double **y_m;

struct timespec t0;

clock_t total_begin = clock();
MatrixXdr pheno;
MatrixXdr mask;
MatrixXdr covariate;
MatrixXdr Q;
MatrixXdr v1; //W^ty
MatrixXdr v2;            //QW^ty
MatrixXdr v3;    //WQW^ty
MatrixXdr new_pheno;

genotype g;
genotype g1;
genotype g2;
MatrixXdr geno_matrix; //(p,n)
genotype* Geno;
int MAX_ITER;
int k,p,n;
int k_orig;

MatrixXdr c; //(p,k)
MatrixXdr x; //(k,n)
MatrixXdr v; //(p,k)
MatrixXdr means; //(p,1)
MatrixXdr stds; //(p,1)
MatrixXdr sum2;
MatrixXdr sum;
////////
//related to phenotype
double y_sum;
double y_mean;

options command_line_opts;

bool debug = false;
bool check_accuracy = false;
bool var_normalize=false;
int accelerated_em=0;
double convergence_limit;
bool memory_efficient = false;
bool missing=false;
bool fast_mode = true;
bool text_version = false;
bool use_cov=false;

//// jackknife index wich are computed based on annotation file
MatrixXdr dic_index;
MatrixXdr jack_bin_size;
vector<int> len;
vector<int> Annot;
int Njack=100;
int Nbin=8;
int Nz=10;
///////

//define random vector z's
MatrixXdr  all_zb;
MatrixXdr res;
MatrixXdr XXz;
MatrixXdr Xy;
MatrixXdr yXXy;

std::istream& newline(std::istream& in)
{
    if ((in >> std::ws).peek() != std::char_traits<char>::to_int_type('\n')) {
        in.setstate(std::ios_base::failbit);
    }
    return in.ignore();
}

int read_cov(bool std,int Nind, std::string filename, std::string covname){
	ifstream ifs(filename.c_str(), ios::in);
	std::string line;
	std::istringstream in;
	int covIndex = 0;
	std::getline(ifs,line);
	in.str(line);
	string b;
	vector<vector<int> > missing;
	int covNum=0;
	while(in>>b)
	{
		if(b!="FID" && b !="IID"){
		missing.push_back(vector<int>()); //push an empty row
		if(b==covname && covname!="")
			covIndex=covNum;
		covNum++;
		}
	}
	vector<double> cov_sum(covNum, 0);
	if(covname=="")
	{
		covariate.resize(Nind, covNum);
		cout<< "Read in "<<covNum << " Covariates.. "<<endl;
	}
	else
	{
		covariate.resize(Nind, 1);
		cout<< "Read in covariate "<<covname<<endl;
	}

	int j=0;
	while(std::getline(ifs, line)){
		in.clear();
		in.str(line);
		string temp;
		in>>temp; in>>temp; //FID IID
		for(int k=0; k<covNum; k++){

			in>>temp;
			if(temp=="NA")
			{
				missing[k].push_back(j);
				continue;
			}
			double cur = atof(temp.c_str());
			if(cur==-9)
			{
				missing[k].push_back(j);
				continue;
			}
			if(covname=="")
			{
				cov_sum[k]= cov_sum[k]+ cur;
				covariate(j,k) = cur;
			}
			else
				if(k==covIndex)
				{
					covariate(j, 0) = cur;
					cov_sum[k] = cov_sum[k]+cur;
				}
		}
		//if(j<10)
		//	cout<<covariate.block(j,0,1, covNum)<<endl;
		j++;
	}
	//compute cov mean and impute
	for (int a=0; a<covNum ; a++)
	{
		int missing_num = missing[a].size();
		cov_sum[a] = cov_sum[a] / (Nind - missing_num);

		for(int b=0; b<missing_num; b++)
		{
                        int index = missing[a][b];
                        if(covname=="")
                                covariate(index, a) = cov_sum[a];
                        else if (a==covIndex)
                                covariate(index, 0) = cov_sum[a];
                }
	}
	if(std)
	{
		MatrixXdr cov_std;
		cov_std.resize(1,covNum);
		MatrixXdr sum = covariate.colwise().sum();
		MatrixXdr sum2 = (covariate.cwiseProduct(covariate)).colwise().sum();
		MatrixXdr temp;
//		temp.resize(Nind, 1);
//		for(int i=0; i<Nind; i++)
//			temp(i,0)=1;
		for(int b=0; b<covNum; b++)
		{
			cov_std(0,b) = sum2(0,b) + Nind*cov_sum[b]*cov_sum[b]- 2*cov_sum[b]*sum(0,b);
			cov_std(0,b) =sqrt((Nind- 1)/cov_std(0,b)) ;
			double scalar=cov_std(0,b);
			for(int j=0; j<Nind; j++)
			{
				covariate(j,b) = covariate(j,b)-cov_sum[b];
				covariate(j,b) =covariate(j,b)*scalar;
			}
			//covariate.col(b) = covariate.col(b) -temp*cov_sum[b];

		}
	}
	return covNum;
}

/*void read_cov(int Nind, std::string filename, std::string covname){
	ifstream ifs(filename.c_str(), ios::in);
	std::string line;
	std::istringstream in;
	int covIndex = 0;
	std::getline(ifs,line);
	in.str(line);
	string b;
	vector<vector<int> > missing;
	int covNum=0;
	while(in>>b)
	{
		missing.push_back(vector<int>()); //push an empty row
		if(b==covname && covname!="")
			covIndex=covNum;
		covNum++;
	}
	vector<double> cov_sum(covNum, 0);
	if(covname=="")
	{
		covariate.resize(Nind, covNum);
		cout<< "Read in "<<covNum << " Covariates.. "<<endl;
	}
	else
	{
		covariate.resize(Nind, 1);
		cout<< "Read in covariate "<<covname<<endl;
	}

	int j=0;
	while(std::getline(ifs, line)){
		in.clear();
		in.str(line);
		string temp;
		for(int k=0; k<covNum; k++){
			in>>temp;
			if(temp=="NA")
			{
				missing[k].push_back(j);
				continue;
			}
			int cur = atof(temp.c_str());
			if(cur==-9)
			{
				missing[k].push_back(j);
				continue;
			}
			if(covname=="")
			{
				cov_sum[k]= cov_sum[k]+ cur;
				covariate(j,k) = cur;
			}
			else
				if(k==covIndex)
				{
					covariate(j, 0) = cur;
					cov_sum[k] = cov_sum[k]+cur;
				}
		}
		j++;
	}
	//compute cov mean and impute
	for (int a=0; a<covNum ; a++)
	{
		int missing_num = missing[a].size();
		cov_sum[a] = cov_sum[a] / (covNum - missing_num);

		for(int b=0; b<missing_num; b++)
		{
                        int index = missing[a][b];
                        if(covname=="")
                                covariate(index, a) = cov_sum[a];
                        else if (a==covIndex)
                                covariate(index, 0) = cov_sum[a];
                }
	}
}*/
void read_pheno2(int Nind, std::string filename){
//	pheno.resize(Nind,1);
	ifstream ifs(filename.c_str(), ios::in);

	std::string line;
	std::istringstream in;
	int phenocount=0;
//read header
	std::getline(ifs,line);
	in.str(line);
	string b;
	while(in>>b)
	{
		if(b!="FID" && b !="IID")
			phenocount++;
	}
	pheno.resize(Nind, phenocount);
	mask.resize(Nind, phenocount);
	int i=0;
	while(std::getline(ifs, line)){
		in.clear();
		in.str(line);
		string temp;
		//fid,iid
		//todo: fid iid mapping;
		//todo: handle missing phenotype
		in>>temp; in>>temp;
		for(int j=0; j<phenocount;j++) {
			in>>temp;
			double cur = atof(temp.c_str());
			if(temp=="NA" || cur==-9){
			pheno(i,j)=0;
			mask(i,j)=0;
			}
			else{
			pheno(i,j)=atof(temp.c_str());
			mask(i,j)=1;

			}

		}
		i++;
	}
	//cout<<pheno;
}
void read_pheno(int Nind, std::string filename){
	pheno.resize(Nind, 1);
	ifstream ifs(filename.c_str(), ios::in);

	std::string line;
	int i=0;
	while(std::getline(ifs, line)){
		pheno(i,0) = atof(line.c_str());
		if(pheno(i,0)==-1)
			cout<<"WARNING: missing phenotype"<<endl;
		i++;
	}

}
void multiply_y_pre_fast(MatrixXdr &op, int Ncol_op ,MatrixXdr &res,bool subtract_means){

	for(int k_iter=0;k_iter<Ncol_op;k_iter++){
		sum_op[k_iter]=op.col(k_iter).sum();
	}

			//cout << "Nops = " << Ncol_op << "\t" <<g.Nsegments_hori << endl;
	#if DEBUG==1
		if(debug){
			print_time ();
			cout <<"Starting mailman on premultiply"<<endl;
			cout << "Nops = " << Ncol_op << "\t" <<g.Nsegments_hori << endl;
			cout << "Segment size = " << g.segment_size_hori << endl;
			cout << "Matrix size = " <<g.segment_size_hori<<"\t" <<g.Nindv << endl;
			cout << "op = " <<  op.rows () << "\t" << op.cols () << endl;
		}
	#endif

	//TODO: Memory Effecient SSE FastMultipy

	for(int seg_iter=0;seg_iter<g.Nsegments_hori-1;seg_iter++){
		mailman::fastmultiply(g.segment_size_hori,g.Nindv,Ncol_op,g.p[seg_iter],op,yint_m,partialsums,y_m);
		int p_base = seg_iter*g.segment_size_hori;
		for(int p_iter=p_base; (p_iter<p_base+g.segment_size_hori) && (p_iter<g.Nsnp) ; p_iter++ ){
			for(int k_iter=0;k_iter<Ncol_op;k_iter++)
				res(p_iter,k_iter) = y_m[p_iter-p_base][k_iter];
		}
	}

	int last_seg_size = (g.Nsnp%g.segment_size_hori !=0 ) ? g.Nsnp%g.segment_size_hori : g.segment_size_hori;
	mailman::fastmultiply(last_seg_size,g.Nindv,Ncol_op,g.p[g.Nsegments_hori-1],op,yint_m,partialsums,y_m);
	int p_base = (g.Nsegments_hori-1)*g.segment_size_hori;
	for(int p_iter=p_base; (p_iter<p_base+g.segment_size_hori) && (p_iter<g.Nsnp) ; p_iter++){
		for(int k_iter=0;k_iter<Ncol_op;k_iter++)
			res(p_iter,k_iter) = y_m[p_iter-p_base][k_iter];
	}

	#if DEBUG==1
		if(debug){
			print_time ();
			cout <<"Ending mailman on premultiply"<<endl;
		}
	#endif

	if(!subtract_means)
		return;

	for(int p_iter=0;p_iter<p;p_iter++){
 		for(int k_iter=0;k_iter<Ncol_op;k_iter++){
			res(p_iter,k_iter) = res(p_iter,k_iter) - (g.get_col_mean(p_iter)*sum_op[k_iter]);
			if(var_normalize)
				res(p_iter,k_iter) = res(p_iter,k_iter)/(g.get_col_std(p_iter));
 		}
 	}

}

void multiply_y_post_fast(MatrixXdr &op_orig, int Nrows_op, MatrixXdr &res,bool subtract_means){

	MatrixXdr op;
	op = op_orig.transpose();

	if(var_normalize && subtract_means){
		for(int p_iter=0;p_iter<p;p_iter++){
			for(int k_iter=0;k_iter<Nrows_op;k_iter++)
				op(p_iter,k_iter) = op(p_iter,k_iter) / (g.get_col_std(p_iter));
		}
	}

	#if DEBUG==1
		if(debug){
			print_time ();
			cout <<"Starting mailman on postmultiply"<<endl;
		}
	#endif

	int Ncol_op = Nrows_op;

	//cout << "ncol_op = " << Ncol_op << endl;

	int seg_iter;
	for(seg_iter=0;seg_iter<g.Nsegments_hori-1;seg_iter++){
mailman::fastmultiply_pre(g.segment_size_hori,g.Nindv,Ncol_op, seg_iter * g.segment_size_hori, g.p[seg_iter],op,yint_e,partialsums,y_e);
	}
	int last_seg_size = (g.Nsnp%g.segment_size_hori !=0 ) ? g.Nsnp%g.segment_size_hori : g.segment_size_hori;
	mailman::fastmultiply_pre(last_seg_size,g.Nindv,Ncol_op, seg_iter * g.segment_size_hori, g.p[seg_iter],op,yint_e,partialsums,y_e);

	for(int n_iter=0; n_iter<n; n_iter++)  {
		for(int k_iter=0;k_iter<Ncol_op;k_iter++) {
			res(k_iter,n_iter) = y_e[n_iter][k_iter];
			y_e[n_iter][k_iter] = 0;
		}
	}

	#if DEBUG==1
		if(debug){
			print_time ();
			cout <<"Ending mailman on postmultiply"<<endl;
		}
	#endif

	if(!subtract_means)
		return;

	double *sums_elements = new double[Ncol_op];
 	memset (sums_elements, 0, Nrows_op * sizeof(int));

 	for(int k_iter=0;k_iter<Ncol_op;k_iter++){
 		double sum_to_calc=0.0;
 		for(int p_iter=0;p_iter<p;p_iter++)
 			sum_to_calc += g.get_col_mean(p_iter)*op(p_iter,k_iter);
 		sums_elements[k_iter] = sum_to_calc;
 	}
 	for(int k_iter=0;k_iter<Ncol_op;k_iter++){
 		for(int n_iter=0;n_iter<n;n_iter++)
 			res(k_iter,n_iter) = res(k_iter,n_iter) - sums_elements[k_iter];
 	}

}

void multiply_y_pre_naive_mem(MatrixXdr &op, int Ncol_op ,MatrixXdr &res){
	for(int p_iter=0;p_iter<p;p_iter++){
		for(int k_iter=0;k_iter<Ncol_op;k_iter++){
			double temp=0;
			for(int n_iter=0;n_iter<n;n_iter++)
				temp+= g.get_geno(p_iter,n_iter,var_normalize)*op(n_iter,k_iter);
			res(p_iter,k_iter)=temp;
		}
	}
}

void multiply_y_post_naive_mem(MatrixXdr &op, int Nrows_op ,MatrixXdr &res){
	for(int n_iter=0;n_iter<n;n_iter++){
		for(int k_iter=0;k_iter<Nrows_op;k_iter++){
			double temp=0;
			for(int p_iter=0;p_iter<p;p_iter++)
				temp+= op(k_iter,p_iter)*(g.get_geno(p_iter,n_iter,var_normalize));
			res(k_iter,n_iter)=temp;
		}
	}
}

void multiply_y_pre_naive(MatrixXdr &op, int Ncol_op ,MatrixXdr &res){
	res = geno_matrix * op;
}

void multiply_y_post_naive(MatrixXdr &op, int Nrows_op ,MatrixXdr &res){
	res = op * geno_matrix;
}

void multiply_y_post(MatrixXdr &op, int Nrows_op ,MatrixXdr &res,bool subtract_means){
    if(fast_mode)
        multiply_y_post_fast(op,Nrows_op,res,subtract_means);
    else{
		if(memory_efficient)
			multiply_y_post_naive_mem(op,Nrows_op,res);
		else
			multiply_y_post_naive(op,Nrows_op,res);
	}
}

void multiply_y_pre(MatrixXdr &op, int Ncol_op ,MatrixXdr &res,bool subtract_means){
    if(fast_mode)
        multiply_y_pre_fast(op,Ncol_op,res,subtract_means);
    else{
		if(memory_efficient)
			multiply_y_pre_naive_mem(op,Ncol_op,res);
		else
			multiply_y_pre_naive(op,Ncol_op,res);
	}
}

void initial_var(int key)
{
    /*if(key==1)
        g=g1;
    if(key==2)
    	g=g2;*/
   // g=Geno[key];
    p = g.Nsnp;
	n = g.Nindv;

	c.resize(p,k);
	x.resize(k,n);
	v.resize(p,k);
	means.resize(p,1);
	stds.resize(p,1);
	sum2.resize(p,1);
	sum.resize(p,1);

	if(!fast_mode && !memory_efficient){
		geno_matrix.resize(p,n);
		g.generate_eigen_geno(geno_matrix,var_normalize);
	}

	//TODO: Initialization of c with gaussian distribution
	c = MatrixXdr::Random(p,k);

	// Initial intermediate data structures
	blocksize = k;
	 hsegsize = g.segment_size_hori; 	// = log_3(n)
	int hsize = pow(3,hsegsize);
	int vsegsize = g.segment_size_ver; 		// = log_3(p)
	int vsize = pow(3,vsegsize);

	partialsums = new double [blocksize];
	sum_op = new double[blocksize];
	yint_e = new double [hsize*blocksize];
	yint_m = new double [hsize*blocksize];
	memset (yint_m, 0, hsize*blocksize * sizeof(double));
	memset (yint_e, 0, hsize*blocksize * sizeof(double));

	y_e  = new double*[g.Nindv];
	for (int i = 0 ; i < g.Nindv ; i++) {
		y_e[i] = new double[blocksize];
		memset (y_e[i], 0, blocksize * sizeof(double));
	}

	y_m = new double*[hsegsize];
	for (int i = 0 ; i < hsegsize ; i++)
		y_m[i] = new double[blocksize];
	for(int i=0;i<p;i++){
		means(i,0) = g.get_col_mean(i);
		stds(i,0) =1/g.get_col_std(i);
		//sum2(i,0) =g.get_col_sum2(i);
		sum(i,0)= g.get_col_sum(i);
	}

}

MatrixXdr multi_Xz (MatrixXdr zb){

              for(int j=0; j<g.Nsnp;j++)
                zb(j,0) =zb(j,0) *stds(j,0);

		MatrixXdr new_zb = zb.transpose();
	        MatrixXdr new_res(1, g.Nindv);
		multiply_y_post_fast(new_zb, 1, new_res, false);
		MatrixXdr new_resid(1, g.Nsnp);
		MatrixXdr zb_scale_sum = new_zb * means;
		new_resid = zb_scale_sum * MatrixXdr::Constant(1,g.Nindv, 1);
		new_res=new_res - new_resid;
       return new_res;

}

double compute_yVKVy(int s){
	MatrixXdr new_pheno_sum = new_pheno.colwise().sum();
	MatrixXdr res(g.Nsnp, 1);
	multiply_y_pre_fast(new_pheno,1,res,false);
	res = res.cwiseProduct(stds);
	MatrixXdr resid(g.Nsnp, 1);
	resid = means.cwiseProduct(stds);
	resid = resid *new_pheno_sum;
	MatrixXdr Xy(g.Nsnp,1);
	Xy = res-resid;
	double ytVKVy = (Xy.array()* Xy.array()).sum();
	ytVKVy = ytVKVy/g.Nsnp;
	return ytVKVy;

}

double compute_yXXy(){

//for (int i=0;i<g.Nindv;i++)
	//pheno(i,0)=1;

        MatrixXdr res(g.Nsnp, 1);
        multiply_y_pre_fast(pheno,1,res,false);
        res = res.cwiseProduct(stds);
        MatrixXdr resid(g.Nsnp, 1);
        resid = means.cwiseProduct(stds);
        resid = resid *y_sum;
        MatrixXdr Xy(g.Nsnp,1);
        Xy = res-resid;

        double yXXy = (Xy.array()* Xy.array()).sum();

        return yXXy;

}

 double compute_tr_k(int s){

	   /* if (s==1)
         initial_var(1);
        if(s==2)
         initial_var(2);
*/
        initial_var(s);
	    double tr_k =0 ;
 		MatrixXdr temp = sum2 + g.Nindv* means.cwiseProduct(means) - 2 * means.cwiseProduct(sum);
		temp = temp.cwiseProduct(stds);
		temp = temp.cwiseProduct(stds);
		tr_k = temp.sum() / g.Nsnp;

	//    cout<<g.Nindv<<"    "<<g.Nsnp<<" s:   "<<temp.sum()<<"\n";
	//    cout<<tr_k<<"\n";
	   return tr_k;
	}

MatrixXdr  compute_XXz (){

	//mask
	for (int i=0;i<Nz;i++)
	   for(int j=0;j<g.Nindv;j++)
		 all_zb(j,i)=all_zb(j,i)*mask(j,0);

         res.resize(g.Nsnp, Nz);
         multiply_y_pre_fast(all_zb,Nz,res, false);

//         cout<<res<<endl;

        MatrixXdr zb_sum = all_zb.colwise().sum();

	for(int j=0; j<g.Nsnp; j++)
            for(int k=0; k<Nz;k++)
                 res(j,k) = res(j,k)*stds(j,0);

        MatrixXdr resid(g.Nsnp, Nz);
        MatrixXdr inter = means.cwiseProduct(stds);
        resid = inter * zb_sum;
        MatrixXdr inter_zb = res - resid;

	for(int k=0; k<Nz; k++)
            for(int j=0; j<g.Nsnp;j++)
                inter_zb(j,k) =inter_zb(j,k) *stds(j,0);

       MatrixXdr new_zb = inter_zb.transpose();
       MatrixXdr new_res(Nz, g.Nindv);

       multiply_y_post_fast(new_zb, Nz, new_res, false);

       MatrixXdr new_resid(Nz, g.Nsnp);
       MatrixXdr zb_scale_sum = new_zb * means;
       new_resid = zb_scale_sum * MatrixXdr::Constant(1,g.Nindv, 1);

	//return new_res;

                      /// new zb
       MatrixXdr temp=new_res - new_resid;

	for (int i=0;i<Nz;i++)
           for(int j=0;j<g.Nindv;j++)
                 temp(i,j)=temp(i,j)*mask(j,0);

	return temp.transpose();

}

void read_annot (string filename){
        ifstream inp(filename.c_str());
        if (!inp.is_open()){
                cerr << "Error reading file "<< filename <<endl;
                exit(1);
        }
        string line;
        int j = 0 ;
        int linenum = 0 ;
        int num_parti;
        stringstream check1(line);
        string intermediate;
        vector <string> tokens;
        while(std::getline (inp, line)){
                linenum ++;
                char c = line[0];
                if (c=='#')
                        continue;
                istringstream ss (line);
                if (line.empty())
                        continue;
                j++;
                //cout<<line<<endl;

                stringstream check1(line);
                string intermediate;
                vector <string> tokens;
                // Tokenizing w.r.t. space ' '
                while(getline(check1, intermediate, ' '))
                 {
                      tokens.push_back(intermediate);
                 }
                 if(linenum==1){
                 num_parti=tokens.size();
                 if(num_parti!=Nbin)
                        cout<<"number of col of annot file does not match number of bins"<<endl;
                len.resize(num_parti,0);
                }
                int index_annot=0;
                for(int i = 0; i < tokens.size(); i++){
                        if (tokens[i]=="1")
                            index_annot=i;
                }
                   Annot.push_back(index_annot);
                   len[index_annot]++;
       }

	dic_index=MatrixXdr::Zero(Njack,Nbin);
  	jack_bin_size=MatrixXdr::Zero(Njack,Nbin);

	int num_snps=0;
        for (int i=0;i<num_parti;i++){
                //cout<<len[i]<<endl;
                num_snps+=len[i];
        }
  //      cout<<"step size: "<<endl;
        int step_size=num_snps/Njack;
         int step_size_rem=num_snps%Njack;
    //    cout<<step_size<<endl;
      //  cout<<"reminder: "<<step_size_rem<<endl;
         int temp=step_size;

         //for (int i=0;i<Njack;i++)
           //      for (int j=0;j<num_parti;j++)
             //           dic_index(i,j)=0;

        j=1;

        for (int i=0;i<Annot.size();i++){
                if(i==(step_size*j) && j<Njack ){
                     j++;
                     for (int k=0; k<num_parti;k++)
                        dic_index(j-1,k)=dic_index(j-2,k);
                }
                dic_index(j-1,Annot[i])=dic_index(j-1,Annot[i])+1;
        }

        //handle not removing a bin in jackknife se
        for(int i=0;i<Nbin;i++){
	   for(int j=0;j<Njack;j++){
		if(j==0 && dic_index(j,i)==len[i]){
			dic_index(j,i)=len[i]/2;
			dic_index(j+1,i)=len[i]-dic_index(j,i);
		}
		else if ( j!=0 && (dic_index(j,i)-dic_index(j-1,i))==len[i] ){
			dic_index(j-1,i)=len[i]/2;
			dic_index(j,i)=len[i]-dic_index(j-1,i);
		}
	   }
       }
//cout<<"end reading annot"<<endl;

}

void count_fam(std::string filename){
        ifstream ifs(filename.c_str(), ios::in);

        std::string line;
        int i=0;
        while(std::getline(ifs, line)){
                i++;
        }
        g.Nindv=i-1;
}

int main(int argc, char const *argv[]){

parse_args(argc,argv);
////////////////////////////////////////////
///////////////////////////////////////////

    //MAX_ITER =  command_line_opts.max_iterations ;
        int B = command_line_opts.batchNum;
        k_orig = command_line_opts.num_of_evec ;
        debug = command_line_opts.debugmode ;
        check_accuracy = command_line_opts.getaccuracy;
        var_normalize = false;
        accelerated_em = command_line_opts.accelerated_em;
        k = k_orig + command_line_opts.l;
        k = (int)ceil(k/10.0)*10;
        command_line_opts.l = k - k_orig;
        //p = g.Nsnp;
        //n = g.Nindv;
        bool toStop=false;
        toStop=true;
        srand((unsigned int) time(0));
        //Nz=10;
	Nz=command_line_opts.num_of_evec;
        k=Nz;
         ///clock_t io_end = clock();

	Njack=command_line_opts.jack_number;

////
string filename;
//filename="/home/alipazoki/filter4_no_mhc/mafld/annot.txt";
//filename="/home/alipazoki/RHEmc_online/example/annot_filter4.txt";
//filename=command_line_opts.Annot_PATH;
//read_annot(filename);
//cout<<dic_index<<endl;
//cout<<jack_bin_size<<endl;
//////////////////////////// Read multi genotypes
string line;
int cov_num;
int num_files=0;
//string name="/home/alipazoki/filter4_no_mhc/mafld/adr.txt";
//string name="/home/alipazoki/UKBB/maf_ld/sub_indv/adr.txt";
string name=command_line_opts.GENOTYPE_FILE_PATH;
//cout<<name<<endl;
ifstream f (name.c_str());
while(getline(f,line))
   num_files++;

string file_names[num_files];

int i=0;
ifstream ff (name.c_str());
while(getline(ff,line)) {

    file_names[i]=line;
    cout<<file_names[i]<<"\n";
    i++;
}

Nbin=num_files;

 cout<<"Number of the bins: "<<Nbin<<endl;

filename=command_line_opts.Annot_PATH;
read_annot(filename);

///reading phnotype and save the number of indvs
//filename="/home/alipazoki/UKBB/kathy_pheno/height.pheno";
//filename="/home/alipazoki/UKBB/maf_ld/sub_indv/10k.bmi.pheno";
filename=command_line_opts.PHENOTYPE_FILE_PATH;
count_fam(filename);
read_pheno2(g.Nindv,filename);
cout<<"Number of Indvs :"<<g.Nindv<<endl;
y_sum=pheno.sum();

//read covariate
//std::string covfile="/home/alipazoki/UKBB/kathy_pheno/height.covar";
//:qstd::string covfile="/home/alipazoki/UKBB/maf_ld/sub_indv/10k.bmi.covar";
//bool usee_cov=false;
//if(usee_cov==true){
std::string covfile=command_line_opts.COVARIATE_FILE_PATH;
std::string covname="";
if(covfile!=""){
     use_cov=true;
     cov_num=read_cov(false,g.Nindv, covfile, covname);
	//cout<<cov_num<<endl;
}
else if(covfile=="")
     cout<<"No Covariate File Specified"<<endl;

/// regress out cov from phenotypes

if(use_cov==true){
MatrixXdr mat_mask=mask.replicate(1,cov_num);
covariate=covariate.cwiseProduct(mat_mask);

MatrixXdr WtW= covariate.transpose()*covariate;
Q=WtW.inverse(); // Q=(W^tW)^-1
//cout<<" Number of covariates"<<cov_num<<endl;

MatrixXdr v1=covariate.transpose()*pheno; //W^ty
MatrixXdr v2=Q*v1;            //QW^ty
MatrixXdr v3=covariate*v2;    //WQW^ty
pheno=pheno-v3;
pheno=pheno.cwiseProduct(mask);
 }
////// normalize phenotype

//bool pheno_norm=false;
y_sum=pheno.sum();
y_mean = y_sum/mask.sum();

//if(pheno_norm==true){
for(int i=0; i<g.Nindv; i++){
   if(pheno(i,0)!=0)
      pheno(i,0) =pheno(i,0) - y_mean; //center phenotype
}
y_sum=pheno.sum();

//}

//define random vector z's
//Nz=1;

all_zb= MatrixXdr::Random(g.Nindv,Nz);
all_zb = all_zb * sqrt(3);
MatrixXdr output;
//define

//e
//Njack=1;

XXz=MatrixXdr::Zero(g.Nindv,Nbin*(Njack+1)*Nz);
yXXy=MatrixXdr::Zero(Nbin,Njack+1);

for(int bin_index=0; bin_index<Nbin; bin_index++){

    std::stringstream f3;
    f3 << file_names[bin_index] << ".bed";
    string name=f3.str();
    cout<<name<<endl;
    ifstream ifs (name.c_str(), ios::in|ios::binary);

    g.read_header=true;
     //E
     for (int jack_index=0;jack_index<Njack;jack_index++){

	//cout<<"reading "<<jack_index<<"-th jckknf blck of "<<bin_index<<"-th bin"<<endl;

	if (jack_index==0){
		g.Nsnp=dic_index(jack_index,bin_index);
		jack_bin_size(jack_index,bin_index)=g.Nsnp;
	}
	else{
	        g.Nsnp=dic_index(jack_index,bin_index)-dic_index(jack_index-1,bin_index);
		jack_bin_size(jack_index,bin_index)=g.Nsnp;
        }
       if(g.Nsnp!=0){
	  //cout<<"Zero SNPSsss"<<endl;

	//g.Nsnp=len[bin_index];
  	//cout<<"#SNPs"<<g.Nsnp<<endl;

	  g.read_plink(ifs,file_names[bin_index],missing,fast_mode);
    	  initial_var(0);
//////// do computation for i-th jack block of j-th bin

	/// compute XXz
	output=compute_XXz();
//	cout<<output.col(0).sum()<<endl;
	for (int z_index=0;z_index<Nz;z_index++){
		 XXz.col((bin_index*(Njack+1)*Nz)+(jack_index*Nz)+z_index)=output.col(z_index);
		 XXz.col((bin_index*(Njack+1)*Nz)+(Njack*Nz)+z_index)+=output.col(z_index);   /// save whole sample
	}
	///compute yXXy
	yXXy(bin_index,jack_index)= compute_yXXy();
	yXXy(bin_index,Njack)+= yXXy(bin_index,jack_index);
	//// contribtion of each jackknife SUBSAMPLE (not block)

///////end computation
/////////////////////////////////destruct class g
	delete[] sum_op;
        delete[] partialsums;
        delete[] yint_e;
        delete[] yint_m;
        for (int i  = 0 ; i < hsegsize; i++)
                delete[] y_m [i];
        delete[] y_m;

        for (int i  = 0 ; i < g.Nindv; i++)
                delete[] y_e[i];
        delete[] y_e;

	std::vector< std::vector<int> >().swap(g.p);
        std::vector< std::vector<int> >().swap(g.not_O_j);
        std::vector< std::vector<int> >().swap(g.not_O_i);

	//g.p.clear();
	//g.not_O_j.clear();
	//g.not_O_i.clear();
	g.columnsum.clear();
	g.columnsum2.clear();
	g.columnmeans.clear();
	g.columnmeans2.clear();
	//std::vector< std::vector<int> >().swap(g.columnsum);
	//std::vector< std::vector<int> >().swap(g.columnsum2);
	//std::vector< std::vector<double> >().swap(g.columnmeans);
	//std::vector< std::vector<double> >().swap(g.columnmeans2);
	g.read_header=false;
/////////////////////////////////////////////
       } //end of else
   }// end of loop over jack blocks

  if(bin_index==0){
        //cout<<"zzzzzzzzzzzz"<<XXz.col((bin_index*Njack*Nz)+(Njack*Nz)).sum()<<endl;
        //cout<<bin_index<<" "<<Njack<<" "<<0<<endl;
	//cout<<"zzzzzzzzzzzz"<<XXz.col((bin_index*Njack*Nz)+(Njack*Nz)).sum()<<endl;
	}
   // contribtion of each jackknife SUBSAMPLE (not block)

 for(int jack_index=0;jack_index<Njack;jack_index++){
	for (int z_index=0;z_index<Nz;z_index++){
	 MatrixXdr v1=XXz.col((bin_index*(Njack+1)*Nz)+(Njack*Nz)+z_index);
	// cout<<bin_index<<" "<<Njack<<" "<<z_index<<endl;

	//cout<<"v1: "<<v1.sum()<<endl;
	MatrixXdr v2=XXz.col((bin_index*(Njack+1)*Nz)+(jack_index*Nz)+z_index);
        // cout<<bin_index<<" "<<jack_index<<" "<<z_index<<endl;
	//cout<<"v2: "<<v2.sum()<<endl;
	XXz.col((bin_index*(Njack+1)*Nz)+(jack_index*Nz)+z_index)=v1-v2;

	//cout<<"real v1: "<<XXz.col((bin_index*Njack*Nz)+(Njack*Nz)+z_index).sum()<<endl;
	//cout<<"real v2: "<<XXz.col((bin_index*Njack*Nz)+(jack_index*Nz)+z_index).sum()<<endl;

       }
	yXXy(bin_index,jack_index)=yXXy(bin_index,Njack)-yXXy(bin_index,jack_index);
}

} //end of loop over bins

/// normal equations LHS
MatrixXdr  A_trs(Nbin,Nbin);
MatrixXdr b_trk(Nbin,1);
MatrixXdr c_yky(Nbin,1);

MatrixXdr X_l(Nbin+1,Nbin+1);
MatrixXdr Y_r(Nbin+1,1);
//int bin_index=0;
int jack_index=Njack;
MatrixXdr B1;
MatrixXdr B2;
MatrixXdr C1;
MatrixXdr C2;
double trkij;
double yy=(pheno.array() * pheno.array()).sum();
int Nindv_mask=mask.sum();
MatrixXdr jack;
MatrixXdr point_est;
MatrixXdr enrich_jack;
MatrixXdr enrich_point_est;

jack.resize(Nbin+1,Njack);
point_est.resize(Nbin+1,1);

enrich_jack.resize(Nbin,Njack);
enrich_point_est.resize(Nbin,1);

for (jack_index=0;jack_index<=Njack;jack_index++){

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

	b_trk(i,0)=Nindv_mask;

	if(jack_index==Njack)
	c_yky(i,0)=yXXy(i,jack_index)/len[i];
	else
	c_yky(i,0)=yXXy(i,jack_index)/(len[i]-jack_bin_size(jack_index,i));
	//cout<<"bin "<<i<<"yXXy "<<yXXy(i,jack_index)<<endl;
	for (int j=i;j<Nbin;j++){
		//cout<<Njack<<endl;
		B1=XXz.block(0,(i*(Njack+1)*Nz)+(jack_index*Nz),g.Nindv,Nz);
		B2=XXz.block(0,(j*(Njack+1)*Nz)+(jack_index*Nz),g.Nindv,Nz);
		C1=B1.array()*B2.array();
		C2=C1.colwise().sum();
		trkij=C2.sum();

		//cout<<"tr"<<i<<" "<<j<<" : "<<trkij<<endl;
	        if(jack_index==Njack)
		trkij=trkij/len[i]/len[j]/Nz;
		else
		 trkij=trkij/(len[i]-jack_bin_size(jack_index,i))/(len[j]-jack_bin_size(jack_index,j))/Nz;
		A_trs(i,j)=trkij;
		A_trs(j,i)=trkij;

	}
  }

X_l<<A_trs,b_trk,b_trk.transpose(),Nindv_mask;
Y_r<<c_yky,yy;

MatrixXdr herit=X_l.colPivHouseholderQr().solve(Y_r);

double temp_sig=0;
double temp_sum=0;

if(jack_index==Njack){
     for(int i=0;i<(Nbin+1);i++)
	  point_est(i,0)=herit(i,0);
}
else{
for(int i=0;i<(Nbin+1);i++)
      jack(i,jack_index)=herit(i,0);
}

double total_val=0;
for(int i=0; i<Nbin;i++)
    total_val+=herit(i,0);

/*
for(int i=0; i<Nbin;i++){
   cout<<herit(i,0)/herit.sum()<<" ";
}
	cout<<total_val/herit.sum()<<endl;
*/
//cout<<X_l<<endl;
//cout<<Y_r<<endl;
//cout<<"ddddddddddd"<<endl;

}// end of loop over jacks

//cout<<"helolll"<<endl;
double temp_sig=0;
double temp_sum=0;

temp_sig=0;
temp_sum=point_est.sum();
for (int j=0;j<Nbin;j++){
        point_est(j,0)=point_est(j,0)/temp_sum;
        temp_sig+=point_est(j,0);
}
point_est(Nbin,0)=temp_sig;

for (int i=0;i<Njack;i++){
   temp_sig=0;
   temp_sum=jack.col(i).sum();
   for (int j=0;j<Nbin;j++){
        jack(j,i)=jack(j,i)/temp_sum;
        temp_sig+=jack(j,i);
   }
   jack(Nbin,i)=temp_sig;
}

///compute enrichment

double per_her;
double per_size;
int total_size=0;

for (int i=0;i<Nbin;i++)
   total_size+=len[i];

for (int j=0;j<Nbin;j++){
        per_her=point_est(j,0)/point_est(Nbin,0);
        per_size=(double)len[j]/total_size;
        enrich_point_est(j,0)=per_her/per_size;
	//cout<<j<<" "<<per_her<<" "<<total_size<<" "<<len[j]<<" "<<per_size<<" "<<enrich_point_est(j,0)<<endl;
}

for (int i=0;i<Njack;i++){
    per_size=0;
    total_size=0;
    for (int j=0;j<Nbin;j++){
	total_size+=(len[j]-jack_bin_size(i,j));
    }
   for (int j=0;j<Nbin;j++){
   	per_her=jack(j,i)/jack(Nbin,i);
	per_size=(double)(len[j]-jack_bin_size(i,j))/total_size;
	enrich_jack(j,i)=per_her/per_size;
	}
}

////compute jackknife SE
MatrixXdr sum_row=jack.rowwise().mean();
MatrixXdr SEjack;
SEjack=MatrixXdr::Zero(Nbin+1,1);
double temp_val=0;
for (int i=0;i<=Nbin;i++){
    for (int j=0;j<Njack;j++){
	temp_val=jack(i,j)-sum_row(i);
	temp_val= temp_val* temp_val;
	SEjack(i,0)+=temp_val;
    }
    SEjack(i,0)=SEjack(i,0)*(Njack-1)/Njack;
    SEjack(i,0)=sqrt(SEjack(i,0));
}
///////// compute jackknife SE of enrichment
 sum_row=enrich_jack.rowwise().mean();
MatrixXdr enrich_SEjack;
enrich_SEjack=MatrixXdr::Zero(Nbin,1);
 temp_val=0;
for (int i=0;i<Nbin;i++){
    for (int j=0;j<Njack;j++){
        temp_val=enrich_jack(i,j)-sum_row(i);
        temp_val= temp_val* temp_val;
        enrich_SEjack(i,0)+=temp_val;
    }
    enrich_SEjack(i,0)=enrich_SEjack(i,0)*(Njack-1)/Njack;
    enrich_SEjack(i,0)=sqrt(enrich_SEjack(i,0));
}

//for (int i=0;i<Njack;i++)
  //  cout<<jack.col(i).transpose()<<endl;
cout<<"OUTPUT: "<<endl;
for (int j=0;j<Nbin;j++)
     cout<<"h^2 of bin "<<j<<" : "<<point_est(j,0)<<" ,  SE: "<<SEjack(j,0)<<endl;
cout<<"Total h^2 : "<<point_est(Nbin,0)<<" , SE: "<<SEjack(Nbin,0)<<endl;
for (int j=0;j<Nbin;j++)
     cout<<"Enrichment of bin "<<j<<" :"<<enrich_point_est(j,0)<<" ,  SE: "<<enrich_SEjack(j,0)<<endl;

/*cout<<"Point estimates :"<<endl;
cout<<point_est.transpose()<<endl;
cout<<"SEs     :"<<endl;
cout<<SEjack.transpose()<<endl;

cout<<"Enrichment :"<<endl;
cout<<enrich_point_est.transpose()<<endl;
cout<<"SEs     :"<<endl;
cout<<enrich_SEjack.transpose()<<endl;
*/
 std::ofstream outfile;
string add_output=command_line_opts.OUTPUT_FILE_PATH;
outfile.open(add_output.c_str(), std::ios_base::app);

outfile<<"Point estimates :"<<endl;
outfile<<point_est.transpose()<<endl;
outfile<<"SEs     :"<<endl;
outfile<<SEjack.transpose()<<endl;

outfile<<"Enrichment :"<<endl;
outfile<<enrich_point_est.transpose()<<endl;
outfile<<"SEs     :"<<endl;
outfile<<enrich_SEjack.transpose()<<endl;

	return 0;
}


#include "discrete_sequence.hpp"
#include "pow2.hpp"
#include <boost/hana/integral_constant.hpp>
#include <array>

using namespace boost::hana::literals;

std::array<int, 5> a{{1, 2, 3, 4, 5}};

template <typename UnderlyingExpr>
struct multiply_expr
{
    constexpr explicit multiply_expr(unsigned mult, UnderlyingExpr const& expr) :
        mult{mult}, expr{expr} {};

    constexpr auto operator()(size_t index) const {
        return mult * expr(index);
    }

    unsigned              mult;
    UnderlyingExpr const& expr;
};

template <long long N, typename UnderlyingSequence>
constexpr auto operator*(unsigned mult, pow_expr<N, UnderlyingSequence> const& pow)
{
    return multiply_expr{mult, pow};
}

int main()
{
    auto s = discrete_sequence{a};
    auto e = 2 * (s ^ 2_c);

    return e(0);
}


/**
 * \ file Chebyshev1Filter.cpp
 */

#include <ATK/EQ/Chebyshev1Filter.h>
#include <ATK/EQ/IIRFilter.h>

#include <ATK/Mock/FFTCheckerFilter.h>
#include <ATK/Mock/SimpleSinusGeneratorFilter.h>

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_NO_MAIN
#include <boost/test/unit_test.hpp>

#define PROCESSSIZE (1024*64)

BOOST_AUTO_TEST_CASE( IIRFilter_Chebyshev1LowPassCoefficients_1k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::IIRFilter<ATK::Chebyshev1LowPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(100);
  filter.set_ripple(3);
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0.01587182644655375));
  frequency_checks.push_back(std::make_pair(10000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_Chebyshev1LowPassCoefficients_100_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(100);

  ATK::IIRFilter<ATK::Chebyshev1LowPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(100);
  filter.set_ripple(3);
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(10, 0));
  frequency_checks.push_back(std::make_pair(100, 0.8413951421002385));
  frequency_checks.push_back(std::make_pair(1000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_Chebyshev1LowPassCoefficients_2k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(2000);

  ATK::IIRFilter<ATK::Chebyshev1LowPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(100);
  filter.set_ripple(3);
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(2000, 0.005576397202690614));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_Chebyshev1LowPassCoefficients_200_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(200);

  ATK::IIRFilter<ATK::Chebyshev1LowPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(100);
  filter.set_ripple(3);
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(200, 0.1962613670420864));
  frequency_checks.push_back(std::make_pair(1000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_Chebyshev1HighPassCoefficients_1k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::IIRFilter<ATK::Chebyshev1HighPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(100);
  filter.set_ripple(3);
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0.9792933050029933));
  frequency_checks.push_back(std::make_pair(10000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_Chebyshev1HighPassCoefficients_100_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(100);

  ATK::IIRFilter<ATK::Chebyshev1HighPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(100);
  filter.set_ripple(3);
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(10, 0));
  frequency_checks.push_back(std::make_pair(100, 0.841395141674365));
  frequency_checks.push_back(std::make_pair(1000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_Chebyshev1HighPassCoefficients_2k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(2000);

  ATK::IIRFilter<ATK::Chebyshev1HighPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(100);
  filter.set_ripple(3);
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(2000, 0.994533852735074));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_Chebyshev1HighPassCoefficients_200_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(200);

  ATK::IIRFilter<ATK::Chebyshev1HighPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(100);
  filter.set_ripple(3);
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(200, 0.841014519725825));
  frequency_checks.push_back(std::make_pair(1000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_Chebyshev1BandPassCoefficients_1k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::IIRFilter<ATK::Chebyshev1BandPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequencies(std::make_pair(200, 1000));
  filter.set_ripple(3);
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0.84139514149622));
  frequency_checks.push_back(std::make_pair(10000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_Chebyshev1BandPassCoefficients_100_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(100);

  ATK::IIRFilter<ATK::Chebyshev1BandPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequencies(std::make_pair(200, 1000));
  filter.set_ripple(3);
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(10, 0));
  frequency_checks.push_back(std::make_pair(100, 0.14682528623369445));
  frequency_checks.push_back(std::make_pair(1000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_Chebyshev1BandPassCoefficients_2k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(2000);

  ATK::IIRFilter<ATK::Chebyshev1BandPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequencies(std::make_pair(200, 1000));
  filter.set_ripple(3);
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(2000, 0.1462850319776135));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_Chebyshev1BandPassCoefficients_200_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(200);

  ATK::IIRFilter<ATK::Chebyshev1BandPassCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequencies(std::make_pair(200, 1000));
  filter.set_ripple(3);
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(200, 0.8413951007802791));
  frequency_checks.push_back(std::make_pair(1000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_Chebyshev1BandStopCoefficients_1k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::IIRFilter<ATK::Chebyshev1BandStopCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequencies(std::make_pair(200, 1000));
  filter.set_ripple(3);
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0.8413951401352051));
  frequency_checks.push_back(std::make_pair(10000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_Chebyshev1BandStopCoefficients_100_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(100);

  ATK::IIRFilter<ATK::Chebyshev1BandStopCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequencies(std::make_pair(200, 1000));
  filter.set_ripple(3);
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(10, 0));
  frequency_checks.push_back(std::make_pair(100, 0.8485500087021252));
  frequency_checks.push_back(std::make_pair(1000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_Chebyshev1BandStopCoefficients_2k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(2000);

  ATK::IIRFilter<ATK::Chebyshev1BandStopCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequencies(std::make_pair(200, 1000));
  filter.set_ripple(3);
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(2000, 0.8490125934799859));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_Chebyshev1BandStopCoefficients_200_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(200);

  ATK::IIRFilter<ATK::Chebyshev1BandStopCoefficients<double> > filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequencies(std::make_pair(200, 1000));
  filter.set_ripple(3);
  filter.set_order(3);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(200, 0.8413953236422822));
  frequency_checks.push_back(std::make_pair(1000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}


/******************************************************************************
 * Copyright (C) 2013 by Jerome Maye                                          *
 * jerome.maye@gmail.com                                                      *
 *                                                                            *
 * This program is free software; you can redistribute it and/or modify       *
 * it under the terms of the Lesser GNU General Public License as published by*
 * the Free Software Foundation; either version 3 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              *
 * Lesser GNU General Public License for more details.                        *
 *                                                                            *
 * You should have received a copy of the Lesser GNU General Public License   *
 * along with this program. If not, see <http://www.gnu.org/licenses/>.       *
 ******************************************************************************/

/** \file VectorDesignVariableTest.cpp
    \brief This file tests the VectorDesignVariable class.
  */

#include <cstddef>
#include <iomanip>
#include <iostream>

#include <gtest/gtest.h>

#include <Eigen/Core>
#include <Eigen/Dense>

#include <cholmod.h>
#include <SuiteSparseQR.hpp>

#include "aslam/calibration/algorithms/linalg.h"
#include "aslam/calibration/base/Timestamp.h"
#include "aslam/calibration/core/LinearSolver.h"
#include "aslam/calibration/statistics/NormalDistribution.h"

void evaluateSVDSPQRSolver(const Eigen::MatrixXd& A, const Eigen::VectorXd& b, const Eigen::VectorXd& x,
                           double tol = 1e-9) {
    cholmod_common cholmod;
    cholmod_l_start(&cholmod);
    cholmod_sparse* A_CS = aslam::calibration::eigenDenseToCholmodSparseCopy(A, &cholmod);
    cholmod_dense b_CD;
    aslam::calibration::eigenDenseToCholmodDenseView(b, &b_CD);
    Eigen::VectorXd x_est;
    aslam::calibration::LinearSolver linearSolver;
    for (std::ptrdiff_t i = 1; i < A.cols(); ++i) {
        //    double before = aslam::calibration::Timestamp::now();
        linearSolver.solve(A_CS, &b_CD, i, x_est);
        //    double after = aslam::calibration::Timestamp::now();
        double error = (b - A * x_est).norm();
        //    std::cout << std::fixed << std::setprecision(18) << "noscale: " << "error: "
        //      << error << " est_diff: " << (x - x_est).norm() << " time: "
        //      << after - before << std::endl;
        ASSERT_NEAR(error, 0, tol);
        linearSolver.getOptions().columnScaling = true;
        //    before = aslam::calibration::Timestamp::now();
        linearSolver.solve(A_CS, &b_CD, i, x_est);
        //    after = aslam::calibration::Timestamp::now();
        error = (b - A * x_est).norm();
        //    std::cout << std::fixed << std::setprecision(18) << "onscale: " << "error: "
        //      << error << " est_diff: " << (x - x_est).norm() << " time: "
        //      << after - before << std::endl;
        linearSolver.getOptions().columnScaling = false;
        ASSERT_NEAR(error, 0, tol);
        //    std::cout << "SVD rank: " << linearSolver.getSVDRank() << std::endl;
        //    std::cout << "SVD rank deficiency: " << linearSolver.getSVDRankDeficiency()
        //      << std::endl;
        //    std::cout << "QR rank: " << linearSolver.getQRRank() << std::endl;
        //    std::cout << "QR rank deficiency: "
        //      << linearSolver.getQRRankDeficiency() << std::endl;
        //    std::cout << "SV gap: " << linearSolver.getSvGap() << std::endl;
    }
    cholmod_l_free_sparse(&A_CS, &cholmod);
    cholmod_l_finish(&cholmod);
}

void evaluateSVDSolver(const Eigen::MatrixXd& A, const Eigen::VectorXd& b, const Eigen::VectorXd& x) {
    //  const double before = aslam::calibration::Timestamp::now();
    const Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV);
    Eigen::VectorXd x_est = svd.solve(b);
    //  const double after = aslam::calibration::Timestamp::now();
    //  const double error = (b - A * x_est).norm();
    //  std::cout << std::fixed << std::setprecision(18) << "error: " << error
    //    << " est_diff: " << (x - x_est).norm() << " time: " << after - before
    //    << std::endl;
    //  std::cout << "estimated rank: " << svd.nonzeroSingularValues() << std::endl;
    //  std::cout << "estimated rank deficiency: "
    //    << A.cols() - svd.nonzeroSingularValues() << std::endl;
}

void evaluateSPQRSolver(const Eigen::MatrixXd& A, const Eigen::VectorXd& b, const Eigen::VectorXd& x) {
    cholmod_common cholmod;
    cholmod_l_start(&cholmod);
    cholmod_sparse* A_CS = aslam::calibration::eigenDenseToCholmodSparseCopy(A, &cholmod);
    cholmod_dense b_CD;
    aslam::calibration::eigenDenseToCholmodDenseView(b, &b_CD);
    Eigen::VectorXd x_est;
    //  const double before = aslam::calibration::Timestamp::now();
    SuiteSparseQR_factorization<double>* factor =
        SuiteSparseQR_factorize<double>(SPQR_ORDERING_BEST, SPQR_DEFAULT_TOL, A_CS, &cholmod);
    cholmod_dense* Qtb = SuiteSparseQR_qmult<double>(SPQR_QTX, factor, &b_CD, &cholmod);
    cholmod_dense* x_est_cd = SuiteSparseQR_solve<double>(SPQR_RETX_EQUALS_B, factor, Qtb, &cholmod);
    cholmod_l_free_dense(&Qtb, &cholmod);
    aslam::calibration::cholmodDenseToEigenDenseCopy(x_est_cd, x_est);
    cholmod_l_free_dense(&x_est_cd, &cholmod);
    //  std::cout << "estimated rank: " << factor->rank << std::endl;
    //  std::cout << "estimated rank deficiency: " << A.cols() - factor->rank
    //    << std::endl;
    SuiteSparseQR_free(&factor, &cholmod);
    //  const double after = aslam::calibration::Timestamp::now();
    //  const double error = (b - A * x_est).norm();
    //  std::cout << std::fixed << std::setprecision(18) << "error: " << error
    //    << " est_diff: " << (x - x_est).norm() << " time: " << after - before
    //    << std::endl;
    cholmod_l_free_sparse(&A_CS, &cholmod);
    cholmod_l_finish(&cholmod);
}

TEST(AslamCalibrationTestSuite, testLinearSolver) {
    Eigen::MatrixXd A = Eigen::MatrixXd::Random(100, 30);
    const Eigen::VectorXd x = Eigen::VectorXd::Random(30);
    Eigen::VectorXd b = A * x;

    //  std::cout << "-------------------------------------------------" << std::endl;
    //  std::cout << "|                  Standard case                |" << std::endl;
    //  std::cout << "-------------------------------------------------" << std::endl;
    //  std::cout << "SVD-SPQR solver" << std::endl;
    evaluateSVDSPQRSolver(A, b, x);
    //  std::cout << "SVD solver" << std::endl;
    evaluateSVDSolver(A, b, x);
    //  std::cout << "SPQR solver" << std::endl;
    evaluateSPQRSolver(A, b, x);

    //  std::cout << "-------------------------------------------------" << std::endl;
    //  std::cout << "|                 Badly scaled case             |" << std::endl;
    //  std::cout << "-------------------------------------------------" << std::endl;
    //  A.col(2) = 1e6 * A.col(2);
    //  A.col(28) = 1e6 * A.col(28);
    //  b = A * x;
    //  std::cout << "SVD-SPQR solver" << std::endl;
    //  evaluateSVDSPQRSolver(A, b, x, 1e-3);
    //  std::cout << "SVD solver" << std::endl;
    //  evaluateSVDSolver(A, b, x);
    //  std::cout << "SPQR solver" << std::endl;
    //  evaluateSPQRSolver(A, b, x);

    //  std::cout << "-------------------------------------------------" << std::endl;
    //  std::cout << "|                 Rank-deficient case 1         |" << std::endl;
    //  std::cout << "-------------------------------------------------" << std::endl;
    //  A = Eigen::MatrixXd::Random(100, 30);
    //  A.col(10) = Eigen::VectorXd::Zero(A.rows());
    //  b = A * x;
    //  std::cout << "SVD-SPQR solver" << std::endl;
    //  evaluateSVDSPQRSolver(A, b, x, 1e-3);
    //  std::cout << "SVD solver" << std::endl;
    //  evaluateSVDSolver(A, b, x);
    //  std::cout << "SPQR solver" << std::endl;
    //  evaluateSPQRSolver(A, b, x);

    //  std::cout << "-------------------------------------------------" << std::endl;
    //  std::cout << "|                 Rank-deficient case 2         |" << std::endl;
    //  std::cout << "-------------------------------------------------" << std::endl;
    //  A = Eigen::MatrixXd::Random(100, 30);
    //  A.col(10) = 2 * A.col(1) + 5 * A.col(20);
    //  b = A * x;
    //  std::cout << "SVD-SPQR solver" << std::endl;
    //  evaluateSVDSPQRSolver(A, b, x, 1e-3);
    //  std::cout << "SVD solver" << std::endl;
    //  evaluateSVDSolver(A, b, x);
    //  std::cout << "SPQR solver" << std::endl;
    //  evaluateSPQRSolver(A, b, x);

    //  std::cout << "-------------------------------------------------" << std::endl;
    //  std::cout << "|                 Near rank-deficient case 1    |" << std::endl;
    //  std::cout << "-------------------------------------------------" << std::endl;
    //  A = Eigen::MatrixXd::Random(100, 30);
    //  A.col(10) = Eigen::VectorXd::Zero(A.rows());
    //  b = A * x;
    //  A.col(10) = aslam::calibration::NormalDistribution<100>(
    //    Eigen::VectorXd::Zero(A.rows()),
    //    1e-6 * Eigen::MatrixXd::Identity(A.rows(), A.rows())).getSample();
    //  std::cout << "SVD-SPQR solver" << std::endl;
    //  evaluateSVDSPQRSolver(A, b, x, 1e-3);
    //  std::cout << "SVD solver" << std::endl;
    //  evaluateSVDSolver(A, b, x);
    //  std::cout << "SPQR solver" << std::endl;
    //  evaluateSPQRSolver(A, b, x);

    //  std::cout << "-------------------------------------------------" << std::endl;
    //  std::cout << "|                 Near rank-deficient case 2    |" << std::endl;
    //  std::cout << "-------------------------------------------------" << std::endl;
    //  A = Eigen::MatrixXd::Random(100, 30);
    //  A.col(10) = 2 * A.col(1) + 5 * A.col(20);
    //  b = A * x;
    //  A.col(10) = aslam::calibration::NormalDistribution<100>(A.col(10),
    //    1e-20 * Eigen::MatrixXd::Identity(A.rows(), A.rows())).getSample();
    //  std::cout << "SVD-SPQR solver" << std::endl;
    //  evaluateSVDSPQRSolver(A, b, x, 1e-3);
    //  std::cout << "SVD solver" << std::endl;
    //  evaluateSVDSolver(A, b, x);
    //  std::cout << "SPQR solver" << std::endl;
    //  evaluateSPQRSolver(A, b, x);
}


#include <csapex/model/node.h>
#include <csapex/msg/io.h>
#include <csapex/utility/register_apex_plugin.h>

#include <csapex/msg/generic_value_message.hpp>
#include <csapex_scan_2d/scan_message.h>

#include <csapex/model/node_modifier.h>
#include <csapex/param/parameter_factory.h>

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics.hpp>
#include <cslibs_laser_processing/common/yaml-io.hpp>

namespace
{
double normalize(double angle)
{
    while (angle <= -M_PI)
        angle += 2 * M_PI;
    while (angle > M_PI)
        angle -= 2 * M_PI;
    return angle;
}
}  // namespace

namespace csapex
{
using namespace lib_laser_processing;
using namespace boost::accumulators;
using namespace connection_types;

class HeightPitchAngle : public csapex::Node
{
public:
    HeightPitchAngle() : toggle_counter_(0), toggle_(false)
    {
    }

    virtual void setup(NodeModifier& node_modifier) override
    {
        input_ = node_modifier.addInput<ScanMessage>("filtered scan");
        output_pitch_ = node_modifier.addOutput<double>("pitch");
        output_height_ = node_modifier.addOutput<double>("height");
    }

    void setupParameters(Parameterizable& parameters) override
    {
        parameters.addParameter(param::factory::declareRange("start height", 0.05, 1.0, 0.1, 0.001), std::bind(&HeightPitchAngle::updateStartParameters, this));
        parameters.addParameter(param::factory::declareRange("start pitch", -M_PI, M_PI, 0.0, 0.01), std::bind(&HeightPitchAngle::updateStartParameters, this));
        parameters.addParameter(param::factory::declareRange("switch after", 1, 100, 1, 1));
        parameters.addParameter(param::factory::declareBool("renew", false));

        parameters.addParameter(param::factory::declareTrigger("reset"), std::bind(&HeightPitchAngle::reset, this));
        parameters.addParameter(param::factory::declareBool("degrees", false));
    }

    virtual void process() override
    {
        ScanMessage::ConstPtr in = msg::getMessage<ScanMessage>(input_);
        const Scan& scan = in->value;
        bool deg = readParameter<bool>("degrees");
        int toggle_after = readParameter<int>("switch after");
        bool renew = readParameter<bool>("renew");

        for (const LaserBeam& b : scan.rays) {
            if (b.range() > 0.f)
                mean_dist_(b.posX());
        }

        if (toggle_) {
            /// fit height
            height_ = sin(pitch_) * mean(mean_dist_);
        } else {
            /// fit angle
            pitch_ = normalize(asin(height_ / mean(mean_dist_)));
        }
        ++toggle_counter_;

        if (toggle_counter_ >= toggle_after) {
            toggle_counter_ = 0;
            toggle_ = !toggle_;
            if (renew)
                reset();
        }

        double out_pitch = pitch_;
        if (deg)
            out_pitch *= 180.0 / M_PI;

        msg::publish(output_pitch_, out_pitch);
        msg::publish(output_height_, height_);
    }

private:
    Input* input_;
    Output* output_pitch_;
    Output* output_height_;
    double height_;
    double pitch_;
    int toggle_counter_;
    bool toggle_;

    accumulator_set<double, stats<tag::mean>> mean_dist_;

    void reset() override
    {
        mean_dist_ = accumulator_set<double, stats<tag::mean>>();
    }

    void updateStartParameters()
    {
        height_ = readParameter<double>("start height");
        pitch_ = readParameter<double>("start pitch");
        reset();
    }
};
}  // namespace csapex

CSAPEX_REGISTER_CLASS(csapex::HeightPitchAngle, csapex::Node)


//==================================================================================================
/*!
    @file

    @Copyright 2016 Numscale SAS

    Distributed under the Boost Software License, Version 1.0.
    (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_X86_SSE1_SCALAR_FUNCTION_RSQRT_HPP_INCLUDED
#define BOOST_SIMD_ARCH_X86_SSE1_SCALAR_FUNCTION_RSQRT_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/function/raw.hpp>
#include <boost/simd/function/pedantic.hpp>
#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/if_nan_else.hpp>
#include <boost/simd/function/if_zero_else.hpp>
#include <boost/simd/function/is_eqz.hpp>
#include <boost/simd/function/is_ltz.hpp>
#include <boost/simd/function/refine_rsqrt.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/detail/constant/denormalfactor.hpp>
#include <boost/simd/detail/constant/denormalsqrtfactor.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd =  boost::dispatch;
  namespace bs =  boost::simd;
  BOOST_DISPATCH_OVERLOAD ( rsqrt_
                          , (typename A0)
                          , bs::sse1_
                          , bs::raw_tag
                          , bd::scalar_<bd::single_<A0>>
                          )
  {
    BOOST_FORCEINLINE A0 operator() (raw_tag const&
                                    , const A0 & a0) const BOOST_NOEXCEPT
    {
      float inv;
      _mm_store_ss( &inv, _mm_rsqrt_ss( _mm_load_ss( &a0 ) ) );
      return inv;
    }
  };

  BOOST_DISPATCH_OVERLOAD ( rsqrt_
                          , (typename A0)
                          , bs::sse1_
                          , bs::raw_tag
                          , bd::scalar_<bd::double_<A0>>
                          )
  {
    BOOST_FORCEINLINE A0 operator() (raw_tag const&
                                    , const A0 & a0) const BOOST_NOEXCEPT
    {
      float inv = a0;
      _mm_store_ss( &inv, _mm_rsqrt_ss( _mm_load_ss( &inv ) ) );
      return double(inv);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( rsqrt_
                          , (typename A0)
                          , bs::sse1_
                          , bd::scalar_<bd::single_<A0>>
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const A0 & a00) const BOOST_NOEXCEPT
    {
      if (is_eqz(a00)) return Inf<A0>();
      #ifndef BOOST_SIMD_NO_INFINITIES
      if (a00 == Inf<A0>()) return Zero<A0>();
      #endif
      A0 a0 =  raw_(rsqrt)(a00);
      A0 y = sqr(a0)*a00;
      return a0*Ratio<A0, 1, 8>()*fnms(y, fnms(A0(3), y, A0(10)), A0(15)); //this is Halley cubically convergent iteration
    }
  };

  BOOST_DISPATCH_OVERLOAD ( rsqrt_
                          , (typename A0)
                          , bs::sse1_
                          , bs::pedantic_tag
                          , bd::scalar_<bd::single_<A0>>
                          )
  {
    BOOST_FORCEINLINE A0 operator() (pedantic_tag const&
                                    ,const A0 & a00) const BOOST_NOEXCEPT
    {
      if (is_eqz(a00)) return Inf<A0>();
      #ifndef BOOST_SIMD_NO_INFINITIES
      if (a00 == Inf<A0>()) return Zero<A0>();
      #endif
      A0 a0 = a00;
      auto is_den = bs::abs(a00) < Smallestposval<A0>();
      #ifndef BOOST_SIMD_NO_DENORMALS
      a0 *= if_else(is_den, Denormalfactor<A0>(), One<A0>());
      #endif
      a0 = refine_rsqrt(a0, refine_rsqrt(a0, raw_(rsqrt)(a0)));// Two Newton is sometimes better than one halley
      #ifndef BOOST_SIMD_NO_DENORMALS
      a0 *= if_else(is_den, Denormalsqrtfactor<A0>(), One<A0>());
      #endif
      return a0;
    }
  };

  BOOST_DISPATCH_OVERLOAD ( rsqrt_
                          , (typename A0)
                          , bs::sse1_
                          , bs::pedantic_tag
                          , bd::scalar_<bd::double_<A0>>
                          )
  {
    BOOST_FORCEINLINE A0 operator() (pedantic_tag const&
                                    ,const A0 & a00) const BOOST_NOEXCEPT
    {
      if (is_eqz(a00)) return Inf<A0>();
      #ifndef BOOST_SIMD_NO_INFINITIES
      if (a00 == Inf<A0>()) return Zero<A0>();
      #endif
      A0 a01 =  a00;
      auto is_den = bs::abs(a00) < Smallestposval<A0>();
      #ifndef BOOST_SIMD_NO_DENORMALS
      a01 *= if_else(is_den, Denormalfactor<A0>(), One<A0>());
      #endif
      A0 a0 =  raw_(rsqrt)(a01);
      A0 y = sqr(a0)*a01;
      a0 = a0*Ratio<A0, 1, 8>()*fnms(y, fnms(A0(3), y, A0(10)), A0(15)); //this is Halley cubically convergent iteration
      a0 = refine_rsqrt(a00, a0);//this is Newton iteration
      #ifndef BOOST_SIMD_NO_DENORMALS
      a0 *= if_else(is_den, Denormalsqrtfactor<A0>(), One<A0>());
      #endif
      return a0;
    }
  };
} } }

#endif