// Copyright (c) 2007, 2008 libmv authors.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to
// deal in the Software without restriction, including without limitation the
// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
// sell copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
// IN THE SOFTWARE.

// This file is part of OpenMVG, an Open Multiple View Geometry C++ library.

// Copyright (c) 2012, 2013 Pierre MOULON.

// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "openMVG/numeric/numeric.h"

#include <Eigen/Geometry>

#include <fstream>

namespace openMVG {

Mat3 CrossProductMatrix(const Vec3 &x) {
  Mat3 X;
  X << 0, -x(2),  x(1),
    x(2),     0, -x(0),
   -x(1),  x(0),     0;
  return X;
}

Mat3 RotationAroundX(double angle) {
  return Eigen::AngleAxisd(angle, Vec3::UnitX()).toRotationMatrix();
}

Mat3 RotationAroundY(double angle) {
  return Eigen::AngleAxisd(angle, Vec3::UnitY()).toRotationMatrix();
}

Mat3 RotationAroundZ(double angle) {
  return Eigen::AngleAxisd(angle, Vec3::UnitZ()).toRotationMatrix();
}

double getRotationMagnitude(const Mat3 & R2) {
  const Mat3 R1 = Mat3::Identity();
  double cos_theta = (R1.array() * R2.array()).sum() / 3.0;
  cos_theta = clamp(cos_theta, -1.0, 1.0);
  return std::acos(cos_theta);
}

Mat3 LookAt(const Vec3 &center, const Vec3 & up) {
  Vec3 zc = center.normalized();
  Vec3 xc = up.cross(zc).normalized();
  Vec3 yc = zc.cross(xc);
  Mat3 R;
  R.row(0) = xc;
  R.row(1) = yc;
  R.row(2) = zc;
  return R;
}

//eyePosition3D is a XYZ position. This is where you are (your eye is).
//center3D is the XYZ position where you want to look at.
//upVector3D is a XYZ normalized vector. Quite often 0.0, 1.0, 0.0

Mat3 LookAt2(const Vec3 &eyePosition3D,
  const Vec3 &center3D,
  const Vec3 &upVector3D )
{
  Vec3 forward, side, up;
  Mat3 matrix2, resultMatrix;
  //------------------
  forward = center3D - eyePosition3D;
  forward.normalize();
  //------------------
  //Side = forward x up
  //ComputeNormalOfPlane(side, forward, upVector3D);
  side[0]=(forward[1]*upVector3D[2])-(forward[2]*upVector3D[1]);
  side[1]=(forward[2]*upVector3D[0])-(forward[0]*upVector3D[2]);
  side[2]=(forward[0]*upVector3D[1])-(forward[1]*upVector3D[0]);
  side.normalize();
  //------------------
  //Recompute up as: up = side x forward
  //ComputeNormalOfPlane(up, side, forward);
  up[0]=(side[1]*forward[2])-(side[2]*forward[1]);
  up[1]=(side[2]*forward[0])-(side[0]*forward[2]);
  up[2]=(side[0]*forward[1])-(side[1]*forward[0]);

  //------------------
  matrix2(0) = side[0];
  matrix2(1) = side[1];
  matrix2(2) = side[2];
  //------------------
  matrix2(3) = up[0];
  matrix2(4) = up[1];
  matrix2(5) = up[2];
  //------------------
  matrix2(6) = -forward[0];
  matrix2(7) = -forward[1];
  matrix2(8) = -forward[2];

  return matrix2;
}

void MeanAndVarianceAlongRows(const Mat &A,
  Vec *mean_pointer,
  Vec *variance_pointer) {
  const Mat::Index n = A.rows();
  const double m = static_cast<double>(A.cols());
  (*mean_pointer) = Vec::Zero(n);
  (*variance_pointer) = Vec::Zero(n);

  for (Mat::Index i = 0; i < n; ++i) {
    (*mean_pointer)(i) += A.row(i).array().sum();
    (*variance_pointer)(i) += (A.row(i).array() * A.row(i).array()).array().sum();
  }
  (*mean_pointer) /= m;
  for (Mat::Index i = 0; i < n; ++i) {
    (*variance_pointer)(i) = (*variance_pointer)(i) / m - Square((*mean_pointer)(i));
  }
}

bool exportMatToTextFile(const Mat & mat, const std::string & filename,
  const std::string & sPrefix)
{
  bool bOk = false;
  std::ofstream outfile;
  outfile.open(filename.c_str(), std::ios_base::out);
  if (outfile.is_open()) {
    outfile << sPrefix << "=[" << std::endl;
    for (int j=0; j < mat.rows(); ++j)  {
      for (int i=0; i < mat.cols(); ++i)  {
        outfile << mat(j,i) << " ";
      }
      outfile << ";\n";
    }
    outfile << "];";
    bOk = true;
  }
  outfile.close();
  return bOk;
}

}  // namespace openMVG


/******************************************************************************
 * 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/>.       *
 ******************************************************************************/

#include "aslam/calibration/functions/IncompleteGammaQFunction.h"

#include <boost/math/special_functions/gamma.hpp>

namespace aslam {
  namespace calibration {

/******************************************************************************/
/* Constructors and Destructor                                                */
/******************************************************************************/

    IncompleteGammaQFunction::IncompleteGammaQFunction(double alpha) :
        mAlpha(alpha) {
    }

    IncompleteGammaQFunction::IncompleteGammaQFunction(const
        IncompleteGammaQFunction& other) :
        mAlpha(other.mAlpha) {
    }

    IncompleteGammaQFunction& IncompleteGammaQFunction::operator = (const
        IncompleteGammaQFunction& other) {
      if (this != &other) {
        mAlpha = other.mAlpha;
      }
      return *this;
    }

    IncompleteGammaQFunction::~IncompleteGammaQFunction() {
    }

/******************************************************************************/
/* Stream operations                                                          */
/******************************************************************************/

    void IncompleteGammaQFunction::read(std::istream& stream) {
    }

    void IncompleteGammaQFunction::write(std::ostream& stream) const {
      stream << "alpha: " << mAlpha;
    }

    void IncompleteGammaQFunction::read(std::ifstream& stream) {
    }

    void IncompleteGammaQFunction::write(std::ofstream& stream) const {
    }

/******************************************************************************/
/* Accessors                                                                  */
/******************************************************************************/

    double IncompleteGammaQFunction::getValue(const VariableType& argument)
        const {
      return boost::math::gamma_q(mAlpha, argument);
    }

    double IncompleteGammaQFunction::getAlpha() const {
      return mAlpha;
    }

    void IncompleteGammaQFunction::setAlpha(double alpha) {
      mAlpha = alpha;
    }

  }
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2011 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2011 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2011 Mateusz Loskot, London, UK.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
// Transformation Example

#include <iostream>

#include <boost/geometry/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/adapted/c_array.hpp>

BOOST_GEOMETRY_REGISTER_C_ARRAY_CS(cs::cartesian)

int main()
{
    using namespace boost::geometry;

    typedef model::d2::point_xy<double> point_2d;
    point_2d p(1, 1);
    point_2d p2;

    // Example: translate a point over (5,5)
    strategy::transform::translate_transformer<point_2d, point_2d> translate(5, 5);

    transform(p, p2, translate);
    std::cout << "transformed point " << boost::geometry::dsv(p2) << std::endl;

    // Transform a polygon
    model::polygon<point_2d> poly, poly2;
    const double coor[][2] = { {0, 0}, {0, 7}, {2, 2}, {2, 0}, {0, 0} };
    // note that for this syntax you have to include the two
    // include files above (c_array.hpp)
    assign_points(poly, coor);
    //read_wkt("POLYGON((0 0,0 7,4 2,2 0,0 0))", poly);
    transform(poly, poly2, translate);

    std::cout << "source      polygon " << boost::geometry::dsv(poly) << std::endl;
    std::cout << "transformed polygon " << boost::geometry::dsv(poly2) << std::endl;

    // Many more transformations are possible:
    // - from Cartesian to Spherical coordinate systems and back
    // - from Cartesian to Cartesian (mapping, affine transformations) and back (inverse)
    // - Map Projections
    // - from Degree to Radian and back in spherical_equatorial or geographic coordinate systems

    return 0;
}


/*
 * Copyright Andrey Semashev 2020
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or copy at
 * https://www.boost.org/LICENSE_1_0.txt)
 */
/*!
 * \file is_power_of_2.hpp
 *
 * This header defines \c is_power_of_2 algorithm, which returns \c true if the passed integer is a power of 2.
 */

#ifndef BOOST_BIT_OPS_POW2_IS_POWER_OF_2_HPP_INCLUDED_
#define BOOST_BIT_OPS_POW2_IS_POWER_OF_2_HPP_INCLUDED_

#include <boost/bit_ops/detail/config.hpp>
#include <boost/bit_ops/detail/type_traits/enable_if.hpp>
#include <boost/bit_ops/detail/type_traits/is_integral.hpp>
#include <boost/bit_ops/detail/type_traits/is_enum.hpp>
#include <boost/bit_ops/detail/type_traits/is_signed.hpp>
#include <boost/bit_ops/detail/type_traits/is_unsigned.hpp>
#include <boost/bit_ops/detail/type_traits/make_unsigned.hpp>
#include <boost/bit_ops/detail/type_traits/underlying_type.hpp>

namespace boost {
namespace bit_ops {

//! Tests if the integer is a power of 2
template< typename T >
inline BOOST_CONSTEXPR typename bit_ops::detail::enable_if<
    bit_ops::detail::is_integral< T >::value && bit_ops::detail::is_unsigned< T >::value,
    bool
>::type is_power_of_2(T value) BOOST_NOEXCEPT
{
    return static_cast< bool >((value == static_cast< T >(0u)) ^ ((value & (value - static_cast< T >(1u))) == static_cast< T >(0u)));
}

//! Tests if the integer is a power of 2
template< typename T >
inline BOOST_CONSTEXPR typename bit_ops::detail::enable_if<
    bit_ops::detail::is_integral< T >::value && bit_ops::detail::is_signed< T >::value,
    bool
>::type is_power_of_2(T value) BOOST_NOEXCEPT
{
    typedef typename bit_ops::detail::make_unsigned< T >::type unsigned_type;
    return value <= static_cast< T >(0) ? false : bit_ops::is_power_of_2(static_cast< unsigned_type >(value));
}

//! Tests if the enum is a power of 2
template< typename T >
inline BOOST_CONSTEXPR typename bit_ops::detail::enable_if<
    bit_ops::detail::is_enum< T >::value,
    bool
>::type is_power_of_2(T value) BOOST_NOEXCEPT
{
    return bit_ops::is_power_of_2(static_cast< typename bit_ops::detail::underlying_type< T >::type >(value));
}

} // namespace bit_ops
} // namespace boost

#endif // BOOST_BIT_OPS_POW2_IS_POWER_OF_2_HPP_INCLUDED_


/*
  [auto_generated]
  boost/numeric/odeint/stepper/symplectic_rkn_sb3a_m4_mclachlan.hpp

  [begin_description]
  tba.
  [end_description]

  Copyright 2009-2012 Karsten Ahnert
  Copyright 2009-2012 Mario Mulansky

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE_1_0.txt or
  copy at http://www.boost.org/LICENSE_1_0.txt)
*/

#ifndef BOOST_NUMERIC_ODEINT_STEPPER_SYMPLECTIC_RKN_SB3A_M4_MCLACHLAN_HPP_DEFINED
#define BOOST_NUMERIC_ODEINT_STEPPER_SYMPLECTIC_RKN_SB3A_M4_MCLACHLAN_HPP_DEFINED

#include <boost/numeric/odeint/algebra/default_operations.hpp>
#include <boost/numeric/odeint/algebra/algebra_dispatcher.hpp>
#include <boost/numeric/odeint/algebra/operations_dispatcher.hpp>

#include <boost/numeric/odeint/util/resizer.hpp>

namespace boost {
namespace numeric {
namespace odeint {

#ifndef DOXYGEN_SKIP
namespace detail {
namespace symplectic_rkn_sb3a_m4_mclachlan {

    /*
      exp( a1 t A ) exp( b1 t B )
      exp( a2 t A ) exp( b2 t B )
      exp( a3 t A )
      exp( b2 t B ) exp( a2 t A )
      exp( b1 t B ) exp( a1 t A )
    */

    template< class Value >
    struct coef_a_type : public boost::array< Value , 5 >
    {
        coef_a_type( void )
        {
            using std::sqrt;

            Value z = sqrt( static_cast< Value >( 7 ) / static_cast< Value >( 8 ) ) / static_cast< Value >( 3 );
            (*this)[0] = static_cast< Value >( 1 ) / static_cast< Value >( 2 ) - z ;
            (*this)[1] = static_cast< Value >( -1 ) / static_cast< Value >( 3 ) + z ;
            (*this)[2] = static_cast< Value >( 2 ) / static_cast< Value >( 3 );
            (*this)[3] = (*this)[1];
            (*this)[4] = (*this)[0];
        }
    };

    template< class Value >
    struct coef_b_type : public boost::array< Value , 5 >
    {
        coef_b_type( void )
        {
            (*this)[0] = static_cast< Value >( 1 );
            (*this)[1] = static_cast< Value >( -1 ) / static_cast< Value >( 2 );
            (*this)[2] = (*this)[1];
            (*this)[3] = (*this)[0];
            (*this)[4] = static_cast< Value >( 0 );
        }
    };

} // namespace symplectic_rkn_sb3a_m4_mclachlan
} // namespace detail
#endif // DOXYGEN_SKIP

template<
    class Coor ,
    class Momentum = Coor ,
    class Value = double ,
    class CoorDeriv = Coor ,
    class MomentumDeriv = Coor ,
    class Time = Value ,
    class Algebra = typename algebra_dispatcher< Coor >::algebra_type ,
    class Operations = typename operations_dispatcher< Coor >::operations_type ,
    class Resizer = initially_resizer
    >
#ifndef DOXYGEN_SKIP
class symplectic_rkn_sb3a_m4_mclachlan :
        public symplectic_nystroem_stepper_base
<
    5 , 4 ,
    Coor , Momentum , Value , CoorDeriv , MomentumDeriv , Time , Algebra , Operations , Resizer
    >
#else
class symplectic_rkn_sb3a_m4_mclachlan : public symplectic_nystroem_stepper_base
#endif
{
public:
#ifndef DOXYGEN_SKIP
    typedef symplectic_nystroem_stepper_base
    <
    5 , 4 ,
    Coor , Momentum , Value , CoorDeriv , MomentumDeriv , Time , Algebra , Operations , Resizer
    > stepper_base_type;
#endif
    typedef typename stepper_base_type::algebra_type algebra_type;
    typedef typename stepper_base_type::value_type value_type;

    symplectic_rkn_sb3a_m4_mclachlan( const algebra_type &algebra = algebra_type() )
        : stepper_base_type(
            detail::symplectic_rkn_sb3a_m4_mclachlan::coef_a_type< value_type >() ,
            detail::symplectic_rkn_sb3a_m4_mclachlan::coef_b_type< value_type >() ,
            algebra )
    { }
};

/***************** DOXYGEN ***************/

/**
 * \class symplectic_rkn_sb3a_m4_mclachlan
 * \brief Implementation of the symmetric B3A Runge-Kutta Nystroem method of fifth order.
 *
 * The method is of fourth order and has five stages. It is described HERE. This method can be used
 * with multiprecision types since the coefficients are defined analytically.
 *
 * ToDo: add reference to paper.
 *
 * \tparam Order The order of the stepper.
 * \tparam Coor The type representing the coordinates q.
 * \tparam Momentum The type representing the coordinates p.
 * \tparam Value The basic value type. Should be something like float, double or a high-precision type.
 * \tparam CoorDeriv The type representing the time derivative of the coordinate dq/dt.
 * \tparam MomemtnumDeriv The type representing the time derivative of the momentum dp/dt.
 * \tparam Time The type representing the time t.
 * \tparam Algebra The algebra.
 * \tparam Operations The operations.
 * \tparam Resizer The resizer policy.
 */

    /**
     * \fn symplectic_rkn_sb3a_m4_mclachlan::symplectic_rkn_sb3a_m4_mclachlan( const algebra_type &algebra )
     * \brief Constructs the symplectic_rkn_sb3a_m4_mclachlan. This constructor can be used as a default
     * constructor if the algebra has a default constructor.
     * \param algebra A copy of algebra is made and stored inside explicit_stepper_base.
     */

} // namespace odeint
} // namespace numeric
} // namespace boost

#endif // BOOST_NUMERIC_ODEINT_STEPPER_SYMPLECTIC_RKN_SB3A_M4_MCLACHLAN_HPP_DEFINED


/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu>, Randi Cabezas <rcabezas@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */

#pragma once
#include <iostream>
#include <stdint.h>
#include <vector>
#include <Eigen/Dense>

#include <boost/shared_ptr.hpp>

#include <dpMM/dpMM.hpp>
#include <dpMM/cat.hpp>
#include <dpMM/dir.hpp>
#include <dpMM/niw.hpp>
#include <dpMM/sampler.hpp>
#include <dpMM/basemeasure.hpp>

using namespace Eigen;
using std::cout;
using std::endl;
using boost::shared_ptr;
using std::vector;

template<typename T=double>
class DirNaiveBayes : public DpMM<T>{

public:
  DirNaiveBayes(const Dir<Cat<T>, T>& alpha, const boost::shared_ptr<BaseMeasure<T> >& theta);
  DirNaiveBayes(const Dir<Cat<T>, T>& alpha, const vector<boost::shared_ptr<BaseMeasure<T> > >&thetas);
  virtual ~DirNaiveBayes();

  virtual void initialize(const vector< Matrix<T,Dynamic,Dynamic> >&x);
  virtual void initialize(const boost::shared_ptr<ClGMMData<T> >&cld)
    {cout<<"not supported"<<endl; assert(false);};

  virtual void sampleLabels();
  virtual void sampleParameters();

  virtual T logJoint(bool verbose=false);
  virtual const VectorXu& labels(){return z_;};
  virtual const VectorXu& getLabels(){return z_;};
  virtual uint32_t getK() const { return K_;};

//  virtual MatrixXu mostLikelyInds(uint32_t n);
  virtual MatrixXu mostLikelyInds(uint32_t n, Matrix<T,Dynamic,Dynamic>& logLikes);

  Matrix<T,Dynamic,1> getCounts();

  virtual void inferAll(uint32_t nIter, bool verbose=false);
  virtual void dump(std::ofstream& fOutMeans, std::ofstream& fOutCovs);

protected:
  uint32_t K_;
  Dir<Cat<T>, T> dir_;
  Cat<T> pi_;
#ifdef CUDA
  SamplerGpu<T>* sampler_;
#else
  Sampler<T>* sampler_;
#endif
  Matrix<T,Dynamic,Dynamic> pdfs_;
//  Cat cat_;
  vector<boost::shared_ptr<BaseMeasure<T> > > thetas_;

  vector<Matrix<T,Dynamic,Dynamic> > x_;
  VectorXu z_;
};

// --------------------------------------- impl -------------------------------

template<typename T>
DirNaiveBayes<T>::DirNaiveBayes(const Dir<Cat<T>,T>& alpha, const boost::shared_ptr<BaseMeasure<T> >& theta) :
  K_(alpha.K_), dir_(alpha), pi_(dir_.sample()) //cat_(dir_.sample()),
{
// init the parameters
    cout<<"[DirNaiveBayes::DirNaiveBayes] creating thetas (you only gave me one)"<<endl;
    for (uint32_t k=0; k<K_; ++k)
      thetas_.push_back(boost::shared_ptr<BaseMeasure<T> >(theta->copy()));
};

template<typename T>
DirNaiveBayes<T>::DirNaiveBayes(const Dir<Cat<T>,T>& alpha,
    const vector<boost::shared_ptr<BaseMeasure<T> > >& thetas) :
  K_(alpha.K_), dir_(alpha), pi_(dir_.sample()), //cat_(dir_.sample()),
  thetas_(thetas)
{};

template<typename T>
DirNaiveBayes<T>::~DirNaiveBayes()
{
  if (sampler_ != NULL) delete sampler_;
};

template <typename T>
Matrix<T,Dynamic,1> DirNaiveBayes<T>::getCounts()
{
  return counts<T,uint32_t>(z_,K_);
};

template<typename T>
void DirNaiveBayes<T>::initialize(const vector< Matrix<T,Dynamic,Dynamic> > &x)
{
  cout<<"init"<<endl;
  x_ = x;
  // randomly init labels from prior
  z_.setZero(x.size());
  cout<<"sample pi"<<endl;
  pi_ = dir_.sample();
  cout<<"init pi="<<pi_.pdf().transpose()<<endl;
  pi_.sample(z_);

  pdfs_.setZero(x.size(),K_);
#ifdef CUDA
  sampler_ = new SamplerGpu<T>(uint32_t(x.size()),K_,dir_.pRndGen_);
#else
  sampler_ = new Sampler<T>(dir_.pRndGen_);
#endif

#pragma omp parallel for
  for(int32_t k=0; k<int32_t(K_); ++k)
    thetas_[k]->posterior(x_,z_,k);
};

template<typename T>
void DirNaiveBayes<T>::sampleLabels()
{
  // obtain posterior categorical under labels
  pi_ = dir_.posterior(z_).sample();
//  cout<<pi_.pdf().transpose()<<endl;

#pragma omp parallel for
  for(int32_t i=0; i<z_.size(); ++i)
  {
    //TODO: could buffer this better
    // compute categorical distribution over label z_i
    VectorXd logPdf_z = pi_.pdf().array().log();
    for(uint32_t k=0; k<K_; ++k)
    {
//      cout<<thetas_[k].logLikelihood(x_.col(i))<<" ";
		for(uint32_t w=0; w<x_[i].cols(); ++w)
		{
			logPdf_z[k] += thetas_[k]->logLikelihood(x_[i],w);
		}
    }
//    cout<<endl;
    // make pdf sum to 1. and exponentiate
    pdfs_.row(i) = (logPdf_z.array()-logSumExp(logPdf_z)).exp().matrix().transpose();
//    cout<<pi_.pdf().transpose()<<endl;
//    cout<<pdf.transpose()<<" |.|="<<pdf.sum();
//    cout<<" z_i="<<z_[i]<<endl;
  }
  // sample z_i
  sampler_->sampleDiscPdf(pdfs_,z_);
};

template<typename T>
void DirNaiveBayes<T>::sampleParameters()
{
#pragma omp parallel for
  for(int32_t k=0; k<int32_t(K_); ++k)
  {
    thetas_[k]->posterior(x_,z_,k);
//    cout<<"k:"<<k<<endl;
//    thetas_[k]->print();
  }
};

template<typename T>
T DirNaiveBayes<T>::logJoint(bool verbose)
{
  T logJoint = dir_.logPdf(pi_);
  if(verbose)
  	cout<<"log p(pi)="<<logJoint<<" -> ";

#pragma omp parallel for reduction(+:logJoint)
  for (int32_t k=0; k<int32_t(K_); ++k)
    logJoint = logJoint + thetas_[k]->logPdfUnderPrior();
	if(verbose)
		cout<<"log p(pi)*p(theta)="<<logJoint<<" -> ";

#pragma omp parallel for reduction(+:logJoint)
  for (int32_t i=0; i<z_.size(); ++i)
	  for(int32_t w=0; w<x_[i].cols(); ++w)
		logJoint = logJoint + thetas_[z_[i]]->logLikelihood(x_[i],w);
  if(verbose)
  	cout<<"log p(phi)*p(theta)*p(x|z,theta)="<<logJoint<<"]"<<endl;

  return logJoint;
};

template<typename T>
MatrixXu DirNaiveBayes<T>::mostLikelyInds(uint32_t n, Matrix<T,Dynamic,Dynamic>& logLikes)
{
  MatrixXu inds = MatrixXu::Zero(n,K_);
  logLikes = Matrix<T,Dynamic,Dynamic>::Ones(n,K_);
  logLikes *= -99999.0;

#pragma omp parallel for
  for (int32_t k=0; k<int32_t(K_); ++k)
  {
    for (uint32_t i=0; i<z_.size(); ++i)
      if(z_(i) == k)
      {
        T logLike = thetas_[z_[i]]->logLikelihood(x_[i]);
        for (uint32_t j=0; j<n; ++j)
          if(logLikes(j,k) < logLike)
          {
            for(uint32_t l=n-1; l>j; --l)
            {
              logLikes(l,k) = logLikes(l-1,k);
              inds(l,k) = inds(l-1,k);
            }
            logLikes(j,k) = logLike;
            inds(j,k) = i;
//            cout<<"after update "<<logLike<<endl;
//            Matrix<T,Dynamic,Dynamic> out(n,K_*2);
//            out<<logLikes.cast<T>(),inds.cast<T>();
//            cout<<out<<endl;
            break;
          }
      }
  }
  cout<<"::mostLikelyInds: logLikes"<<endl;
  cout<<logLikes<<endl;
  cout<<"::mostLikelyInds: inds"<<endl;
  cout<<inds<<endl;
  return inds;
};

template<typename T>
void DirNaiveBayes<T>::inferAll(uint32_t nIter, bool verbose)
{
  if(verbose){
  	cout<<"[DirNaiveBayes::inferALL] ------ inferingALL (nIter=" << nIter << ") ------"<<endl;
  	cout <<"initial labels:"<< endl;
  	cout<<this->labels().transpose()<<endl;
  }
  for(uint32_t t=0; t<nIter; ++t)
  {
    this->sampleLabels();
    this->sampleParameters();
    if(verbose){
		cout << "[" << std::setw(3)<< std::setfill('0')  << t <<"] label: "
    	<< this->labels().transpose()
      	<< " [joint= " << std::setw(6) << this->logJoint(false) << "]"<< endl;
    }
  }
}

template <typename T>
void DirNaiveBayes<T>::dump(std::ofstream& fOutMeans, std::ofstream& fOutCovs)
{
	cout << "dumping naiveBayes" << endl;
	cout << "doc index: " << endl;
	cout << this->labels().transpose() << endl;

	cout << "printing cluster params: " << endl;
	cout << K_ << endl;
	for(uint32_t k=0; k<K_; ++k)
	{
		cout << "theta: " << k << endl;
		thetas_[k]->print();
	}

	cout << "printing mixture params: " << endl;
	pi_.print();

}


/*! \file demo_2d_uncertainty.cpp
    \brief Demonstration of some 2D plot features.
    \details including showing values with uncertainty information as 'plus minus', confidence intervals and degrees of freedom estimates.
*/

// Copyright Jacob Voytko 2007
// Copyright Paul A. Bristow 2007, 2008, 2009, 2021

// Use, modification and distribution are subject to the
// Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)

// This file is written to be included from a Quickbook .qbk document.
// It can be compiled by the C++ compiler, and run. Any output can
// also be added here as comment or included or pasted in elsewhere.
// Caution: this file contains Quickbook markup as well as code
// and comments: don't change any of the special comment markups!

//[demo_2d_uncertainty_1

/*`First we need some includes to use Boost.SVG_Plot, and C++ Standard Library:
*/

#include <boost/svg_plot/svg_2d_plot.hpp>
// using namespace boost::svg;

//#include <boost/svg_plot/show_2d_settings.hpp> // Only needed for showing ALL the many settings used.
//using boost::svg::show_2d_plot_settings;
//void boost::svg::show_2d_plot_settings(svg_2d_plot&);

#include <boost/svg_plot/detail/pair.hpp>
// using boost::svg::detail::pair; operator<<

#include <boost/quan/unc.hpp>
//#include <boost/quan/unc_init.hpp>

#include <boost/quan/meas.hpp>

#include <algorithm>
//  using std::copy;
//#include <functional>

#include <map>
  //using std::map;
  //using std::multimap;

#include <utility>
  //using std::pair;
  //using std::make_pair;

#include <vector>
//  using std::vector;

#include <cmath>
//   using std::sqrt;

#include <iostream>
   //using std::cout;
   //using std::endl;
   //using std::scientific;
   //using std::hex;
   //using std::ios;
   //using std::boolalpha;
#include <iomanip>
// Using std::setw;
// using std::setprecision;

#include <iterator>
//   using std::ostream_iterator;

//] [/demo_2d_uncertainty_1]

  enum side
  { //! \enum side Where axis labels go.
    left_side = -1,
    on_axis = 0,
    right_side = +1,
    bottom_side = -1,
    top_side = +1,
  };

int main()
{
  using std::pair;
  using std::make_pair;
  using std::vector;
  using std::map;
  using std::multimap;
  using std::cout;
  using std::endl;
  using std::scientific;
  using std::hex;
  using std::ios;
  using std::boolalpha;
  using std::sqrt;

  using namespace boost::svg;
  using boost::svg::detail::operator<<;

  try
  {
//[demo_2d_uncertainty_2

/*`STL map is used as the container for our two data-series,
and pairs of values and their uncertainty information
(approximately standard deviation and degrees of freedom)
are inserted using push_back. Since this is a 2-D plot
the order of data values is important.
*/
  typedef unc<false> uncun; // Uncertain Uncorrelated (the normal case).

  using boost::svg::detail::operator<<;
  using std::ostream_iterator;

  setUncDefaults(std::cout);  // Initialize for uncertain types.

  //setPlusminusSds(2); // cout.iword(plusminusSdsIndex);

  // Create pair for fundamental type double that is implicitly exact with no uncertainty information.
  pair<double, double> double_pair; // double X and Y pair.
  double_pair = make_pair(double(-2.234), double(-8.76)); // Construct and then echo the data values thus:
  std::cout << double_pair.first << ", " << double_pair.second << std::endl; //  make_pair(double(-2.234), double(-8.76)) = -2.234, -8.76
  // But it is more convenient to use a specialization of operator<< for `std::pair` to show both:
  std::cout << "make_pair(double(-2.234), double(-8.76)) = " << double_pair << std::endl;
  // make_pair(double(-2.234), double(-8.76)) = -2.234, -8.76

  // Or, more informatively, use an uncertain type `uncun` that holds explicit uncertainty information
  // (standard-deviation, degrees of freedom and so can compute confidence internals).

  uncun ux(2.0, 0.5F, 7); // For an X-value.
  //uncun ux(1.03, 0.56F, 7); // For an X-value.
  // Using the  `uncun operator<<` provided we can output all the details of the uncertain value.
  std::cout << scientific << plusminus << addlimits << adddegfree << std::setw(20)  << std::left
    << "ux = " << ux << std::endl; // 1.2 +/-0.56 <0.82, 1.64> (7)

  uncun uy(4.0, 0.5F, 9); // For a Y-value.
//  uncun uy(3.45, 0.67F, 9); // For a Y-value.
  std::cout << "uy = " << uy << std::endl; // 3.5 +/-0.67 <3.01, 3.89> (9)

  // Now we make a pair of X and Y uncertain values:
  pair<uncun, uncun > mp1 = make_pair(ux, uy); // X & Y pair of two uncertain values.
  // Echo both X and Y-values and add all the uncertainty information, standard deviation and degrees of freedom :
  std::cout << "mp1 = "<< mp1 << std::endl; // 1.2 +/-0.56 <0.82, 1.64> (7), 3.5 +/-0.67 <3.01, 3.89> (9)

  std::map<uncun, uncun > data1; // Container for X & Y pairs of data-point values.
  data1.insert(mp1); // Insert 1st pair of X & Y.
  //data1.insert(make_pair(uncun(3.9, 0.01F, 8), uncun(1.1, 0.1F, 18))); // and add another X&Y-pair
  data1.insert(make_pair(uncun(3.9, 0.01F, 8), uncun(1.1, 0.2F, 18))); // and add another X&Y-pair
  data1.insert(make_pair(uncun(-2.234, 0.1F, 7), uncun(-5.76, 0.4F, 9))); // and a third pair.

 /*
`Make very sure you don't forget either uncun(...) like this
`data1.insert(make_pair((-2.234, 0.12F, 7),(-8.76, 0.56F, 9)));`
because, by the bizarre operation of the comma operator, the result will be an integer!
So you will astonished to find that the values will be the *pair of degrees of freedom, (7, 9)*
and the other parts of uncun will be undefined!

Echo the values input, correctly rounded using the uncertainy and degrees of freedom:
  */
  std::cout << data1.size() << " XY data pairs:" << std::endl;
  std::copy(data1.begin(), data1.end(), ostream_iterator<pair<uncun, uncun> >(std::cout, "\n"));
  std::cout << std::endl;

  /*
  3 XY data pairs:
   -2.23 +/-0.030 <-2.26, -2.21> (7), -8.8 +/-0.90 <-9.35, -8.17> (9)
   1.2 +/-0.56 <0.82, 1.64> (7), 3.5 +/-0.67 <3.01, 3.89> (9)
   4.1 +/-0.40 <3.82, 4.38> (8), 3.1 +/-0.30 <2.96, 3.24> (18)
  */

  svg_2d_plot my_plot;  // Construct an empty plot.

  /*`If you can be confident that the data set(s) only contains normal, valid data,
  so none are 'at limits' - too big or too small to be meaningful, infinite or NaN (NotANumber),
  then these checks can be skipped (for speed).
  An instrument or operator input might be known to provide only normal data.
  For this example, we know this is true, so override the default autoscale_check_limits(true).
  */
  my_plot.autoscale_check_limits(false);
  /*`The default is autoscale_plusminus(3.) so that confidence ellipses
  at 1, 2 and 3 (uncertainty nominally standard deviations)
  are all within the plot window,
  but if you are less interested in seeing the 2 and 3 ellipses,
  you could risk the outer edges spilling over the borders
  by reducing autoscale_plusminus, for example, to 1.5, down to zero.
  */
  //my_plot.autoscale_plusminus(1.5); // default is 3.
  // my_plot.confidence(0.01);  // Change from default 0.05 to 0.01 for 99% confidence.

  /*`Use data set `data` to autoscale. (You can use a different data set to scale from the one you chose to plot).
  */
  //my_plot.xy_autoscale(data1); // But may display data point value labels illegibly outside the plot frame or image.
  // Specifying the X- and Y-axes explicitly is usually best.

  //my_plot.confidence(0.01);  // Optionally use an alpha of 0.01 (99%) rather than default of 0.05 (95%) for culculation of confidence interval.

  //my_plot.plusminus_sds(2.); // Optionally display uncertainty multiplied by a factor of two instead of default one.

  my_plot
    // X values settings:
    .x_label("times (sec)")
    .x_range(-3, +10)
    //.x_values_on(true) // Show X-values next to each point. Triggers a warning if also write .xy_values_on(true)
    // SVG_plot warning : x_values_on has overwritten xy_values_on!

     //! \note Essential use of Unicode space &\#x00A0; in all strings - ANSI space has no effect!
  //.x_decor("t ", ", ", "sec") // Keep all on one line using X-separator does NOT start with a newline.
   .x_decor("g ", "\n", "sec") // Split X and Y onto two lines because X-separator *does* start with newline.
  // .x_decor("after t ", "\ntravels", "sec") // Split X and Y onto two lines because X-separator *does* start with newline,
    // and add some descriptive text too.
   // .x_values_rotation(backdown) // Not good.
   // .x_values_rotation(downward) //
   .x_values_rotation(uphill)
   //.x_values_rotation(steepup) //
   // .x_values_rotation(horizontal) // Lines up vertically, but shifted too high???
   // .x_values_rotation(upward) //
   // .x_values_rotation(rightward) //
    .x_values_font_size(10) // Bigger than default.
    .x_values_font_family("Times New Roman") // Serif font just to show difference from sans-serif used for Y value-labels.
    .x_major_grid_on(true)
    .y_major_grid_on(true)
    .x_major_interval(1)
    .y_major_interval(1)

    .x_plusminus_color(red) // Show plus/minus +/- uncertainty data-point value-label in red.
    .x_plusminus_on(true) // Show plus/minus +/- uncertainty with data-point value-labels, for example "2.1 +/- 0.001"

    // Y values settings:
    .y_label("distance (km)")
    .y_range(-10., +10.) // But may be over-written by x and or  y-autoscale below.
    //.y_values_on(true) // Show Y values next to each point.
    // SVG_plot warning : xy_values_on has overwritten y_values_on!

   // .y_values_rotation(downhill) // is ignored if both X and Y-values are shown on line below.
    .y_values_font_family("Arial") // Sans serif different from X-values font just to show effect.
    .y_values_font_size(8) // Smaller than default.
    .y_decor("y=", "", " km")
 //   .y_decor("           d  ", " ", " km")
//    .y_decor("   time = ", " ", " sec")
     // Note: a few padding spaces are used to get Y-value-labels to lie more nearly under X-value-labels.
     // This is only necessary when value-labels are not horizontal.
     // y_prefix "   "   3 spaces.
     // y_separator " time ="  space before word time.
     // y_suffix " sec" space before word sec.

    .y_plusminus_on(true) // Show plus/minus +/- uncertainty with data-point value-labels, for example "2.1 +/- 0.001"
    .y_plusminus_color(magenta) // Show plus/minus +/- uncertainty in magenta.

    .y_addlimits_on(true)  // Show plus/minus +/- confidence limits for data-point value-labels.
    .y_addlimits_color(darkgreen) // Show +/- in darkgreen, for example: "+/- 0.03".

    .y_df_on(true) // Show degrees of freedom (usually observations -1) for data-points.
    .y_df_color(green) // Show degrees of freedom in green, for examples: "11").

    .xy_values_on(true) // Show both X-values and Y-values next to each point.
     // IS needed if both .x_values_on(true) and .y_values_on(true) specified.
     // .xy_autoscale(data1) // may result in value-labels running off the plot and image.
   // .y_values_alignment(align_style::left_align)
    //.x_values_alignment(align_style::right_align) // By default, set by code #L2881 switch statement depending on rotation.

  /*`The default uncertainty ellipse colors (that apply to both X and Y axes)
  can be changed thus:
  */
    .one_sd_color(lightblue) // Color of ellipse for one standard deviation (about 66% probability).
    .two_sd_color(svg_color(200, 230, 255)) // Color of ellipse for two standard deviation (~95%).
    .three_sd_color(svg_color(230, 240, 255)) // Color of ellipse for two standard deviation (~99%).
   ; // my_plot

//  my_plot.plot(data1, "data1").shape(unc_ellipse).fill_color(lightyellow).stroke_color(magenta);
  my_plot.plot(data1, "data1").shape(unc_ellipse).fill_color(blue).stroke_color(magenta);
  // TODO the uncertainty ellipses are now not showing anything useful for 2D but OK for 1D??? :-(

  my_plot.write("./demo_2d_uncertainty");

  std::cout << "confidence alpha " << my_plot.confidence() <<", plusminus sd multiplier = " << my_plot.plusminus_sds() << std::endl;

  // show_2d_plot_settings(my_plot); // Needs #include <boost/svg_plot/show_2d_settings.hpp>

 //] [/demo_2d_uncertainty_2]

  }
  catch (const std::exception& e)
  {
    std::cout <<
      "\n""Message from thrown exception was:\n   " << e.what() << std::endl;
  }
  return 0;
} // int main()

/*

Output :

   ------ Build started: Project: demo_2d_uncertainty, Configuration: Debug Win32 ------
  unc.cpp
  unc_print.cpp
  unc_read.cpp
  demo_2d_uncertainty.cpp
  Generating Code...
  demo_2d_uncertainty.vcxproj -> J:\Cpp\SVG\Debug\demo_2d_uncertainty.exe
  make_pair(double(-2.234), double(-8.76)) = -2.234, -8.76
   make_pair(double(-2.234), double(-8.76)) = -2.234, -8.76
  ux = 1.2
  1.2, 3.5
  3 XY data pairs:
  -2.23, -8.8
  1.2, 3.5
  4.10, 3.100

  No limits checks: x_min = -2.234, x_max = 4.1, y_min = -8.76, y_max = 3.45

  axes_on true
  background_border_width 2
  background_border_color RGB(255,255,0)
  background_color RGB(255,255,255)
  image_border_margin() 3
  image_border_width() 2
  coord_precision 3
  copyright_date
  copyright_holder
  description
  document_title ""
  x_size 500
  image y_size 400
  image_filename
  legend_on false
  legend_place 2
  legend_top_left -1, -1, legend_bottom_right -1, -1
  legend_background_color blank
  legend_border_color RGB(255,255,0)
  legend_color blank
  legend_title ""
  legend_title_font_size 14
  legend_font_weight
  legend_width 0
  legend_lines true
  limit points stroke color RGB(119,136,153)
  limit points fill color RGB(250,235,215)
  license_on false
  license_reproduction permits
  license_distribution permits
  license_attribution requires
  license_commercialuse permits
  plot_background_color RGB(255,255,255)
  plot_border_color RGB(119,136,153)
  plot_border_width 2
  plot_window_on true
  plot_window_x 92.6, 474
  plot_window_x_left 92.6
  plot_window_x_right 474
  plot_window_y 8, 341
  plot_window_y_top 8
  plot_window_y_bottom 341
  title_on false
  title ""
  title_color blank
  title_font_alignment 2
  title_font_decoration
  title_font_family Verdana
  title_font_rotation 0
  title_font_size 18
  title_font_stretch
  title_font_style
  title_font_weight
  x_values_on false
  x_values_font_size 16
  x_values_font_family Times New Roman
  x_values_precision 3
  x_values_ioflags 200 IOS format flags (0x200) dec.
  y_values_precision 3
  y_values_font_size() 3
  y_values_ioflags 200 IOS format flags (0x200) dec.
  y_values_color blank
  y_values_font_family() Arial
  y_values_font_size() 12
  x_max 5
  x_min -3
  x_autoscale true
  y_autoscale true
  xy_autoscale true
  x_autoscale_check_limits false
  x_axis_on true
  x_axis_color() RGB(0,0,0)
  x_axis_label_color blank
  x_values_color blank
  x_axis_width 1
  x_label_on true
  x_label "times (sec)"
  x_label_color blank
  x_label_font_family Verdana
  x_label_font_size 14
  x_label_units
  x_label_units_on false
  x_major_labels_side left
  x_major_label_rotation 0
  x_major_grid_color RGB(200,220,255)
  x_major_grid_on false
  x_major_grid_width 1
  x_major_interval 1
  x_major_tick 1
  x_major_tick_color RGB(0,0,0)
  x_major_tick_length 5
  x_major_tick_width 2
  x_minor_interval 0
  x_minor_tick_color RGB(0,0,0)
  x_minor_tick_length 2
  x_minor_tick_width 1
  x_minor_grid_on false
  x_minor_grid_color RGB(200,220,255)
  x_minor_grid_width 0.5
  x_range() -3, 5
  x_num_minor_ticks 4
  x_ticks_down_on true
  x_ticks_up_on false
  x_ticks_on_window_or_axis bottom
  y_axis_position y_axis_position intersects X axis (X range includes zero)
  x_axis_position x_axis_position intersects Y axis (Y range includes zero)
  x_plusminus_on true
  x_plusminus_color RGB(0,255,255)
  x_df_on true
  x_df_color RGB(255,0,255)
  x_prefix
  x_separator
  x_suffix
  xy_values_on true
  y_label_on "true"
  y_label_axis distance (km)
  y_axis_color RGB(0,0,0)
  y_axis_label_color blank
  y_axis_on true
  axes_on true
  y_axis_value_color RGB(0,0,0)
  y_axis_width 1
  y_label distance (km)
  y_label_color blank
  y_label_font_family Verdana
  y_label_font_size 14
  y_label_on true
  y_label_units
  y_label_units_on false
  y_label_width 0
  y_major_grid_on false
  y_major_grid_color RGB(200,220,255)
  y_major_grid_width 1
  y_major_interval 2.5
  y_major_labels_side bottom
  y_major_label_rotation 0
  y_major_tick_color RGB(0,0,0)
  y_major_tick_length  5
  y_major_tick_width  2
  y_minor_grid_on false
  y_minor_grid_color  RGB(200,220,255)
  y_minor_grid_width 0.5
  y_minor_interval 0
  y_minor_tick_color RGB(0,0,0)
  y_minor_tick_length 2
  y_minor_tick_width 1
  y_range() -10, 5
  y_num_minor_ticks
  y_ticks_left_on true
  y_ticks_right_on false
  y_ticks_on_window_or_axis left
  y_max 5
  y_min -10
  y_values_on false
  y_plusminus_on true
  y_plusminus_color RGB(255,0,0)
  y_df_on true
  y_df_color RGB(0,128,0)
  y_prefix "   "
  y_separator "time = "
  y_suffix " sec"
  data lines width 2
========== Build: 1 succeeded, 0 failed, 0 up-to-date, 0 skipped ==========

*/


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 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_ELLIPTIC_FUNCTIONS_SCALAR_ELLIK_HPP_INCLUDED
#define NT2_ELLIPTIC_FUNCTIONS_SCALAR_ELLIK_HPP_INCLUDED
#include <nt2/elliptic/functions/ellik.hpp>
#include <boost/math/special_functions/ellint_1.hpp>
#include <nt2/include/constants/eps.hpp>
#include <nt2/include/constants/nan.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/pi.hpp>
#include <nt2/include/constants/pio_2.hpp>
#include <nt2/include/functions/scalar/abs.hpp>
#include <nt2/include/functions/scalar/atan.hpp>
#include <nt2/include/functions/scalar/average.hpp>
#include <nt2/include/functions/scalar/ellint_1.hpp>
#include <nt2/include/functions/scalar/is_eqz.hpp>
#include <nt2/include/functions/scalar/is_ltz.hpp>
#include <nt2/include/functions/scalar/log.hpp>
#include <nt2/include/functions/scalar/oneminus.hpp>
#include <nt2/include/functions/scalar/oneplus.hpp>
#include <nt2/include/functions/scalar/sqrt.hpp>
#include <nt2/include/functions/scalar/tan.hpp>
#include <nt2/include/functions/scalar/toint.hpp>
#include <nt2/sdk/error/policies.hpp>

namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( ellik_, tag::cpu_
                            , (A0)
                            , (scalar_< double_<A0> >)
                              (scalar_< double_<A0> >)
                            )
  {
    typedef A0 result_type;
    NT2_FUNCTOR_CALL_REPEAT(2)
    {
      if (a1>nt2::One<A0>()||(nt2::is_ltz(a1))) return nt2::Nan<A0>();
      if (nt2::is_eqz(a1))  return A0(a0);
      return boost::math::ellint_1(nt2::sqrt(a1), a0, nt2_policy());
    }
  };

  BOOST_DISPATCH_IMPLEMENT  ( ellik_, tag::cpu_
                            , (A0)
                            , (scalar_< single_<A0> >)(scalar_< single_<A0> >)
                            )
  {
    typedef A0 result_type;
    NT2_FUNCTOR_CALL_REPEAT(2)
    {
      if (a1>nt2::One<A0>()||(nt2::is_ltz(a1))) return nt2::Nan<A0>();
      if (nt2::is_eqz(a1))  return a0;
      A0 phi = nt2::abs(a0);
      A0 m = a1;
      A0 a = nt2::One<A0>();
      A0 b = nt2::oneminus(m);
      if( nt2::is_eqz(b) )   return nt2::log(nt2::tan(nt2::average(Pio_2<A0>(),phi)));
      b = nt2::sqrt(b);
      A0 c = nt2::sqrt(m);
      A0 d = nt2::One<A0>();
      A0 t = nt2::tan(phi);
      int mod = nt2::toint((phi+nt2::Pio_2<A0>())/nt2::Pi<A0>());
      while( nt2::abs(c) > nt2::abs(a)*nt2::Eps<A0>() )
      {
        A0 temp = b/a;
        phi += nt2::atan(t*temp) + mod*nt2::Pi<A0>();
        mod = nt2::toint((phi+nt2::Pio_2<A0>())/nt2::Pi<A0>());
        t = t*nt2::oneplus(temp)/( nt2::oneminus(temp*t*t));
        c = nt2::average(a,-b);
        temp = nt2::sqrt(a*b);
        a = nt2::average(a,b);
        b = temp;
        d += d;
      }
      A0 temp = (nt2::atan(t) + mod * nt2::Pi<A0>())/(d * a);
      if( nt2::is_ltz(a0) )  temp = -temp;
      return temp;
    }
  };
} }

#endif


#include "xpbd/neohookean_elasticity_constraint.h"

#include <Eigen/Dense>
#include <eigen/SVD>

namespace xpbd {

neohookean_elasticity_constraint_t::neohookean_elasticity_constraint_t(
    std::initializer_list<index_type> indices,
    positions_type const& p,
    scalar_type young_modulus,
    scalar_type poisson_ratio,
    scalar_type const alpha)
    : base_type(indices, alpha), V0_{0.}, DmInv_{}, mu_{}, lambda_{}
{
    assert(indices.size() == 4u);

    auto const v1 = this->indices().at(0);
    auto const v2 = this->indices().at(1);
    auto const v3 = this->indices().at(2);
    auto const v4 = this->indices().at(3);

    auto const p1 = p.row(v1);
    auto const p2 = p.row(v2);
    auto const p3 = p.row(v3);
    auto const p4 = p.row(v4);

    Eigen::Matrix3d Dm;
    Dm.col(0) = (p1 - p4).transpose();
    Dm.col(1) = (p2 - p4).transpose();
    Dm.col(2) = (p3 - p4).transpose();

    V0_     = (1. / 6.) * Dm.determinant();
    DmInv_  = Dm.inverse();
    mu_     = (young_modulus) / (2. * (1 + poisson_ratio));
    lambda_ = (young_modulus * poisson_ratio) / ((1 + poisson_ratio) * (1 - 2 * poisson_ratio));
}

void neohookean_elasticity_constraint_t::project(
    positions_type& p,
    masses_type const& m,
    scalar_type& lagrange,
    scalar_type const dt) const
{
    auto const v1 = this->indices().at(0);
    auto const v2 = this->indices().at(1);
    auto const v3 = this->indices().at(2);
    auto const v4 = this->indices().at(3);

    auto const p1 = p.row(v1);
    auto const p2 = p.row(v2);
    auto const p3 = p.row(v3);
    auto const p4 = p.row(v4);

    auto const w1 = 1. / m(v1);
    auto const w2 = 1. / m(v2);
    auto const w3 = 1. / m(v3);
    auto const w4 = 1. / m(v4);

    auto const Vsigned        = signed_volume(p);
    bool const is_V_positive  = Vsigned >= 0.;
    bool const is_V0_positive = V0_ >= 0.;
    bool const is_tet_inverted =
        (is_V_positive && !is_V0_positive) || (!is_V_positive && is_V0_positive);

    Eigen::Matrix3d Ds;
    Ds.col(0) = (p1 - p4).transpose();
    Ds.col(1) = (p2 - p4).transpose();
    Ds.col(2) = (p3 - p4).transpose();

    Eigen::Matrix3d const F = Ds * DmInv_;
    Eigen::Matrix3d const I = Eigen::Matrix3d::Identity();

    scalar_type constexpr epsilon = 1e-20;

    Eigen::Matrix3d Piola;
    scalar_type psi{};

    // TODO: Implement correct inversion handling described in
    // Irving, Geoffrey, Joseph Teran, and Ronald Fedkiw. "Invertible finite elements for robust
    // simulation of large deformation." Proceedings of the 2004 ACM SIGGRAPH/Eurographics symposium
    // on Computer animation. 2004.
    if (is_tet_inverted)
    {
        Eigen::JacobiSVD<Eigen::Matrix3d> UFhatV(F, Eigen::ComputeFullU | Eigen::ComputeFullV);
        Eigen::Vector3d const Fsigma = UFhatV.singularValues();
        Eigen::Matrix3d Fhat;
        Fhat.setZero();
        Fhat(0, 0) = Fsigma(0);
        Fhat(1, 1) = Fsigma(1);
        Fhat(2, 2) = Fsigma(2);

        Eigen::Matrix3d U       = UFhatV.matrixU();
        Eigen::Matrix3d const V = UFhatV.matrixV();

        auto smallest_element_idx = 0;
        if (Fsigma(0) < Fsigma(1) && Fsigma(0) < Fsigma(2))
            smallest_element_idx = 0;
        if (Fsigma(1) < Fsigma(0) && Fsigma(1) < Fsigma(2))
            smallest_element_idx = 1;
        if (Fsigma(2) < Fsigma(0) && Fsigma(2) < Fsigma(1))
            smallest_element_idx = 2;

        Fhat(smallest_element_idx, smallest_element_idx) =
            -Fhat(smallest_element_idx, smallest_element_idx);
        U.col(smallest_element_idx) = -U.col(smallest_element_idx);

        // stress reaches maximum at 58% compression
        scalar_type constexpr min_singular_value = 0.577;
        Fhat(0, 0)                               = std::min(Fhat(0, 0), min_singular_value);
        Fhat(1, 1)                               = std::min(Fhat(1, 1), min_singular_value);
        Fhat(2, 2)                               = std::min(Fhat(2, 2), min_singular_value);

        Eigen::Matrix3d const Fprime = U * Fhat * V.transpose();
        Eigen::Matrix3d const F2     = Fprime.transpose() * Fprime;
        Eigen::Matrix3d const Finv   = Fprime.inverse();
        Eigen::Matrix3d const FinvT  = Finv.transpose();
        scalar_type const I1         = F2.trace();
        scalar_type const J          = Fprime.determinant();

        scalar_type const logJ = std::log(J);
        // psi(I1, J) = (mu/2)*(I1 - 3) - mu*log(J) + (lambda/2)*log^2(J)
        psi = static_cast<scalar_type>(0.5) * mu_ * (I1 - static_cast<scalar_type>(3.)) -
              mu_ * logJ + static_cast<scalar_type>(0.5) * lambda_ * logJ * logJ;
        // P(F) = mu*(F - mu*F^-T) + lambda*log(J)*F^-T
        Piola = mu_ * (Fprime - mu_ * FinvT) + lambda_ * logJ * FinvT;
    }
    else
    {
        Eigen::Matrix3d const F2    = F.transpose() * F;
        Eigen::Matrix3d const Finv  = F.inverse();
        Eigen::Matrix3d const FinvT = Finv.transpose();
        scalar_type const I1        = F2.trace();
        scalar_type const J         = F.determinant();

        scalar_type const logJ = std::log(J);
        // psi(I1, J) = (mu/2)*(I1 - 3) - mu*log(J) + (lambda/2)*log^2(J)
        psi = static_cast<scalar_type>(0.5) * mu_ * (I1 - static_cast<scalar_type>(3.)) -
              mu_ * logJ + static_cast<scalar_type>(0.5) * lambda_ * logJ * logJ;
        // P(F) = mu*(F - mu*F^-T) + lambda*log(J)*F^-T
        Piola = mu_ * (F - mu_ * FinvT) + lambda_ * logJ * FinvT;
    }

    // H is the negative gradient of the elastic potential
    scalar_type const V0     = std::abs(V0_);
    Eigen::Matrix3d const H  = -V0 * Piola * DmInv_.transpose();
    Eigen::Vector3d const f1 = H.col(0);
    Eigen::Vector3d const f2 = H.col(1);
    Eigen::Vector3d const f3 = H.col(2);
    Eigen::Vector3d const f4 = -(f1 + f2 + f3);

    // clang-format off
     auto const weighted_sum_of_gradients =
        w1 * f1.squaredNorm() +
        w2 * f2.squaredNorm() +
        w3 * f3.squaredNorm() +
        w4 * f4.squaredNorm();
    // clang-format on

    if (weighted_sum_of_gradients < epsilon)
        return;

    scalar_type const C           = V0 * psi;
    scalar_type const alpha_tilde = alpha_ / (dt * dt);
    scalar_type const delta_lagrange =
        -(C + alpha_tilde * lagrange) / (weighted_sum_of_gradients + alpha_tilde);

    lagrange += delta_lagrange;
    // because f = - grad(potential), then grad(potential) = -f and thus grad(C) = -f
    p.row(v1) += w1 * -f1 * delta_lagrange;
    p.row(v2) += w2 * -f2 * delta_lagrange;
    p.row(v3) += w3 * -f3 * delta_lagrange;
    p.row(v4) += w4 * -f4 * delta_lagrange;
}

neohookean_elasticity_constraint_t::scalar_type
neohookean_elasticity_constraint_t::signed_volume(positions_type const& V) const
{
    Eigen::RowVector3d const p1 = V.row(indices()[0]);
    Eigen::RowVector3d const p2 = V.row(indices()[1]);
    Eigen::RowVector3d const p3 = V.row(indices()[2]);
    Eigen::RowVector3d const p4 = V.row(indices()[3]);

    Eigen::Matrix3d Ds;
    Ds.col(0)      = (p1 - p4).transpose();
    Ds.col(1)      = (p2 - p4).transpose();
    Ds.col(2)      = (p3 - p4).transpose();
    auto const vol = (1. / 6.) * Ds.determinant();
    return vol;
}

} // namespace xpbd


#include "mesh_components.hpp"
#include "plane3d.hpp"
#include <vector>
#include <functional>
#include <array>
#include <iostream>
#include <Eigen/Dense>

using namespace Eigen;
using namespace std;

Tetrahedron::Tetrahedron(const int _id, const vector<reference_wrapper<Vertex3d>> _vertices, const double _weight, const int _label){
    id = _id;
    vertices = _vertices;
    weight = _weight;
    label = _label;
    //cout << vertices[0].get().Vec() << endl;

    Vector3d v0 = vertices[0].get().Vec();
    Vector3d v1 = vertices[1].get().Vec();
    Vector3d v2 = vertices[2].get().Vec();
    Vector3d v3 = vertices[3].get().Vec();

    //array<double, 3> faceCentroid = {(v0[0] + v1[0] + v2[0]) / 3, (v0[1] + v1[1] + v2[1]) / 3, (v0[2] + v1[2] + v2[2]) / 3};

    //Vector3f pt3 = vertices[3]->Vec();

    sphereCenter = { (v0[0] + v1[0] + v2[0] + v3[0])/4, (v0[1] + v1[1] + v2[1] + v3[1])/4, (v0[2] + v1[2] + v2[2] + v3[2])/4};

    double r0 = sqrt( pow(sphereCenter[0]-v0[0], 2) + pow(sphereCenter[1]-v0[1], 2) + pow(sphereCenter[2]-v0[2], 2));

    double r1 = sqrt( pow(sphereCenter[0]-v1[0], 2) + pow(sphereCenter[1]-v1[1], 2) + pow(sphereCenter[2]-v1[2], 2));

    double r2 = sqrt( pow(sphereCenter[0]-v2[0], 2) + pow(sphereCenter[1]-v2[1], 2) + pow(sphereCenter[2]-v2[2], 2));

    double r3 = sqrt( pow(sphereCenter[0]-v3[0], 2) + pow(sphereCenter[1]-v3[1], 2) + pow(sphereCenter[2]-v3[2], 2));

    double radii [4] = {r0, r1, r2, r3};

    double r = r0;

    for(int i=1; i<4; ++i){
	//float tmp_r = ;
	if (radii[i] > r){
	    r = radii[i];
	}
    }

    sphereRadius = r;
}

void Tetrahedron::addNeighbor(unsigned long int neighborId){
    neighbors.push_back(neighborId);
}

Face::Face(const vector<reference_wrapper<Vertex3d>> _vertices, unsigned long int _tetId){
    vertices = _vertices;
    tetId=_tetId;
}

bool Tetrahedron::contains(const array<double, 3> pt_target){
    if (sqrt( pow(pt_target[0] - sphereCenter[0], 2) + pow(pt_target[1] - sphereCenter[1], 2) + pow(pt_target[2] - sphereCenter[2], 2)) < sphereRadius){
        /*cout << pt_target[0] << endl;
        cout << pt_target[1] << endl;
        cout << pt_target[2] << endl;
        cout << "HERE" << endl;*/

	Vector3d pt0 = {vertices[0].get().Vec()};
	Vector3d pt1 = {vertices[1].get().Vec()};
	Vector3d pt2 = {vertices[2].get().Vec()};
	Vector3d pt3 = {vertices[3].get().Vec()};

	/*cout << "pt0" << endl;
	cout << pt0 << endl;
	cout << "pt1" << endl;
	cout << pt1 << endl;
	cout << "pt2" << endl;
	cout << pt2 << endl;
	cout << "pt3" << endl;
	cout << pt3 << endl;*/

	Matrix4d m0;
	Matrix4d m1;
	Matrix4d m2;
	Matrix4d m3;
	Matrix4d m4;

	m0 << pt0[0], pt0[1], pt0[2], 1,
	      pt1[0], pt1[1], pt1[2], 1,
	      pt2[0], pt2[1], pt2[2], 1,
	      pt3[0], pt3[1], pt3[2], 1;

	double d0 = m0.determinant();

	m1 << pt_target[0], pt_target[1], pt_target[2], 1,
	      pt1[0], pt1[1], pt1[2], 1,
	      pt2[0], pt2[1], pt2[2], 1,
	      pt3[0], pt3[1], pt3[2], 1;

	double d1 = m1.determinant();

	m2 << pt0[0], pt0[1], pt0[2], 1,
	      pt_target[0], pt_target[1], pt_target[2], 1,
	      pt2[0], pt2[1], pt2[2], 1,
	      pt3[0], pt3[1], pt3[2], 1;

	double d2 = m2.determinant();

	m3 << pt0[0], pt0[1], pt0[2], 1,
	      pt1[0], pt1[1], pt1[2], 1,
	      pt_target[0], pt_target[1], pt_target[2], 1,
	      pt3[0], pt3[1], pt3[2], 1;

	double d3 = m3.determinant();

	m4 << pt0[0], pt0[1], pt0[2], 1,
	      pt1[0], pt1[1], pt1[2], 1,
	      pt2[0], pt2[1], pt2[2], 1,
	      pt_target[0], pt_target[1], pt_target[2], 1;

	double d4 = m4.determinant();

	/*cout << d0 << endl;
	cout << d1 << endl;
	cout << d2 << endl;
	cout << d3 << endl;
	cout << d4 << endl;*/

	return (d0<0 && d1<0 && d2<0 && d3<0 && d4<0) || (d0>0 && d1>0 && d2>0 && d3>0 && d4>0);
    }
    return false;
}

vector<array<double, 3>> Tetrahedron::intersectsPlane(Plane3d plane){
    vector<array<double, 3>> intersectionPoints;

    for(int i=0; i<4; ++i){
	Vector3d v0 = vertices[i].get().Vec();
	if(plane.containsPoint(v0)){
	    array<double, 3> v0Pt = {v0[0], v0[1], v0[2]};
	    intersectionPoints.push_back(v0Pt);
	}
	else {
	    for(int j=i+1; j<4; ++j){
		Vector3d v1 = vertices[j].get().Vec();
		if(plane.intersectsEdge(v0, v1)){
		    intersectionPoints.push_back(plane.findIntersection(v0,v1));
		}
	    }
	}
    }

    if(intersectionPoints.size()==4){
	intersectionPoints = {intersectionPoints[0], intersectionPoints[1], intersectionPoints[3], intersectionPoints[2]};
    }

    return intersectionPoints;

}

bool Face::intersectsPlane(Plane3d plane){
    Vector3d v0 = vertices[0].get().Vec();
    Vector3d v1 = vertices[1].get().Vec();
    Vector3d v2 = vertices[2].get().Vec();

    return (plane.intersectsEdge(v0, v1) || plane.intersectsEdge(v1, v2) || plane.intersectsEdge(v0, v2));
}

unsigned long int Tetrahedron::Id(){
    return id;
}

vector<unsigned long int> Tetrahedron::Neighbors(){
    return neighbors;
}

vector<reference_wrapper<Vertex3d>> Tetrahedron::Vertices(){
    return vertices;
}

double Tetrahedron::Weight(){
    return weight;
}

int Tetrahedron::Label(){
    return label;
}

unsigned long int Face::TetId(){
    return tetId;
}

vector<reference_wrapper<Vertex3d>> Face::Vertices(){
    return vertices;
}

Vertex3d::Vertex3d(array<double, 3> _vec) {
    //vec = _vec;
    vec << _vec[0], _vec[1], _vec[2];
}

Vector3d Vertex3d::Vec(){
    return vec;
}


//  Copyright Matt Borland 2021.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
// Basic sanity check that header
// #includes all the files that it needs to.
#include <boost/math/differentiation/finite_difference.hpp>
//
// Note this header includes no other headers, this is
// important if this test is to be meaningful:
//
#include "test_compile_result.hpp"

void compile_and_link_test()
{
    check_result<float>(boost::math::differentiation::finite_difference_derivative([](float x){return x;}, f));
    check_result<double>(boost::math::differentiation::finite_difference_derivative([](double x){return x;}, d));
    #ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
    check_result<long double>(boost::math::differentiation::finite_difference_derivative([](long double x){return x;}, static_cast<long double>(0)));
    #endif

    check_result<float>(boost::math::differentiation::complex_step_derivative([](std::complex<float> x){return x;}, f));
    check_result<double>(boost::math::differentiation::complex_step_derivative([](std::complex<double> x){return x;}, d));
    #ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
    check_result<long double>(boost::math::differentiation::complex_step_derivative([](std::complex<long double> x){return x;},
                                                                                static_cast<long double>(0)));
    #endif
}


#include <iostream>

#include <opencv2/features2d/features2d.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/SVD>
#include <opencv2/core/core.hpp>
#include <opencv2/calib3d/calib3d.hpp>

#include "BA.h"
#include "Edge.h"
#include "match.h"
#include "pixel2cam.h"
#include "pose_estimate_3d3d.h"

using namespace std;
using namespace cv;

int main(int argc, char**argv)
{
    if(argc !=5)
    {
        cout<<"usage: pose_estimation_3d3d img1 img2 depth1 depth2! "<<endl;
        return  1;
    }

    Mat img_1 = imread( argv[1], CV_LOAD_IMAGE_COLOR);
    Mat img_2 = imread( argv[2], CV_LOAD_IMAGE_COLOR);

    vector<KeyPoint> keypoints_1, keypoints_2;
    vector<DMatch> matches;

    BA AAA;
    EdgeProjectXYZRGBD BBB;
    match CCC;
    pixel2cam DDD;
    pose_estimate_3d3d FFF;

    CCC.find_feature_matches(img_1,img_2,keypoints_1,keypoints_2,matches);
    cout<<"totally found: "<<matches.size()<<" pairs. "<<endl;

    //build 3D points
    Mat depth1 = imread ( argv[3], CV_LOAD_IMAGE_UNCHANGED);
    Mat depth2 = imread ( argv[4], CV_LOAD_IMAGE_UNCHANGED); //unsigned single channel

    Mat K = ( Mat_<double> (3,3) << 520.9, 0, 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
    vector<Point3f> pts1, pts2;

    for ( DMatch m:matches ) //DMatch:size matches=m
    {
        ushort d1 = depth1.ptr <unsigned short> ( int (keypoints_1[m.queryIdx].pt.y ) ) [int(keypoints_1[m.queryIdx].pt.x)];
        ushort d2 = depth1.ptr <unsigned short> ( int (keypoints_2[m.trainIdx].pt.y ) ) [int(keypoints_2[m.queryIdx].pt.x)];
        if (d1==0||d2==0)
            continue;
        Point2d p1 = DDD.trans ( keypoints_1[m.queryIdx].pt, K );
        Point2d p2 = DDD.trans ( keypoints_2[m.trainIdx].pt, K );
        float dd1 = float ( d1 ) /1000.0;
        float dd2 = float ( d2 ) /1000.0;
        pts1.push_back( Point3f ( p1.x*dd1, p1.y*dd1, dd1 ) );
        pts2.push_back( Point3f ( p2.x*dd2, p2.y*dd2, dd2 ) );
    }
    cout<<"3d-3d pairs: "<<pts1.size()<<endl;

    Mat R, t;
    FFF.pose_estimation_3d3d(pts1, pts2, R, t );
    cout<<"ICP via SVD results: "<<endl;
    cout<<"R = "<<R<<endl;
    cout<<"t = "<<t<<endl;
    cout<<"R_inv = "<<R.t()<<endl;
    cout<<"t_inv = "<<-R.t()<<endl;

    cout<<"calling bundle adjustment"<<endl;

//    AAA.bundleadjustment(pts1, pts2, R, t,);
}


#ifndef HEADER_CONTROL_POINT
#define HEADER_CONTROL_POINT
#include <armadillo>
#include <memory>

#include <set>

class Element;
class ControlPoint;
template <class PointType> class ShapeModel;

class ControlPoint {

public:

	ControlPoint(ShapeModel<ControlPoint> * owning_shape = nullptr);

	/**
	Getter to the vertex's coordinates
	@return coordinates vertex coordinates
	*/
	const arma::vec::fixed<3> & get_point_coordinates() const;

	/**
	Setter to the vertex's coordinates
	@param coordinates vertex coordinates
	*/
	void set_point_coordinates(arma::vec::fixed<3> coordinates);

	/**
	Adds $facet to the vector of std::shared_ptr<Element>  that own this vertex.
	Nothing happens if the facet is already listed
	@param facet index to the element owning this vertex
	*/
	void add_ownership(int el_index);

	arma::vec::fixed<3> get_normal_coordinates(bool bezier) const;

	std::set< int >  common_elements(int control_point_index) const;

	/**
	Determines if $this is owned by $facet
	@param facet Facet whose relationship with the facet is to be tested
	@return true is $this is owned by $facet, false otherwise
	*/
	bool is_owned_by( int el_index) const;

	/**
	Delete $facet from the list of Element * owning $this
	Nothing happens if the facet was not listed (maybe throw a warning)>
	@param facet Pointer to the facet owning this vertex
	*/
	void remove_ownership(int el_index);

	/**
	Removes all ownership relationships
	*/
	void reset_ownership();

	/**
	Returns the owning elements
	@return Owning elements
	*/
	std::set< int  > get_owning_elements() const;

	/**
	Sets the owning elements
	@param Owning elements
	*/
	void set_owning_elements(std::set< int  > & owning_elements);

	/**
	Returns point covariance
	@return point covariance
	*/
	arma::mat get_covariance() const;

	/**
	@param element owning element
	@param local_indices triplet of indices numbering this control point within the owning element
	*/
	void add_local_numbering(int element,const arma::uvec & local_indices);

	/**
	Returns the local numbering of this control point within the specified element
	@param element pointer to element to consider
	*/
	arma::uvec get_local_numbering(int element) const;

	/**
	Sets the control point covariance
	@param P covariance
	*/
	void set_covariance(arma::mat P);

	void set_deviation(arma::vec d) { this -> deviation = d;}
	arma::vec get_deviation() const { return this -> deviation; }

	/**
	Returns the number of facets owning this vertex
	@return N number of owning of facets
	*/
	unsigned int get_number_of_owning_elements() const ;

	/**
	Get global index (shape wise)
	@return global index
	*/
	int get_global_index() const;

	/**
	Set global index (shape wise)
	@param global index
	*/
	void set_global_index(int index);

protected:
	arma::vec::fixed<3> coordinates;
	arma::vec mean_coordinates;

	std::set<int> owning_elements;
	std::map<int,arma::uvec> local_numbering;
	arma::mat covariance = arma::zeros<arma::mat>(3,3);
	arma::vec deviation = arma::zeros<arma::vec>(3);

	int global_index;
	ShapeModel<ControlPoint> * owning_shape;

};

#endif


//==================================================================================================
/*!
    @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_SIMD_FUNCTION_RSQRT_HPP_INCLUDED
#define BOOST_SIMD_ARCH_X86_SSE1_SIMD_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_else.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
                          , bs::pack_<bd::single_<A0>, bs::sse_>
                          )
  {
    BOOST_FORCEINLINE A0 operator() (raw_tag const&
                                    , const A0 & a0) const BOOST_NOEXCEPT
    {
      return _mm_rsqrt_ps(a0);
    }
  };

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

  BOOST_DISPATCH_OVERLOAD ( rsqrt_
                          , (typename A0)
                          , bs::sse1_
                          , bs::pedantic_tag
                          , bs::pack_<bd::single_<A0>, bs::sse_>
                          )
  {
    BOOST_FORCEINLINE A0 operator() (pedantic_tag const&
                                    ,const A0 & a00) const BOOST_NOEXCEPT
    {
      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)));
      #ifndef BOOST_SIMD_NO_DENORMALS
      a0 *= if_else(is_den, Denormalsqrtfactor<A0>(), One<A0>());
      #endif

      #ifndef BOOST_SIMD_NO_INFINITIES
      a0 = if_zero_else(a00 == Inf<A0>(),a0);
      #endif
      return if_else(is_eqz(a00), Inf<A0>(), a0);
    }
  };

} } }

#endif


//
// Copyright 2012 Chung-Lin Wen
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
#ifndef BOOST_GIL_EXTENSION_TOOLBOX_COLOR_SPACES_LAB_HPP
#define BOOST_GIL_EXTENSION_TOOLBOX_COLOR_SPACES_LAB_HPP

#include <boost/gil/extension/toolbox/color_spaces/xyz.hpp>

#include <boost/gil/color_convert.hpp>
#include <boost/gil.hpp> // FIXME: Include what you use, not everything, even in extensions!

#include <boost/mpl/vector.hpp>

namespace boost{ namespace gil {

/// \addtogroup ColorNameModel
/// \{
namespace lab_color_space
{
/// \brief Luminance
struct luminance_t {};
/// \brief a Color Component
struct a_color_opponent_t {};
/// \brief b Color Component
struct b_color_opponent_t {};
}
/// \}

/// \ingroup ColorSpaceModel
using lab_t = mpl::vector3
    <
        lab_color_space::luminance_t,
        lab_color_space::a_color_opponent_t,
        lab_color_space::b_color_opponent_t
    >;

/// \ingroup LayoutModel
using lab_layout_t = layout<lab_t>;

GIL_DEFINE_ALL_TYPEDEFS(32f, float32_t, lab)

/// \ingroup ColorConvert
/// \brief LAB to XYZ
template <>
struct default_color_converter_impl< lab_t, xyz_t >
{
    template <typename P1, typename P2>
    void operator()( const P1& src, P2& dst ) const
    {
        using namespace lab_color_space;
        using namespace xyz_color_space;

        float32_t p = ((get_color(src, luminance_t()) + 16.f)/116.f);

        get_color(dst, y_t()) =
                1.f * powf(p, 3.f);

        get_color(dst, x_t()) =
                0.95047f * powf((p +
                                 (get_color(src, a_color_opponent_t())/500.f)
                                 ), 3.f);
        get_color(dst, z_t()) =
                1.08883f * powf((p -
                                 (get_color(src, b_color_opponent_t())/200.f)
                                 ), 3.f);
    }
};

/// \ingroup ColorConvert
/// \brief XYZ to LAB
/// \note I assume \c xyz_t
template <>
struct default_color_converter_impl< xyz_t, lab_t >
{
private:
    /// \ref http://www.brucelindbloom.com/index.html?Eqn_XYZ_to_Lab.html
    BOOST_FORCEINLINE
    float32_t forward_companding(float32_t value) const
    {
        if (value > 216.f/24389.f)
        {
            return powf(value, 1.f/3.f);
        }
        else
        {
            return ((24389.f/27.f * value + 16.f)/116.f);
        }
    }

public:
    template <typename P1, typename P2>
    void operator()( const P1& src, P2& dst ) const
    {
        using namespace lab_color_space;

        float32_t f_y =
                forward_companding(
                    channel_convert<float32_t>(
                        get_color(src, xyz_color_space::y_t())
                        )
                    // / 1.f
                    );

        float32_t f_x =
                forward_companding(
                    channel_convert<float32_t>(
                        get_color(src, xyz_color_space::x_t())
                        )
                    * (1.f / 0.95047f)  // if the compiler is smart, it should
                                        // precalculate this, no?
                    );

        float32_t f_z =
                forward_companding(
                    channel_convert<float32_t>(
                        get_color(src, xyz_color_space::z_t())
                        )
                    * (1.f / 1.08883f)  // if the compiler is smart, it should
                                        // precalculate this, no?
                    );

        get_color(dst, luminance_t()) =
                116.f * f_y - 16.f;

        get_color(dst, a_color_opponent_t()) =
                500.f * (f_x - f_y);

        get_color(dst, b_color_opponent_t()) =
                200.f * (f_y - f_z);
    }
};

/// \ingroup ColorConvert
/// \brief RGB to LAB
template <>
struct default_color_converter_impl< rgb_t, lab_t >
{
    template <typename P1, typename P2>
    void operator()( const P1& src, P2& dst ) const
    {
        using namespace lab_color_space;

        xyz32f_pixel_t xyz32f_temp_pixel;
        default_color_converter_impl<rgb_t, xyz_t>()(src, xyz32f_temp_pixel);
        default_color_converter_impl<xyz_t, lab_t>()(xyz32f_temp_pixel, dst);
    }
};

/// \ingroup ColorConvert
/// \brief LAB to RGB
template <>
struct default_color_converter_impl<lab_t,rgb_t>
{
    template <typename P1, typename P2>
    void operator()( const P1& src, P2& dst) const
    {
        using namespace lab_color_space;

        xyz32f_pixel_t xyz32f_temp_pixel;
        default_color_converter_impl<lab_t, xyz_t>()(src, xyz32f_temp_pixel);
        default_color_converter_impl<xyz_t, rgb_t>()(xyz32f_temp_pixel, dst);
    }
};

} // namespace gil
} // namespace boost

#endif


/*!
 * Copyright (C) tkornuta, IBM Corporation 2015-2019
 *
 * 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.
 */
/*!
 * @file mnist_conv_hebbian.cpp
 * @brief Program for visualization of hebbian formed filters trained on MNIST digits.
 * @Author: Alexis Asseman <alexis.asseman@ibm.com>, Tomasz Kornuta <tkornut@us.ibm.com>
 * @Date:   June 8, 2017
 *
 * Copyright (c) 2017, Alexis Asseman, Tomasz Kornuta, IBM Corporation. All rights reserved.
 *
 */

#include <boost/thread/thread.hpp>
#include <boost/bind.hpp>

#include <data_io/MNISTMatrixImporter.hpp>

#include <logger/Log.hpp>
#include <logger/ConsoleOutput.hpp>
using namespace mic::logger;

#include <application/ApplicationState.hpp>

#include <configuration/ParameterServer.hpp>

#include <opengl/visualization/WindowManager.hpp>
#include <opengl/visualization/WindowGrayscaleBatch.hpp>
#include <opengl/visualization/WindowCollectorChart.hpp>

using namespace mic::opengl::visualization;

// Hebbian neural net.
#include <mlnn/HebbianNeuralNetwork.hpp>
using namespace mic::mlnn;

// Encoders.
#include <encoders/ColMatrixEncoder.hpp>
#include <encoders/UIntMatrixEncoder.hpp>

#include <mlnn/experimental/ConvHebbian.hpp>
using namespace mic::mlnn::experimental;

/// Window for displaying the MNIST batch.
WindowGrayscaleBatch<double>* w_input;
/// Window for displaying the weights.
WindowGrayscaleBatch<double>* w_weights1;

WindowGrayscaleBatch<double>* w_output;
WindowGrayscaleBatch<double>* w_reconstruction;
WindowGrayscaleBatch<double>* w_similarity;

/// Data collector.
WindowCollectorChart<double>* w_chart;
mic::data_io::DataCollectorPtr<std::string, double> collector_ptr;

/// MNIST importer.
mic::data_io::MNISTMatrixImporter<double>* importer;
/// Multi-layer neural network.
HebbianNeuralNetwork<double> neural_net;

/// MNIST matrix encoder.
mic::encoders::ColMatrixEncoder<double>* mnist_encoder;
/// Label 2 matrix encoder (1 hot).
//mic::encoders::UIntMatrixXfEncoder* label_encoder;

const size_t patch_size = 28;
const size_t batch_size = 1;
const size_t input_channels = 1;
const size_t filter_size[] = {5};
const size_t filters[] = {16};
const size_t stride[] = {1};

/*!
 * \brief Function for batch sampling.
 * \author tkornuta
 */
void batch_function (void) {

/*	if (neural_net.load(fileName)) {
        LOG(LINFO) << "Loaded neural network from a file";
    } else {*/
        {
        // Create a simple hebbian network.
        neural_net.pushLayer(new ConvHebbian<double>(patch_size, patch_size, input_channels, filters[0], filter_size[0], stride[0]));

        LOG(LINFO) << "Generated new neural network";
    }//: else

    std::shared_ptr<mic::mlnn::experimental::ConvHebbian<double> > layer1 =
            neural_net.getLayer<mic::mlnn::experimental::ConvHebbian<double> >(0);

    size_t iteration = 0;
    // Set training parameters.
    const double learning_rate = 5e-3;

    // Main application loop.
    while (!APP_STATE->Quit()) {

        // If not paused.
        if (!APP_STATE->isPaused()) {

            // If single step mode - pause after the step.
            if (APP_STATE->isSingleStepModeOn())
                APP_STATE->pressPause();

            { // Enter critical section - with the use of scoped lock from AppState!
                APP_DATA_SYNCHRONIZATION_SCOPED_LOCK();

                // Retrieve the next minibatch.
                mic::types::MNISTBatch<double> bt = importer->getRandomBatch();

                // Encode data.
                mic::types::MatrixPtr<double> encoded_batch = mnist_encoder->encodeBatch(bt.data());

                MNISTBatch<double> next_batch = importer->getNextBatch();
                encoded_batch  = mnist_encoder->encodeBatch(next_batch.data());

                neural_net.train(encoded_batch, learning_rate);

                if (iteration % 10 == 0) {
                    //Visualize the weights.
                    // Set batch to be displayed.
                    w_input->setBatchUnsynchronized(layer1->getInputActivations());
                    w_weights1->setBatchUnsynchronized(layer1->getWeightActivations());
                    w_similarity->setBatchUnsynchronized(layer1->getWeightSimilarity(true));
                    w_output->setBatchUnsynchronized(layer1->getOutputActivations());
                    w_reconstruction->setBatchUnsynchronized(layer1->getOutputReconstruction());
                    collector_ptr->addDataToContainer("Reconstruction error", layer1->getOutputReconstructionError());
                    LOG(LINFO) << "Iteration: " << iteration;
                }//: if

                iteration++;
            }//: end of critical section

        }//: if

        // Sleep.
        APP_SLEEP();
    }//: while

}//: image_encoder_and_visualization_test

/*!
 * \brief Main program function. Runs two threads: main (for GLUT) and another one (for data processing).
 * \author tkornuta
 * @param[in] argc Number of parameters (passed to glManaged).
 * @param[in] argv List of parameters (passed to glManaged).
 * @return (not used)
 */
int main(int argc, char* argv[]) {
    // Set console output to logger.
    LOGGER->addOutput(new ConsoleOutput());
    LOG(LINFO) << "Logger initialized. Starting application";

    // Parse parameters.
    PARAM_SERVER->parseApplicationParameters(argc, argv);

    // Initilize application state ("touch it") ;)
    APP_STATE;

    // Load dataset.
    importer = new mic::data_io::MNISTMatrixImporter<double>();
    importer->setDataFilename("../data/mnist/train-images.idx3-ubyte");
    importer->setLabelsFilename("../data/mnist/train-labels.idx1-ubyte");
    importer->setBatchSize(batch_size);

    // Initialize the encoders.
    mnist_encoder = new mic::encoders::ColMatrixEncoder<double>(patch_size, patch_size);
    //label_encoder = new mic::encoders::UIntMatrixXfEncoder(batch_size);

    // Set parameters of all property-tree derived objects - USER independent part.
    PARAM_SERVER->loadPropertiesFromConfiguration();

    // Initialize property-dependent variables of all registered property-tree objects - USER dependent part.
    PARAM_SERVER->initializePropertyDependentVariables();

    // Import data from datasets.
    if (!importer->importData())
        return -1;

    // Initialize GLUT! :]
    VGL_MANAGER->initializeGLUT(argc, argv);

    // Create batch visualization window.
    w_input = new WindowGrayscaleBatch<double>("Input batch", Grayscale::Norm_HotCold, Grayscale::Grid_Both, 70, 0, 250, 250);
    w_weights1 = new WindowGrayscaleBatch<double>("Permanences", Grayscale::Norm_HotCold, Grayscale::Grid_Both, 70+250, 0, 250, 250);
    w_similarity = new WindowGrayscaleBatch<double>("Cosine similarity matrix", Grayscale::Norm_HotCold, Grayscale::Grid_Both, 70+(2*250), 0, 250, 250);
    w_output = new WindowGrayscaleBatch<double>("Output", Grayscale::Norm_HotCold, Grayscale::Grid_Both, 70+(3*250), 0, 250, 250);
    w_reconstruction = new WindowGrayscaleBatch<double>("Reconstruction", Grayscale::Norm_HotCold, Grayscale::Grid_Both, 70+(4*250), 0, 250, 250);

    // Chart.
    w_chart = new WindowCollectorChart<double>("Statistics", 60, 878, 512, 256);
    collector_ptr= std::make_shared < mic::data_io::DataCollector<std::string, double> >( );
    w_chart->setDataCollectorPtr(collector_ptr);

    // Create data containers.
    collector_ptr->createContainer("Reconstruction error", mic::types::color_rgba(255, 255, 255, 180));

    boost::thread batch_thread(boost::bind(&batch_function));

    // Start visualization thread.
    VGL_MANAGER->startVisualizationLoop();

    LOG(LINFO) << "Waiting for threads to join...";
    // End test thread.
    batch_thread.join();
    LOG(LINFO) << "Threads joined - ending application";
}//: main


#include "../accumulate_over_others.hxx"

#include <boost/math/special_functions/expint.hpp>

#include <vector>

Boost_Float integral(const Boost_Float &prefactor, const Boost_Float &b,
                     const Boost_Float &x, const int64_t &k);

void precompute(
  const Boost_Float &base, std::vector<Boost_Float> &sorted_poles,
  std::vector<std::pair<std::vector<Boost_Float>::const_iterator,
                        std::vector<Boost_Float>::const_iterator>>
    &equal_ranges,
  std::vector<int64_t> &lengths, std::vector<Boost_Float> &products,
  std::vector<std::vector<Boost_Float>> &integral_matrix)
{
  sorted_poles.erase(
    std::remove_if(sorted_poles.begin(), sorted_poles.end(),
                   [](const Boost_Float &a) { return a >= 0; }),
    sorted_poles.end());
  std::sort(sorted_poles.begin(), sorted_poles.end());

  for(auto pole(sorted_poles.begin()); pole != sorted_poles.end();)
    {
      const Boost_Float &p(*pole);
      equal_ranges.push_back(
        std::equal_range(pole, sorted_poles.end(), p,
                         [&](const Boost_Float &p, const Boost_Float &q) {
                           return (p < q) && !(abs(p - q) < 1e-2);
                         }));
      auto &equal_range(equal_ranges.back());
      lengths.push_back(std::distance(equal_range.first, equal_range.second));
      int64_t l(lengths.back());

      products.push_back(
        1
        / accumulate_over_others(
            sorted_poles, equal_range, Boost_Float(1),
            [&](const Boost_Float &product, const Boost_Float &q) {
              return product * (p - q);
            }));

      Boost_Float integral_sum(0);
      Boost_Float integral_prefactor(-boost::math::expint(-p * log(base))
                                     * pow(base, p));

      integral_matrix.emplace_back();
      auto &integrals(integral_matrix.back());
      for(int64_t k = 0; k < l; ++k)
        {
          integrals.push_back(
            integral(integral_prefactor, base, p, l - k - 1));
        }

      std::advance(pole, l);
    }
}


//
// Created by Xinyu Zhang on 3/26/21.
//
// gcc -I/Users/xinyuzhang/Desktop/Spring2021/c++pattern/project/cosan /Users/xinyuzhang/Desktop/Spring2021/c++pattern/project/cosan/test/model/RidgeRegressionTest.cpp
#include <iostream>
// #include <Eigen/Dense>
// #include <cosan/io/utils.h>
#include <cosan/data/CosanData.h>
#include <cosan/model/CosanLinearRegression.h>
#include <cosan/model/CosanRidgeRegression.h>
//using namespace Eigen;
//using namespace std;
typedef double db;
int main() {
   Cosan::CosanRawData<db> CD("./example_data/toy/X.csv","./example_data/toy/y.csv");
   db RegularizationTerm = 1;
   Cosan::CosanRidgeRegression<db> CRRwBias(RegularizationTerm,true);
   CRRwBias.fit(CD.GetInput(),CD.GetTarget());
   std::cout<<CRRwBias.GetBeta()<<std::endl;
   std::cout<<(CRRwBias.predict(CD.GetInput())-CD.GetTarget()).norm()<<std::endl;
   return 0;
}


#include <iostream>
#include <fstream>
#include <stdio.h>
#include <math.h>
#include <omp.h>
#include <chrono>

#include <random>
#include <map>
#include <string>
#include <iomanip>

#include <unistd.h>
#include <string>
#include <algorithm>
#include <random>

#include <Eigen/Core>

#include <boost/program_options.hpp>
#include <iterator>

#include "BayesC_distributions.h"
#include "Sampling_functions.hpp"

using namespace std;
using namespace Eigen;
namespace po = boost::program_options;

void ReadFromFile(std::vector<double> &x, const std::string &file_name)
{
	std::ifstream read_file(file_name);
	assert(read_file.is_open());

	std::copy(std::istream_iterator<double>(read_file), std::istream_iterator<double>(),
			std::back_inserter(x));

	read_file.close();
}

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

	po::options_description desc("Options");
	desc.add_options()
		("M", po::value<int>()->required(), "No. of simulated markers")
		("N", po::value<int>()->required(), "No. of simulated individuals")
		("iter", po::value<int>()->default_value(5000), "No. of Gibbs iterations")
		("pNZ", po::value<double>()->default_value(0.5), "Proportion nonzero (simulations)")
		("h2", po::value<double>()->default_value(0.6), "Heritability (simulations)")
		("input", po::value<std::string>()->default_value("none"),"Input filename")
		("out", po::value<std::string>()->default_value("BayesC_out"),"Output filename")
	;

	//clock starts
	std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::now();

	//map variables
	po::variables_map vm;
	po::store(po::parse_command_line(argc,argv,desc),vm);
	po::notify(vm);

	int M=vm["M"].as<int>();
	int N=vm["N"].as<int>();
	int iter=vm["iter"].as<int>();

	string input=vm["input"].as<string>();
	string output=vm["out"].as<string>();

	MatrixXd X(N,M);
	VectorXd Y(N);

	//beta coefficients
	VectorXd beta_true(M);
	beta_true.setZero();

	int i,j,k,l,m=0;

	//Was an input matrix given?

	if (input!="none"){ //Either read input tables for X and Y
		ifstream f1(input+".X");
		//f1 >> m >> n;
		for (int i = 0; i < N; i++)
		{
			for (int j = 0; j < M; j++)
			{
				f1 >> X(i,j);
				//cout<<X(i,j)<<endl;
			}
		}
		f1.close();
		cout<<"finished reading matrix X!"<<endl;

		std::vector<double> Y_in;
		ReadFromFile(Y_in, input+".Y");
		double* ptr_Y = &Y_in[0];
		Eigen::Map<Eigen::VectorXd> Y1(ptr_Y, Y_in.size());
		cout<<"finished reading vector Y!"<<endl;
		if(Y_in.size()!=N){cout<<"input Y vector size doesnt much the size indicated in the command line"<<endl;return 0;}
		Y=Y1;
		Y_in.clear();

	}else //or simulate
	{
		double h2=vm["h2"].as<double>();
		double pNZ=vm["pNZ"].as<double>();
		double sigmaY_true=1;

		int MT=pNZ*M;

		//Fill Genotype matrix
		for (i=0;i<N;i++){
			for (j=0;j<M;j++){
				X(i,j)=rnorm(0,1);
			}
		}
		for (i=0;i<MT;i++){
			beta_true[i]=rnorm(0,sqrt(h2/MT));
		}

		//error
		VectorXd error(N);
		for (i=0;i<N;i++){
			error[i]=rnorm(0,sqrt(1-h2));
		}

		//construct phenotypes
		Y=X*beta_true;
		Y+=error;
	}
	//standardize matrix X
	RowVectorXd mean = X.colwise().mean();
	RowVectorXd sd = ((X.rowwise() - mean).array().square().colwise().sum() / (X.rows() - 1)).sqrt();
	X = (X.rowwise() - mean).array().rowwise() / sd.array();

	//standardize vector Y
    Y = (Y.array() - Y.array().mean());
    Y /= sqrt(Y.squaredNorm() / (double(N - 1)));

    //Initialize variables
	double Emu=0;
	VectorXd vEmu(N);
	vEmu.setOnes();

	VectorXd Ebeta(M);
	Ebeta.setZero();
	VectorXd ny(M);
	ny.setZero();
	double Ew=0.5;
	//residual error
	VectorXd epsilon(N);

	epsilon=Y-X*Ebeta-vEmu*Emu;

	std::vector<int> markerI;
	for (int i=0; i<M; ++i) {
		markerI.push_back(i);
	}
	int marker=0;

	int NZ=0;

	double Esigma2=epsilon.squaredNorm()/(N*0.5);
	double Epsi2=rbeta(1,1);

	//Standard parameterization of hyperpriors for variances
	//double v0E=0.001,s0E=0.001,v0B=0.001,s0B=0.001;

	// Alternative parameterization of hyperpriors for variances
	double v0E=4,v0B=4;
	double s0B=((v0B-2)/v0B)*Epsi2;
	double s0E=((v0E-2)/v0E)*Esigma2;

	//pre-computed elements for calculations
	VectorXd el1(M);
	for (int i=0; i<M; ++i) {
		el1[i]=X.col(i).transpose()*X.col(i);
	}

	//open files for writing
	std::ofstream ofs;
	ofs.open(output+"_estimates.txt");
	for (int i=0; i<M; ++i) {
		ofs << "beta_" <<i<< ' ';
	}
	for (int i=0; i<M; ++i) {
		ofs << "incl_" <<i<< ' ';
	}
	ofs << "Ew" << " ";
	ofs << "Epsi2" << " ";
	ofs << "Esigma2" << " ";
	ofs << "\n";
	ofs.close();

	std::chrono::steady_clock::time_point end1= std::chrono::steady_clock::now();
	std::cout << "Time taken for Reading/generating data = " << std::chrono::duration_cast<std::chrono::nanoseconds> (end1 - begin).count()*1e-9 <<" seconds"<<std::endl;

	//begin GIBBS sampling iterations

	ofs.open (output+"_estimates.txt", std::ios_base::app);
	for (i=0;i<iter;i++){

		Emu=sample_mu(N,Esigma2,Y,X,Ebeta);

		//sample effects and probabilities jointly
		std::random_shuffle(markerI.begin(), markerI.end());

		for (j=0;j<M;j++){
			marker=markerI[j];

			epsilon=epsilon+X.col(marker)*Ebeta[marker];

			double Cj=el1[marker]+Esigma2/Epsi2;
			double rj=X.col(marker).transpose()*epsilon;

			double ratio=(((exp(-(pow(rj,2))/(2*Cj*Esigma2))*sqrt((Epsi2*Cj)/Esigma2))));
			ratio=Ew/(Ew+ratio*(1-Ew));
			ny[marker]=rbernoulli(ratio);

			if (ny[marker]==0){
				Ebeta[marker]=0;
			}
			else if (ny[marker]==1){
				Ebeta[marker]=rnorm(rj/Cj,Esigma2/Cj);
			}

			epsilon=epsilon-X.col(marker)*Ebeta[marker];

		}
		for (j=0;j<M;j++){
			ofs << Ebeta[j] << " ";
		}
		for (j=0;j<M;j++){
			ofs << ny[j] << " ";
		}
		NZ=ny.sum();
		//cout<<NZ<<endl;

		Ew=sample_w(M,NZ);
		epsilon=Y-X*Ebeta-vEmu*Emu;

		Epsi2=sample_psi2_chisq(Ebeta,NZ,v0B,s0B);
		Esigma2=sample_sigma_chisq(N,epsilon,v0E,s0E);

		ofs << Ew << " ";
		ofs << Epsi2 << " ";
		ofs << Esigma2 << " ";
		ofs << "\n";

	}
	ofs.close();
//write out simulated data
if (input=="none"){
	//write to files
	ofstream myfile1;
	myfile1.open (output+"_simulated_Y.txt");
	for (i=0;i<N;i++){
		myfile1 << Y[i] << ' ';
	}
	myfile1 << endl;
	myfile1.close();
/*
	ofstream myfile2;
	myfile2.open (output+"_simulated_X.txt");
	for (i=0;i<N;i++){
		for (j=0;j<M;j++){
			myfile2<<X(i,j)<< ' ';
		}
		myfile2<<endl;
	}
	myfile2.close();
*/
	ofstream myfile3;
	myfile3.open (output+"_simulated_betatrue.txt");
	myfile3 << beta_true << ' ';
	myfile3.close();
}

std::chrono::steady_clock::time_point end2= std::chrono::steady_clock::now();
std::cout << "Time taken for full analysis = " << std::chrono::duration_cast<std::chrono::nanoseconds> (end2 - begin).count()*1e-9 <<" seconds"<<std::endl;

	return 0;
}