//=====================================================
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
//=====================================================
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 2
// of the License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
//
#ifndef MTL4_INTERFACE_HH
#define MTL4_INTERFACE_HH

#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/mtl/operation/cholesky.hpp>
#include <vector>

using namespace mtl;

template<class real>
class mtl4_interface {

public :

  typedef real real_type ;

  typedef std::vector<real>  stl_vector;
  typedef std::vector<stl_vector > stl_matrix;

  typedef mtl::dense2D<real, mtl::matrix::parameters<mtl::tag::col_major> > gene_matrix;
  typedef mtl::dense_vector<real>  gene_vector;

  static inline std::string name() { return "mtl4"; }

  static void free_matrix(gene_matrix & A, int N){
    return ;
  }

  static void free_vector(gene_vector & B){
    return ;
  }

  static inline void matrix_from_stl(gene_matrix & A, stl_matrix & A_stl){
    A.change_dim(A_stl[0].size(), A_stl.size());

    for (int j=0; j<A_stl.size() ; j++){
      for (int i=0; i<A_stl[j].size() ; i++){
        A(i,j) = A_stl[j][i];
      }
    }
  }

  static inline void vector_from_stl(gene_vector & B, stl_vector & B_stl){
    B.change_dim(B_stl.size());
    for (int i=0; i<B_stl.size() ; i++){
      B[i] = B_stl[i];
    }
  }

  static inline void vector_to_stl(gene_vector & B, stl_vector & B_stl){
    for (int i=0; i<B_stl.size() ; i++){
      B_stl[i] = B[i];
    }
  }

  static inline void matrix_to_stl(gene_matrix & A, stl_matrix & A_stl){
    int N=A_stl.size();
    for (int j=0;j<N;j++){
      A_stl[j].resize(N);
      for (int i=0;i<N;i++){
        A_stl[j][i] = A(i,j);
      }
    }
  }

  static inline void matrix_matrix_product(const gene_matrix & A, const gene_matrix & B, gene_matrix & X, int N){
    X = (A*B);
//     morton_dense<double, doppled_64_row_mask> C(N,N);
//     C = B;
//     X = (A*C);
  }

  static inline void transposed_matrix_matrix_product(const gene_matrix & A, const gene_matrix & B, gene_matrix & X, int N){
    X = (trans(A)*trans(B));
  }

  static inline void ata_product(const gene_matrix & A, gene_matrix & X, int N){
    X = (trans(A)*A);
  }

  static inline void aat_product(const gene_matrix & A, gene_matrix & X, int N){
    X = (A*trans(A));
  }

  static inline void matrix_vector_product(gene_matrix & A, gene_vector & B, gene_vector & X, int N){
    X = (A*B);
  }

  static inline void atv_product(gene_matrix & A, gene_vector & B, gene_vector & X, int N){
    X = (trans(A)*B);
  }

  static inline void axpy(const real coef, const gene_vector & X, gene_vector & Y, int N){
    Y += coef * X;
  }

  static inline void axpby(real a, const gene_vector & X, real b, gene_vector & Y, int N){
    Y = a*X + b*Y;
  }

  static inline void cholesky(const gene_matrix & X, gene_matrix & C, int N){
    C = X;
    recursive_cholesky(C);
  }

//   static inline void lu_decomp(const gene_matrix & X, gene_matrix & R, int N){
//     R = X;
//     std::vector<int> ipvt(N);
//     lu_factor(R, ipvt);
//   }

  static inline void trisolve_lower(const gene_matrix & L, const gene_vector& B, gene_vector & X, int N){
    X = lower_trisolve(L, B);
  }

  static inline void copy_matrix(const gene_matrix & source, gene_matrix & cible, int N){
    cible = source;
  }

  static inline void copy_vector(const gene_vector & source, gene_vector & cible, int N){
    cible = source;
  }

};

#endif


/*
 * Random.cpp
 *
 *  Created on: Feb 22, 2009
 *      Author: Tim Babb
 */

#include <boost/static_assert.hpp>
#include <assert.h>
#include <algorithm>
#include <limits>

#include <geomc/random/Random.h>

#define INT_SIGNBIT       (1 << std::numeric_limits<int>::digits)
#define LONG_SIGNBIT      (1L << std::numeric_limits<long>::digits)
#define LONG_LONG_SIGNBIT (1LL << std::numeric_limits<long long>::digits)

using namespace std;
using namespace geom;

typedef union fpbox {
    uint32_t i;
    float f;
    uint64_t l;
    double d;
} FPBox;

/**********************************
 * Structors                      *
 **********************************/

Random::Random():_bitpool(0),
                 _bitsleft(0) {}

Random::~Random() {}

/**********************************
 * Elementary Specializations     *
 **********************************/

namespace geom {

template <> bool Random::rand<bool>() {
    if (_bitsleft == 0) {
        _bitpool = this->rand32();
        _bitsleft = 32;
    }
    int bit = _bitpool & 1;
    _bitpool = _bitpool >> 1;
    _bitsleft--;
    return bit;
}

template <> unsigned int Random::rand<unsigned int>() {
    unsigned int bits = 0;
    const int chunks = (std::numeric_limits<unsigned int>::digits + 32 - 1) / 32; //ceiling(x/y) = (x + y - 1) / y
#if UINT_MAX > (1LL << 32) - 1
    for (int i = 0; i < chunks; i++) {
        bits = (bits << 32) | rand32();
    }
#else
    bits =(unsigned int)rand32();
#endif
    return bits;
}

template <> int Random::rand<int>() {
    return rand<unsigned int>(); //MSB is now a sign bit; covers the entire range of <int> type. might we want to change this?
}

template <> unsigned long Random::rand<unsigned long>() {
    unsigned long bits = 0;
    const int chunks = (std::numeric_limits<unsigned long>::digits + 32 - 1) / 32; //ceiling(x/y) = (x + y - 1) / y
#if ULONG_MAX > (1LL << 32) - 1
    for (int i = 0; i < chunks; i++) {
        bits = (bits << 32) | rand32();
    }
#else
    bits = (unsigned long)rand32();
#endif
    return bits;
}

template <> long Random::rand<long>() {
    return rand<unsigned long>();
}

template <> unsigned long long Random::rand<unsigned long long>() {
    unsigned long long bits = 0;
    const int chunks = (std::numeric_limits<unsigned long long>::digits + 32 - 1) / 32; //ceiling(x/y) = (x + y - 1) / y
    for (int i = 0; i < chunks; i++) {
        bits = (bits << 32) | rand32();
    }
    return bits;
}

template <> long long Random::rand<long long>() {
    return rand<unsigned long long>();
}

/* From the unpublished paper by Allen B. Downey.
 *
 * The conventional method of dividing a random number
 * by a constant excludes about 93% of the representable
 * floating point values between 0.0 and 1.0. Instead
 * we pick the bits of our number explicitly, but must
 * be careful to do so such that the distribution is
 * uniform. This method is fast, and makes efficient
 * use of our pseudorandom bits.
 *
 * The gist: Pick the mantissa, then pick the exponent
 * by iterative coin-flipping.
 */

//TODO: use numeric_limits instead of hard-coded masks
//see: boost::integer's int_t<bits>

template <> float Random::rand<float>() {
    // without IEEE754, our bit twiddling will not work.
    // generally this will only fail for "exotic" systems:
    BOOST_STATIC_ASSERT(std::numeric_limits<float>::is_iec559);

    uint32_t mant;
    int exp, hi_exp, lo_exp;
    FPBox lo, hi, ans;

    lo.f = 0.0;
    hi.f = 1.0;

    lo_exp = (lo.i >> 23) & 0xFF;
    hi_exp = (hi.i >> 23) & 0xFF;

    //not >= because exp is decremented at the end of the last loop
    for (exp = hi_exp - 1; exp > lo_exp; exp--) {
        if (rand<bool>()) break;
    }

    mant = (rand32() & 0xFFFFFE00) >> 9; //use the high quality bits

    if (mant == 0 && rand<bool>()) exp++; //border values must not be skewed towards 1

    ans.i = (((uint32_t)exp) << 23) | mant; //combine exp and mantissa
    return ans.f;
}

template <> double Random::rand<double>() {
    // without IEEE754, our bit twiddling will not work.
    // generally this will only fail for "exotic" systems:
    BOOST_STATIC_ASSERT(std::numeric_limits<double>::is_iec559);

    int exp, hi_exp, lo_exp;
    uint64_t mant;
    FPBox lo, hi, ans;

    lo.d = 0.0;
    hi.d = 1.0;

    lo_exp = (lo.l >> 52) & 0x7FF;
    hi_exp = (hi.l >> 52) & 0x7FF;

    //not >= because exp is decremented at the end of the last loop
    for (exp = hi_exp - 1; exp > lo_exp; exp--) {
        if (rand<bool>()) break;
    }

    mant = ((uint64_t)rand32() << 32) | rand32();
    mant = (mant & 0xFFFFFFFFFFFFF000LL) >> 12; //use the high quality bits

    if (mant == 0 && rand<bool>()) exp++; //border values must not be skewed towards 1

    ans.l = (((uint64_t)exp) << 52) | mant; //combine exp and mantissa
    return ans.d;
}

/**********************************
 * Positive Range Specializations *
 **********************************/

template <> unsigned int Random::rand<unsigned int>(unsigned int hi) {
    unsigned int bits, val, lo;

    do {
        bits = rand<unsigned int>();
        val = bits % hi;
        lo = bits - val;
        //while this chunk wraps around to zero:
        //i.e. lo sign bit is 1, end-of-chunk sign bit is 0
    } while ( (lo & INT_SIGNBIT) & ~(lo+(hi-1)) );
    return val;
}

template <> int Random::rand<int>(int hi) {
    if (hi < 0) {
        return -rand<unsigned int>(-hi);
    } else {
        return rand<unsigned int>(hi);
    }
}

template <> unsigned long Random::rand<unsigned long>(unsigned long hi) {
    unsigned long bits, val, lo;

    do {
        bits = rand<unsigned long>();
        val = bits % hi;
        lo = bits - val;
        //while this chunk wraps around to zero:
        //i.e. lo sign bit is 1, end-of-chunk sign bit is 0
    } while ( (lo & LONG_SIGNBIT) & ~(lo+(hi-1)) );
    return val;
}

template <> long Random::rand<long>(long hi) {
    if (hi < 0) {
        return -rand<unsigned long>(-hi);
    } else {
        return rand<unsigned long>(hi);
    }
}

template <> unsigned long long Random::rand<unsigned long long>(unsigned long long hi) {
    unsigned long long bits, val, lo;

    do {
        bits = rand<unsigned long long>();
        val = bits % hi;
        lo = bits - val;
        //while this chunk wraps around to zero:
        //i.e. lo sign bit is 1, end-of-chunk sign bit is 0
    } while ( (lo & LONG_LONG_SIGNBIT) & ~(lo+(hi-1)) );
    return val;
}

template <> long long Random::rand<long long>(long long hi) {
    if (hi < 0) {
        return -rand<unsigned long long>(-hi);
    } else {
        return rand<unsigned long long>(hi);
    }
}

template <> float Random::rand<float>(float hi) {
    return hi * rand<float>();
}

template <> double Random::rand<double>(double hi) {
    return hi * rand<double>();
}

/**********************************
 * Full Range Specializations     *
 **********************************/

// todo: some of these will fail for ranges crossing zero where lo - hi is
// greater than INT_MAX.

template <> int Random::rand<int>(int lo, int hi) {
    int lo1 = min(hi,lo);
    return rand<int>(max(hi,lo) - lo1) + lo1;
}

template <> long long Random::rand<long long>(long long lo, long long hi) {
    long long lo1 = min(hi,lo);
    return rand<long long>(max(hi,lo) - lo1) + lo1;
}

template <> float Random::rand<float>(float lo, float hi) {
    return (hi-lo)*rand<float>() + lo;
}

template <> double Random::rand<double>(double lo, double hi) {
    return (hi-lo)*rand<double>() + lo;
}

/// @} //ingroup random

} //end namespace geom


#include <iostream>
#include <ojlibs/bignum.hpp>
#include <gtest/gtest.h>
#include <random>
#include <boost/multiprecision/gmp.hpp>

using namespace boost::multiprecision;
using namespace ojlibs::bignum;

std::mt19937 gen;
std::uniform_int_distribution<uint32_t> dist(0, ~0u);

bool SameNumber(const mpz_int &mpz, const nat &bnat) {
    auto mpz_inner = mpz.backend().data();

    size_t mpz_sz = mpz_size(mpz_inner);
    size_t nat_sz = bnat.nlimb();

    size_t gcd = std::min(sizeof(mp_limb_t), sizeof(WORD));

    size_t mpz_k = sizeof(mp_limb_t) / gcd;
    size_t nat_k = sizeof(WORD) / gcd;

    size_t cnt = std::max(mpz_sz * mpz_k, nat_sz * nat_k);

    const mp_limb_t *mpz_data = mpz_limbs_read(mpz_inner);
    const WORD *nat_data = bnat.limbs.data();

    uint64_t mask = (1ULL << (8 * gcd)) - 1;

    for (int i = 0; i < cnt; ++i) {
        uint64_t mpz_part = 0;
        uint64_t nat_part = 0;
        if (i / mpz_k < mpz_sz)
            mpz_part = (mpz_data[i / mpz_k] >> (8 * gcd * (i % mpz_k))) & mask;
        if (i / nat_k < nat_sz)
            nat_part = (nat_data[i / nat_k] >> (8 * gcd * (i % nat_k))) & mask;
        if (mpz_part != nat_part) return false;
    }
    return true;
}
bool SameNumber(WORD n, nat nn) {
    if (n == 0) return nn.nlimb() == 0;
    return nn.nlimb() == 1 && nn.limbs[0] == n;
}
std::vector<WORD> random_limbs(int nlimb) {
    std::vector<WORD> ret(nlimb);
    for (int i = 0; i < nlimb; ++i)
        ret[i] = dist(gen);

    if (nlimb && ret[nlimb - 1] == 0) ret[nlimb - 1] = 1; // should be norm
    return ret;
}
mpz_int limbs_to_mpz(std::vector<WORD> vec) {
    mpz_int ret = 0;
    for (int i = vec.size(); i-- > 0; ) {
        ret = (ret << WBITS) + vec[i];
    }
    return ret;
}

TEST(SANITY, SameNumber) {
    ASSERT_TRUE(SameNumber(mpz_int(34), nat(34u)));
    ASSERT_FALSE(SameNumber(mpz_int(34), nat(35u)));

    for (int tgroup = 0; tgroup < 100; ++tgroup) {
        auto lb = random_limbs(10);
        EXPECT_TRUE(SameNumber(limbs_to_mpz(lb), nat(lb)));
    }
}

TEST(ADD, VW) {
    for (int tgroup = 0; tgroup < 100; ++tgroup) {
        uint32_t r = dist(gen);
        mpz_int mp = r;
        nat nt(r);

        ASSERT_TRUE(SameNumber(mp, nt));

        for (int titer = 0; titer < 100; ++titer) {
            uint32_t r = dist(gen);
            mpz_int mpx = r;
            nat     ntx(r);

            mp = mp + mpx;
            nt = nt + ntx;

            ASSERT_TRUE(SameNumber(mp, nt));
        }
    }

}
TEST(ADD, VV) {
    static const size_t TSIZE = KARA_THRES * 2;
    for (int tgroup = 0; tgroup < 1; ++tgroup) {
        auto lb1 = random_limbs(TSIZE), lb2 = random_limbs(TSIZE);

        nat nt1(lb1), nt2(lb2);
        mpz_int mp1 = limbs_to_mpz(lb1), mp2 = limbs_to_mpz(lb2);

        nat nt = nt1 + nt2;
        mpz_int mp = mp1 + mp2;

        EXPECT_TRUE(SameNumber(mp, nt));
    }
}

TEST(SUB, VV) {
    static const size_t TSIZE = 100;
    std::uniform_int_distribution<int> dist_sz(0, TSIZE);
    for (int tgroup = 0; tgroup < 1; ++tgroup) {
        auto lb1 = random_limbs(dist_sz(gen)), lb2 = random_limbs(dist_sz(gen));

        nat nt1(lb1), nt2(lb2);
        mpz_int mp1 = limbs_to_mpz(lb1), mp2 = limbs_to_mpz(lb2);

        if (mp1 < mp2) std::swap(nt1, nt2), std::swap(mp1, mp2);

        nat nt = nt1 - nt2;
        mpz_int mp = mp1 - mp2;

        EXPECT_TRUE(SameNumber(mp, nt));
    }
}

TEST(MUL, VW) {
    for (int tgroup = 0; tgroup < 100; ++tgroup) {
        uint32_t r = dist(gen);
        mpz_int mp = r;
        nat     nt(r);

        ASSERT_TRUE(SameNumber(mp, nt));

        for (int titer = 0; titer < 100; ++titer) {
            uint32_t r = dist(gen);
            mpz_int mpx = r;
            nat     ntx(r);

            mp = mp * mpx;
            nt = nt * ntx;

            ASSERT_TRUE(SameNumber(mp, nt));
        }

    }
}
TEST(MUL, BASIC) {
    static const size_t TSIZE = KARA_THRES / 2;
    for (int tgroup = 0; tgroup < 100; ++tgroup) {
        auto lb1 = random_limbs(TSIZE), lb2 = random_limbs(TSIZE);
        nat nt1(lb1), nt2(lb2);
        mpz_int mp1 = limbs_to_mpz(lb1), mp2 = limbs_to_mpz(lb2);

        // nat nt = nt1 * nt2;
        nat nt;
        nt.limbs.resize(2 * TSIZE);
        basicMul(nt.to_span(), nt1.to_cspan(), nt2.to_cspan());
        nt.norm();

        mpz_int mp = mp1 * mp2;

        EXPECT_TRUE(SameNumber(mp, nt));
        EXPECT_EQ(0, nt.cmp(nt1 * nt2));
    }
}
TEST(MUL, LEADING) {
    static const size_t TSIZE = KARA_THRES;
    VEC lb1(TSIZE, 0), lb2(TSIZE, 0);
    lb1[TSIZE - 1] = 1;
    lb2[TSIZE - 1] = 1;

    nat nt1(lb1), nt2(lb2);
    mpz_int mp1 = limbs_to_mpz(lb1), mp2 = limbs_to_mpz(lb2);

    nat nt = nt1 * nt2;
    mpz_int mp = mp1 * mp2;
    EXPECT_TRUE(SameNumber(mp, nt));
}
TEST(MUL, VV) {
    static const size_t TSIZE = KARA_THRES * 50;
    std::uniform_int_distribution<int> dist_sz(0, TSIZE);
    for (int tgroup = 0; tgroup < 50; ++tgroup) {
        auto lb1 = random_limbs(dist_sz(gen)), lb2 = random_limbs(dist_sz(gen));

        nat nt1(lb1), nt2(lb2);
        mpz_int mp1 = limbs_to_mpz(lb1), mp2 = limbs_to_mpz(lb2);

        nat nt = nt1 * nt2;
        mpz_int mp = mp1 * mp2;

        // FIXME: EXPECT
        EXPECT_TRUE(SameNumber(mp, nt));
        // fprintf(stderr, "PASS\n");
    }
}
TEST(DIV, VW) {
    static const size_t TSIZE = KARA_THRES * 5;
    std::uniform_int_distribution<int> dist_sz(1, TSIZE);
    for (int tgroup = 0; tgroup < 100; ++tgroup) {
        auto lb1 = random_limbs(dist_sz(gen));
        mpz_int mp1 = limbs_to_mpz(lb1);
        nat nt1{lb1};

        WORD b = dist(gen);
        mpz_int mp2 = b;
        nat nt2{b};

        nat nt3 = nt1 / nt2;
        nat nt4 = nt1 % nt2;

        mpz_int mp3 = mp1 / mp2;
        mpz_int mp4 = mp1 % mp2;

        EXPECT_TRUE(SameNumber(mp3, nt3));
        EXPECT_TRUE(SameNumber(mp4, nt4));
    }
}
TEST(DIV, VVsmall) {
    VEC lb1 = {0, 1, 1, 2};
    mpz_int mp1 = limbs_to_mpz(lb1);
    nat nt1{lb1};

    VEC lb2 = {1, 100, 1, 100};
    mpz_int mp2 = limbs_to_mpz(lb2);
    nat nt2{lb2};

    nat nt3 = nt1 / nt2;
    nat nt4 = nt1 % nt2;

    mpz_int mp3 = mp1 / mp2;
    mpz_int mp4 = mp1 % mp2;

    ASSERT_TRUE(SameNumber(mp3, nt3));
    ASSERT_TRUE(SameNumber(mp4, nt4));
}
TEST(DIV, VV) {
    static const size_t TSIZE = KARA_THRES * 5;
    std::uniform_int_distribution<int> dist_sz(1, TSIZE);
    // std::uniform_int_distribution<int> dist_sz(6, 6);
    for (int tgroup = 0; tgroup < 100; ++tgroup) {
        auto lb1 = random_limbs(dist_sz(gen));
        mpz_int mp1 = limbs_to_mpz(lb1);
        nat nt1{lb1};

        auto lb2 = random_limbs(dist_sz(gen));
        mpz_int mp2 = limbs_to_mpz(lb2);
        nat nt2{lb2};

        nat nt3 = nt1 / nt2;
        nat nt4 = nt1 % nt2;

        mpz_int mp3 = mp1 / mp2;
        mpz_int mp4 = mp1 % mp2;

        ASSERT_TRUE(SameNumber(mp4, nt4));
        ASSERT_TRUE(SameNumber(mp3, nt3));
    }
}
TEST(STRCONV, UTOA) {
    static const size_t TSIZE = 500;
    std::uniform_int_distribution<int> dist_sz(1, TSIZE);
    // std::uniform_int_distribution<int> dist_sz(6, 6);
    for (int tgroup = 0; tgroup < 100; ++tgroup) {
        auto lb1 = random_limbs(dist_sz(gen));
        mpz_int mp1 = limbs_to_mpz(lb1);
        nat nt1{lb1};

        auto mps = mp1.str();
        auto nts = utoa(nt1, 10);

        ASSERT_EQ(mps, nts);
    }
}
TEST(STRCONV, BACKFORTH) {
    static const size_t TSIZE = 50;
    std::uniform_int_distribution<int> dist_sz(1, TSIZE);
    // std::uniform_int_distribution<int> dist_sz(6, 6);
    for (int base = 2; base <= 62; ++base) {
        for (int tgroup = 0; tgroup < 100; ++tgroup) {
            auto lb1 = random_limbs(dist_sz(gen));

            nat nt(lb1);
            std::string s = utoa(nt, base);
            nat nt2 = str_to_nat(s.c_str(), base);

            ASSERT_TRUE(nt == nt2);
        }
    }
}


/*
 * sdf2sdf_optimizer2d.cpp
 *
 *  Created on: Mar 05, 2019
 *      Author: Fei Shan
 */

//libraries
#include <boost/python.hpp>
#include <iostream>

// local
#include "../math/transformation.hpp"
#include "sdf_2_sdf_optimizer2d.hpp"
#include "sdf_gradient_wrt_transformation2d.hpp"

namespace ropt = rigid_optimization;
namespace eig = Eigen;

namespace rigid_optimization {

Sdf2SdfOptimizer2d::Sdf2SdfOptimizer2d(
		float rate,
		int maximum_iteration_count,
		tsdf::Parameters2d tsdf_generation_parameters,
		VerbosityParameters verbosity_parameters) :
		rate(rate),
				maximum_iteration_count(maximum_iteration_count),
				tsdf_generator(tsdf_generation_parameters),
				verbosity_parameters(verbosity_parameters)
{
}
;

Sdf2SdfOptimizer2d::~Sdf2SdfOptimizer2d()
{
}
;

Sdf2SdfOptimizer2d::VerbosityParameters::VerbosityParameters(
		bool print_iteration_max_warp_update,
		bool print_iteration_energy) :
		print_iteration_max_warp_update(print_iteration_max_warp_update),
				print_iteration_energy(print_iteration_energy),
				print_per_iteration_info(
						print_iteration_max_warp_update ||
								print_iteration_energy
								)
{
}
;

eig::Matrix3f Sdf2SdfOptimizer2d::optimize(int image_y_coordinate,
		const eig::MatrixXf canonical_field,
		const eig::Matrix<unsigned short, eig::Dynamic, eig::Dynamic>& live_depth_image,
		float eta,
		const eig::Matrix4f& initial_camera_pose) {

	eig::MatrixXf canonical_weight = canonical_field.replicate(1, 1);
	for (int i = 0; i < canonical_weight.rows(); ++i) { // Determine weight based on thickness
		for (int j = 0; j < canonical_weight.cols(); ++j) {
			canonical_weight(i, j) = (canonical_field(i, j) <= -eta) ? 0.0f : 1.0f;
		}
	}

	eig::Vector3f twist = eig::Vector3f(0.0f, 0.0f, 0.0f);

	for (int iteration_count = 0; iteration_count < maximum_iteration_count; ++iteration_count) {
		eig::Matrix3f matrix_A = eig::Matrix3f::Zero(); // from sdf2sdf paper,
														// used for calculating the optimal transformation.
		eig::Vector3f vector_b = eig::Vector3f::Zero(); // from sdf2sdf paper,
														// used to calculating the optimal transformation.

		eig::Matrix<float, 6, 1> twist3d;
		twist3d << twist(0), 0.0f, twist(1), 0.0f, twist(2), 0.0f;
		eig::Matrix4f twist_matrix3d = math::transformation_vector_to_matrix3d(twist3d);
		eig::MatrixXf live_field = this->tsdf_generator.generate(live_depth_image,
				twist_matrix3d,
				image_y_coordinate);
		eig::MatrixXf live_weight = live_field.replicate(1, 1);
		for (int j = 0; j < live_weight.cols(); ++j) { // Determine weight based on thickness
			for (int i = 0; i < live_weight.rows(); ++i) {
				live_weight(i, j) = (live_field(i, j) <= -eta) ? 0.0f : 1.0f;
			}
		}
		eig::Matrix<eig::Vector3f, eig::Dynamic, eig::Dynamic> live_gradient(canonical_field.rows(),
				canonical_field.cols());
		eig::Vector3i offset(tsdf_generator.parameters.array_offset.x,
				0,
				tsdf_generator.parameters.array_offset.y);
		ropt::gradient_wrt_twist(live_field,
				twist,
				offset,
				tsdf_generator.parameters.voxel_size,
				live_gradient);

		for (int j = 0; j < live_field.cols(); ++j) {
			for (int i = 0; i < live_field.rows(); ++i) {
				matrix_A += live_gradient(i, j) * live_gradient(i, j).transpose();
				vector_b += (canonical_field(i, j) - live_field(i, j) + live_gradient(i, j).transpose() * twist)
						* live_gradient(i, j);
			}
		}

		float energy = .5f * (canonical_field.cwiseProduct(canonical_weight) -
				live_field.cwiseProduct(live_weight)).array().pow(2.f).sum();
		eig::Vector3f optimal_twist = matrix_A.inverse() * vector_b;
		twist = twist + this->rate * (optimal_twist - twist);
		if (this->verbosity_parameters.print_per_iteration_info) {
			std::cout << "[ITERATION " << iteration_count << " COMPLETED]\n";
			if (this->verbosity_parameters.print_iteration_max_warp_update) {
				std::cout << " [optimize twist:" << optimal_twist.transpose() << "]\n";
				std::cout << " [twist:" << twist.transpose() << "]\n";
			}
			if (this->verbosity_parameters.print_iteration_energy) {
				std::cout << " [energy: " << energy << "]\n\n" << std::endl;
			}

		}
	}

    eig::Matrix3f twist_matrix2d = math::transformation_vector_to_matrix2d(twist);
	return twist_matrix2d;
}

}


/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2015, Thomas Mörwald
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the copyright holder(s) nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 *
 *
 */

#include "patch.h"
#include <stdexcept>

#undef Success
#include <Eigen/Dense>

using namespace nurbsfit;

void FitPatch::initSurface(int dims, int order0, int order1, int cps0, int cps1, Domain roi)
{
  if( cps0 < order0 )
    cps0 = order0;

  if( cps1 < order1 )
    cps1 = order1;

  m_nurbs = ON_NurbsSurface (dims, false, order0, order1, cps0, cps1);

  double ddx = roi.width  / (m_nurbs.KnotCount(0) - 2*(order0-2) - 1);
  double ddy = roi.height / (m_nurbs.KnotCount(1) - 2*(order1-2) - 1);

  m_nurbs.MakeClampedUniformKnotVector (0, ddx);
  m_nurbs.MakeClampedUniformKnotVector (1, ddy);

  for (int i = 0; i < m_nurbs.KnotCount(0); i++)
  {
    double k = m_nurbs.Knot (0, i);
    m_nurbs.SetKnot (0, i, k + roi.x);
  }

  for (int i = 0; i < m_nurbs.KnotCount(1); i++)
  {
    double k = m_nurbs.Knot (1, i);
    m_nurbs.SetKnot (1, i, k + roi.y);
  }

  m_x = Eigen::VectorXd(m_nurbs.CVCount()*dims,1);
  m_x.setZero();
  for (int i = 0; i < m_nurbs.CVCount(0); i++)
  {
    for (int j = 0; j < m_nurbs.CVCount(1); j++)
    {
      m_nurbs.SetCV (i, j, ON_3dPoint(m_nurbs.Knot(0, i),m_nurbs.Knot(1, j),0));
    }
  }
}

void FitPatch::initSolver(const Eigen::VectorXd& param0, const Eigen::VectorXd& param1)
{
  if(param0.rows()!=param1.rows())
    throw std::runtime_error("[FitPatch::initSolver] Error, param vectors must be of same length.");

  if(m_nurbs.CVCount() <= 0)
    throw std::runtime_error("[FitPatch::initSolver] Error, surface not initialized (initSurface).");

  int dim = m_nurbs.Dimension();
  m_A = SparseMatrix( param0.rows()*dim, m_nurbs.CVCount()*dim );

  typedef Eigen::Triplet<double> Tri;
  std::vector<Tri> tripletList;
  tripletList.resize( 3*param0.rows() * m_nurbs.Order(0) * m_nurbs.Order(1) );

  if(!m_quiet)
    printf("[FitPatch::initSolver] entries: %lu  rows: %lu  cols: %d\n",
           param0.rows()* m_nurbs.Order(0) * m_nurbs.Order(1),
           param0.rows(),
           m_nurbs.CVCount());

  double *N0 = new double[m_nurbs.Order (0) * m_nurbs.Order (0)];
  double *N1 = new double[m_nurbs.Order (1) * m_nurbs.Order (1)];
  int E,F;
  int ti(0);

  for(Eigen::MatrixXd::Index row=0; row<param0.rows(); row++)
  {
    E = ON_NurbsSpanIndex (m_nurbs.m_order[0], m_nurbs.m_cv_count[0], m_nurbs.m_knot[0], param0(row), 0, 0);
    F = ON_NurbsSpanIndex (m_nurbs.m_order[1], m_nurbs.m_cv_count[1], m_nurbs.m_knot[1], param1(row), 0, 0);

    ON_EvaluateNurbsBasis (m_nurbs.Order (0), m_nurbs.m_knot[0] + E, param0(row), N0);
    ON_EvaluateNurbsBasis (m_nurbs.Order (1), m_nurbs.m_knot[1] + F, param1(row), N1);

    for (int i = 0; i < m_nurbs.Order (0); i++)
    {
      for (int j = 0; j < m_nurbs.Order (1); j++)
      {
        tripletList[ti] = Tri( dim*row, dim*lrc2gl(E, F, i, j), N0[i] * N1[j] );
        if(dim>1)
          tripletList[ti+1] = Tri( dim*row+1, dim*lrc2gl(E, F, i, j)+1, N0[i] * N1[j] );
        if(dim>2)
          tripletList[ti+2] = Tri( dim*row+2, dim*lrc2gl(E, F, i, j)+2, N0[i] * N1[j] );
        ti+=dim;
      } // j
    } // i

  } // row

  delete [] N1;
  delete [] N0;

  m_A.setFromTriplets(tripletList.begin(), tripletList.end());

  if(m_solver==NULL)
    m_solver = new SPQR();
  m_solver->compute(m_A);
  if(m_solver->info()!=Eigen::Success)
    throw std::runtime_error("[FitPatch::initSolver] decomposition failed.");

  if(!m_quiet)
    printf("[FitPatch::initSolver] decomposition done\n");
}

void FitPatch::solve(const Eigen::VectorXd& values)
{
  m_x = m_solver->solve(values);

  updateSurf();
}

void FitPatch::updateSurf()
{
  int ncp = m_nurbs.CVCount ();

  if(m_x.rows()!=ncp*m_nurbs.Dimension())
    throw std::runtime_error("[FitPatch::updateSurf] Error, number of control points does not match.");

  for (int i = 0; i < ncp; i++)
  {
    ON_3dPoint cp;

    if(m_nurbs.Dimension()==1)
    {
      cp.x = m_x(i);
    }

    if(m_nurbs.Dimension()==2)
    {
      cp.x = m_x(2*i+0);
      cp.y = m_x(2*i+1);
    }

    if(m_nurbs.Dimension()==3)
    {
      cp.x = m_x(3*i+0);
      cp.y = m_x(3*i+1);
      cp.z = m_x(3*i+2);
    }

    m_nurbs.SetCV(gl2gr(i), gl2gc(i), cp);
  }
}

Eigen::VectorXd FitPatch::getError(const Eigen::VectorXd& values)
{
  // compute A*x (i.e. points on surface)
  Eigen::VectorXd Ax(values.rows(),1);
  Ax.setZero();
  for (int k=0; k<m_A.outerSize(); ++k)
  for (SparseMatrix::InnerIterator it(m_A,k); it; ++it)
    Ax(it.row()) += it.value() * m_x(it.col());

  // return (A*x-b)
  return (Ax-values);
}

Eigen::Vector2d FitPatch::reparameterize(const Eigen::VectorXd &value, const Eigen::Vector2d &hint,
                                         int& steps, double &accuracy, int maxSteps, double minAccuracy)
{
  if(m_nurbs.CVCount() <= 0)
    throw std::runtime_error("[FitPatch::reparameterize] Error, surface not initialized (initSurface).");

  int nder = 1; // number of derivatives
  int dims = m_nurbs.Dimension();
  int nvals = dims*(nder+1)*(nder+2)/2;
  double pointAndTangents[nvals];

  Eigen::Vector2d current, delta, b, c;
  Eigen::Matrix2d A, I;
  Eigen::VectorXd p(dims), tu(dims), tv(dims), r(dims);
  I = Eigen::Matrix2d::Identity();

  double minU = m_nurbs.Knot(0,0);
  double minV = m_nurbs.Knot(1,0);
  double maxU = m_nurbs.Knot(0,m_nurbs.KnotCount(0)-1);
  double maxV = m_nurbs.Knot(1,m_nurbs.KnotCount(1)-1);
  double nu = 0.5;
  double lambda = 1.0;
  double accuracy_old(DBL_MAX);

  current = hint;

  for (steps = 0; steps < maxSteps; steps++)
  {

    m_nurbs.Evaluate (current(0), current(1), nder, dims, pointAndTangents);

    if(dims==1)
    {
      p(0) = pointAndTangents[0];
      tu(0) = pointAndTangents[1];
      tv(0) = pointAndTangents[2];
    }

    if(dims==2)
    {
      p(0) = pointAndTangents[0];
      p(1) = pointAndTangents[1];
      tu(0) = pointAndTangents[2];
      tu(1) = pointAndTangents[3];
      tv(0) = pointAndTangents[4];
      tv(1) = pointAndTangents[5];
    }

    if(dims==3)
    {
      p(0) = pointAndTangents[0];
      p(1) = pointAndTangents[1];
      p(2) = pointAndTangents[2];
      tu(0) = pointAndTangents[3];
      tu(1) = pointAndTangents[4];
      tu(2) = pointAndTangents[5];
      tv(0) = pointAndTangents[6];
      tv(1) = pointAndTangents[7];
      tv(2) = pointAndTangents[8];
    }

    r = p - value;

    b(0) = -r.dot (tu);
    b(1) = -r.dot (tv);

    A(0, 0) = tu.dot (tu);
    A(0, 1) = tu.dot (tv);
    A(1, 0) = A (0, 1);
    A(1, 1) = tv.dot (tv);

    delta = A.ldlt().solve(b);

    accuracy = delta.norm();
    if (accuracy < minAccuracy)
      return current;

    // step width control (quite heuristic)
    if(accuracy>accuracy_old*nu)
      lambda *= nu;
    accuracy_old = accuracy;

    // make step
    c = current + lambda * delta;

    // clamp to domain borders
    if (c(0) < minU)
      c(0) = minU;
    else if (c (0) > maxU)
      c(0) = maxU;
    if (c(1) < minV)
      c(1) = minV;
    else if (c(1) > maxV)
      c(1) = maxV;

    // compute real step
    delta = c - current;
    current = c;

    accuracy = delta.norm();
    if (accuracy < minAccuracy)
      return current;
  }

  printf ("[FitPatch::reparameterize] Warning: Method did not converge (%e %e %d)\n",
          accuracy, minAccuracy, maxSteps);
  printf ("[FitPatch::reparameterize]   (0: %f %f) (1: %f %f) %f %f ... %f %f\n",
          minU, maxU, minV, maxV,
          hint (0), hint (1), current (0), current (1));

  return current;
}

//int nvals = nurbs.m_dim*(nder+1)*(nder+2)/2;
//double P[nvals];
//nurbs.Evaluate (u, v, nder, nurbs.m_dim, P);

//// positions
//xx = P[0];    xy = P[1];    xz = P[2];

//// 1st derivatives (for normals)
//xu(0) = P[3];    xu(1) = P[4];    xu(2) = P[5];
//xv(0) = P[6];    xv(1) = P[7];    xv(2) = P[8];

//n = xu.cross(xv);
//n.normalize();
//v.normal = TomGine::vec3(n(0),n(1),n(2));

//// 2nd derivatives (for curvature)
//xuu(0) = P[9];     xuu(1) = P[10];    xuu(2) = P[11];
//xuv(0) = P[12];    xuv(1) = P[13];    xuv(2) = P[14];
//xvv(0) = P[15];    xvv(1) = P[16];    xvv(2) = P[17];

//#include <unsupported/Eigen/NumericalDiff>
//#include <unsupported/Eigen/NonLinearOptimization>

//struct SurfaceFunctor
//{
//  const Eigen::VectorXd& m_residuals;
//  const ON_NurbsSurface& m_nurbs;

//  SurfaceFunctor(const Eigen::VectorXd& residuals, const ON_NurbsSurface& nurbs)
//    : m_residuals(residuals), m_nurbs(nurbs){}

//  int operator()(const Eigen::VectorXd& x, Eigen::VectorXd& residuals) const
//  {

//    return 0;
//  }

//  int dx(const Eigen::VectorXd& x, Eigen::MatrixXd& jacobian)
//  {

//    return 0;
//  }

//  int inputs() const { return m_nurbs.CVCount(); }
//  int values() const { return m_residuals.rows(); }
//};

//Eigen::Vector2d FitPatch::reparameterizeLM(const Eigen::VectorXd &value, const Eigen::Vector2d &hint,
//                                         int& steps, double &accuracy, int maxSteps, double minAccuracy)
//{
//  if(m_nurbs.CVCount() <= 0)
//    throw std::runtime_error("[FitPatch::reparameterize] Error, surface not initialized (initSurface).");

//  int nder = 1; // number of derivatives
//  int dims = m_nurbs.Dimension();
//  int nvals = dims*(nder+1)*(nder+2)/2;
//  double pointAndTangents[nvals];

//  Eigen::Vector2d current, delta, b, c;
//  Eigen::Matrix2d A, I;
//  Eigen::VectorXd p(dims), tu(dims), tv(dims), r(dims);
//  I = Eigen::Matrix2d::Identity();

//  double minU = m_nurbs.Knot(0,0);
//  double minV = m_nurbs.Knot(1,0);
//  double maxU = m_nurbs.Knot(0,m_nurbs.KnotCount(0)-1);
//  double maxV = m_nurbs.Knot(1,m_nurbs.KnotCount(1)-1);

//  current = hint;

//  SurfaceFunctor func;
//  Eigen::LevenbergMarquardt<Eigen::NumericalDiff<my_functor>,double> lm(numDiff);

//  for (steps = 0; steps < maxSteps; steps++)
//  {

//    m_nurbs.Evaluate (current(0), current(1), nder, dims, pointAndTangents);

//    if(dims==1)
//    {
//      p(0) = pointAndTangents[0];
//      tu(0) = pointAndTangents[1];
//      tv(0) = pointAndTangents[2];
//    }

//    if(dims==2)
//    {
//      p(0) = pointAndTangents[0];
//      p(1) = pointAndTangents[1];
//      tu(0) = pointAndTangents[2];
//      tu(1) = pointAndTangents[3];
//      tv(0) = pointAndTangents[4];
//      tv(1) = pointAndTangents[5];
//    }

//    if(dims==3)
//    {
//      p(0) = pointAndTangents[0];
//      p(1) = pointAndTangents[1];
//      p(2) = pointAndTangents[2];
//      tu(0) = pointAndTangents[3];
//      tu(1) = pointAndTangents[4];
//      tu(2) = pointAndTangents[5];
//      tv(0) = pointAndTangents[6];
//      tv(1) = pointAndTangents[7];
//      tv(2) = pointAndTangents[8];
//    }

//    r = p - value;

//    b(0) = -r.dot (tu);
//    b(1) = -r.dot (tv);

//    A(0, 0) = tu.dot (tu);
//    A(0, 1) = tu.dot (tv);
//    A(1, 0) = A (0, 1);
//    A(1, 1) = tv.dot (tv);

//    delta = A.ldlt().solve(b);

//    accuracy = delta.norm();
//    if (accuracy < minAccuracy)
//      return current;

//    // make step
//    c = current + delta;

//    // clamp to domain borders
//    if (c(0) < minU)
//      c(0) = minU;
//    else if (c (0) > maxU)
//      c(0) = maxU;
//    if (c(1) < minV)
//      c(1) = minV;
//    else if (c(1) > maxV)
//      c(1) = maxV;

//    // compute real step
//    delta = c - current;
//    current = c;

//    accuracy = delta.norm();
//    if (accuracy < minAccuracy)
//      return current;
//  }

//  printf ("[FitPatch::reparameterize] Warning: Method did not converge (%e %e %d)\n",
//          accuracy, minAccuracy, maxSteps);
//  printf ("[FitPatch::reparameterize]   (0: %f %f) (1: %f %f) %f %f ... %f %f\n",
//          minU, maxU, minV, maxV,
//          hint (0), hint (1), current (0), current (1));

//  return current;
//}


#include "nnw_hierarchy.h"

#include <google/protobuf/stubs/casts.h>

#include <Eigen/Dense>
#include <stan/math/prim/prob.hpp>

#include "hierarchy_prior.pb.h"
#include "ls_state.pb.h"
#include "marginal_state.pb.h"
#include "matrix.pb.h"
#include "src/utils/distributions.h"
#include "src/utils/eigen_utils.h"
#include "src/utils/proto_utils.h"
#include "src/utils/rng.h"

//! \param prec_ Value to set to prec
void NNWHierarchy::set_prec_and_utilities(const Eigen::MatrixXd &prec_) {
  state.prec = prec_;

  // Update prec utilities
  prec_chol = Eigen::LLT<Eigen::MatrixXd>(prec_).matrixL().transpose();
  Eigen::VectorXd diag = prec_chol.diagonal();
  prec_logdet = 2 * log(diag.array()).sum();
}

void NNWHierarchy::initialize() {
  if (prior == nullptr) {
    throw std::invalid_argument("Hierarchy prior was not provided");
  }
  state.mean = hypers->mean;
  set_prec_and_utilities(hypers->var_scaling *
                         Eigen::MatrixXd::Identity(dim, dim));
  clear_data();
}

void NNWHierarchy::clear_data() {
  data_sum = Eigen::VectorXd::Zero(dim);
  data_sum_squares = Eigen::MatrixXd::Zero(dim, dim);
  card = 0;
  cluster_data_idx = std::set<int>();
}

void NNWHierarchy::update_summary_statistics(const Eigen::VectorXd &datum,
                                             bool add) {
  if (add) {
    data_sum += datum;
    data_sum_squares += datum * datum.transpose();
  } else {
    data_sum -= datum;
    data_sum_squares -= datum * datum.transpose();
  }
}

//! \param data                    Matrix of row-vectorial data points
//! \param mu0, lambda0, tau0, nu0 Original values for hyperparameters
//! \return                        Vector of updated values for hyperparameters
NNWHierarchy::Hyperparams NNWHierarchy::normal_wishart_update() {
  // Initialize relevant objects
  Hyperparams post_params;

  // Compute updated hyperparameters
  post_params.var_scaling = hypers->var_scaling + card;
  post_params.deg_free = hypers->deg_free + 0.5 * card;

  Eigen::VectorXd mubar = data_sum.array() / card;  // sample mean
  post_params.mean = (hypers->var_scaling * hypers->mean + card * mubar) /
                     (hypers->var_scaling + card);
  // Compute tau_n
  Eigen::MatrixXd tau_temp =
      data_sum_squares - card * mubar * mubar.transpose();
  tau_temp += (card * hypers->var_scaling / (card + hypers->var_scaling)) *
              (mubar - hypers->mean) * (mubar - hypers->mean).transpose();
  tau_temp = 0.5 * tau_temp + hypers->scale_inv;
  post_params.scale = stan::math::inverse_spd(tau_temp);
  return post_params;
}

void NNWHierarchy::update_hypers(
    const std::vector<bayesmix::MarginalState::ClusterState> &states) {
  auto &rng = bayesmix::Rng::Instance().get();
  if (prior->has_fixed_values()) {
    return;
  }

  else if (prior->has_normal_mean_prior()) {
    // Get hyperparameters
    Eigen::VectorXd mu00 =
        bayesmix::to_eigen(prior->normal_mean_prior().mean_prior().mean());
    Eigen::MatrixXd sigma00 =
        bayesmix::to_eigen(prior->normal_mean_prior().mean_prior().var());
    double lambda0 = prior->normal_mean_prior().var_scaling();
    // Compute posterior hyperparameters
    Eigen::MatrixXd sigma00inv = stan::math::inverse_spd(sigma00);
    Eigen::MatrixXd prec = Eigen::MatrixXd::Zero(dim, dim);
    Eigen::VectorXd num = Eigen::MatrixXd::Zero(dim, 1);
    for (auto &st : states) {
      Eigen::MatrixXd prec_i = bayesmix::to_eigen(st.multi_ls_state().prec());
      prec += prec_i;
      num += prec_i * bayesmix::to_eigen(st.multi_ls_state().mean());
    }
    prec = hypers->var_scaling * prec + sigma00inv;
    num = hypers->var_scaling * num + sigma00inv * mu00;
    Eigen::VectorXd mu_n = prec.llt().solve(num);
    // Update hyperparameters with posterior sampling
    hypers->mean = stan::math::multi_normal_prec_rng(mu_n, prec, rng);
  }

  else if (prior->has_ngiw_prior()) {
    // Get hyperparameters:
    // for mu0
    Eigen::VectorXd mu00 =
        bayesmix::to_eigen(prior->ngiw_prior().mean_prior().mean());
    Eigen::MatrixXd sigma00 =
        bayesmix::to_eigen(prior->ngiw_prior().mean_prior().var());
    // for lambda0
    double alpha00 = prior->ngiw_prior().var_scaling_prior().shape();
    double beta00 = prior->ngiw_prior().var_scaling_prior().rate();
    // for tau0
    double nu00 = prior->ngiw_prior().scale_prior().deg_free();
    Eigen::MatrixXd tau00 =
        bayesmix::to_eigen(prior->ngiw_prior().scale_prior().scale());
    // Compute posterior hyperparameters
    Eigen::MatrixXd sigma00inv = stan::math::inverse_spd(sigma00);
    Eigen::MatrixXd tau_n = Eigen::MatrixXd::Zero(dim, dim);
    Eigen::VectorXd num = Eigen::MatrixXd::Zero(dim, 1);
    double beta_n = 0.0;
    for (auto &st : states) {
      Eigen::VectorXd mean = bayesmix::to_eigen(st.multi_ls_state().mean());
      Eigen::MatrixXd prec = bayesmix::to_eigen(st.multi_ls_state().prec());
      tau_n += prec;
      num += prec * mean;
      beta_n +=
          (hypers->mean - mean).transpose() * prec * (hypers->mean - mean);
    }
    Eigen::MatrixXd prec_n = hypers->var_scaling * tau_n + sigma00inv;
    tau_n += tau00;
    num = hypers->var_scaling * num + sigma00inv * mu00;
    beta_n = beta00 + 0.5 * beta_n;
    Eigen::MatrixXd sig_n = stan::math::inverse_spd(prec_n);
    Eigen::VectorXd mu_n = sig_n * num;
    double alpha_n = alpha00 + 0.5 * states.size();
    double nu_n = nu00 + states.size() * hypers->deg_free;
    // Update hyperparameters with posterior random Gibbs sampling
    hypers->mean = stan::math::multi_normal_rng(mu_n, sig_n, rng);
    hypers->var_scaling = stan::math::gamma_rng(alpha_n, beta_n, rng);
    hypers->scale = stan::math::inv_wishart_rng(nu_n, tau_n, rng);
    hypers->scale_inv = stan::math::inverse_spd(hypers->scale);
  }

  else {
    throw std::invalid_argument("Unrecognized hierarchy prior");
  }
}

//! \param data Matrix of row-vectorial single data point
//! \return     Log-Likehood vector evaluated in data
double NNWHierarchy::like_lpdf(const Eigen::RowVectorXd &datum) const {
  // Initialize relevant objects
  return bayesmix::multi_normal_prec_lpdf(datum, state.mean, prec_chol,
                                          prec_logdet);
}

//! \param data Matrix of row-vectorial a single data point
//! \return     Marginal distribution vector evaluated in data
double NNWHierarchy::marg_lpdf(const Eigen::RowVectorXd &datum) const {
  // Compute dof and scale of marginal distribution
  double nu_n = 2 * hypers->deg_free - dim + 1;
  Eigen::MatrixXd sigma_n = hypers->scale_inv *
                            (hypers->deg_free - 0.5 * (dim - 1)) *
                            hypers->var_scaling / (hypers->var_scaling + 1);

  // TODO: chec if this is optimized as our bayesmix::multi_normal_prec_lpdf
  return stan::math::multi_student_t_lpdf(datum, nu_n, hypers->mean, sigma_n);
}

void NNWHierarchy::draw() {
  // Generate new state values from their prior centering distribution
  auto &rng = bayesmix::Rng::Instance().get();
  Eigen::MatrixXd tau_new =
      stan::math::wishart_rng(hypers->deg_free, hypers->scale, rng);

  // Update state
  state.mean = stan::math::multi_normal_prec_rng(
      hypers->mean, tau_new * hypers->var_scaling, rng);
  set_prec_and_utilities(tau_new);
}

//! \param data Matrix of row-vectorial data points
void NNWHierarchy::sample_given_data() {
  // Update values
  Hyperparams params = normal_wishart_update();

  // Generate new state values from their prior centering distribution
  auto &rng = bayesmix::Rng::Instance().get();
  Eigen::MatrixXd tau_new =
      stan::math::wishart_rng(params.deg_free, params.scale, rng);
  state.mean = stan::math::multi_normal_prec_rng(
      params.mean, tau_new * params.var_scaling, rng);

  // Update state
  set_prec_and_utilities(tau_new);
}

void NNWHierarchy::sample_given_data(const Eigen::MatrixXd &data) {
  data_sum = Eigen::VectorXd::Zero(data.cols());
  data_sum_squares = Eigen::MatrixXd::Zero(data.cols(), data.cols());

  for (int i = 0; i < data.rows(); i++) {
    data_sum += data.row(i);
    data_sum_squares += data.row(i).transpose() * data.row(i);
  }
  card = data.rows();
  log_card = std::log(card);
  sample_given_data();
}

void NNWHierarchy::set_state_from_proto(
    const google::protobuf::Message &state_) {
  auto &statecast = google::protobuf::internal::down_cast<
      const bayesmix::MarginalState::ClusterState &>(state_);
  state.mean = to_eigen(statecast.multi_ls_state().mean());
  set_prec_and_utilities(to_eigen(statecast.multi_ls_state().prec()));
  set_card(statecast.cardinality());
}

void NNWHierarchy::set_prior(const google::protobuf::Message &prior_) {
  auto &priorcast =
      google::protobuf::internal::down_cast<const bayesmix::NNWPrior &>(
          prior_);
  prior = std::make_shared<bayesmix::NNWPrior>(priorcast);
  hypers = std::make_shared<Hyperparams>();
  if (prior->has_fixed_values()) {
    // Set values
    hypers->mean = bayesmix::to_eigen(prior->fixed_values().mean());
    dim = hypers->mean.size();
    hypers->var_scaling = prior->fixed_values().var_scaling();
    hypers->scale = bayesmix::to_eigen(prior->fixed_values().scale());
    hypers->scale_inv = stan::math::inverse_spd(hypers->scale);
    hypers->deg_free = prior->fixed_values().deg_free();
    // Check validity
    if (hypers->var_scaling <= 0) {
      throw std::invalid_argument("Variance-scaling parameter must be > 0");
    }
    if (dim != hypers->scale.rows()) {
      throw std::invalid_argument(
          "Hyperparameters dimensions are not consistent");
    }
    if (hypers->deg_free <= dim - 1) {
      throw std::invalid_argument("Degrees of freedom parameter is not valid");
    }
  }

  else if (prior->has_normal_mean_prior()) {
    // Get hyperparameters
    Eigen::VectorXd mu00 =
        bayesmix::to_eigen(prior->normal_mean_prior().mean_prior().mean());
    dim = mu00.size();
    Eigen::MatrixXd sigma00 =
        bayesmix::to_eigen(prior->normal_mean_prior().mean_prior().var());
    double lambda0 = prior->normal_mean_prior().var_scaling();
    Eigen::MatrixXd tau0 =
        bayesmix::to_eigen(prior->normal_mean_prior().scale());
    double nu0 = prior->normal_mean_prior().deg_free();
    // Check validity
    unsigned int dim = mu00.size();
    if (sigma00.rows() != dim or tau0.rows() != dim) {
      throw std::invalid_argument(
          "Hyperparameters dimensions are not consistent");
    }
    bayesmix::check_spd(sigma00);
    if (lambda0 <= 0) {
      throw std::invalid_argument("Variance-scaling parameter must be > 0");
    }
    bayesmix::check_spd(tau0);
    if (nu0 <= dim - 1) {
      throw std::invalid_argument("Degrees of freedom parameter is not valid");
    }
    // Set initial values
    hypers->mean = mu00;
    hypers->var_scaling = lambda0;
    hypers->scale = tau0;
    hypers->scale_inv = stan::math::inverse_spd(tau0);
    hypers->deg_free = nu0;
  }

  else if (prior->has_ngiw_prior()) {
    // Get hyperparameters:
    // for mu0
    Eigen::VectorXd mu00 =
        bayesmix::to_eigen(prior->ngiw_prior().mean_prior().mean());
    dim = mu00.size();
    Eigen::MatrixXd sigma00 =
        bayesmix::to_eigen(prior->ngiw_prior().mean_prior().var());
    // for lambda0
    double alpha00 = prior->ngiw_prior().var_scaling_prior().shape();
    double beta00 = prior->ngiw_prior().var_scaling_prior().rate();
    // for tau0
    double nu00 = prior->ngiw_prior().scale_prior().deg_free();
    Eigen::MatrixXd tau00 =
        bayesmix::to_eigen(prior->ngiw_prior().scale_prior().scale());
    // for nu0
    double nu0 = prior->ngiw_prior().deg_free();
    // Check validity:
    // dimensionality
    if (sigma00.rows() != dim or tau00.rows() != dim) {
      throw std::invalid_argument(
          "Hyperparameters dimensions are not consistent");
    }
    // for mu0
    bayesmix::check_spd(sigma00);
    // for lambda0
    if (alpha00 <= 0) {
      throw std::invalid_argument("Shape parameter must be > 0");
    }
    if (beta00 <= 0) {
      throw std::invalid_argument("Rate parameter must be > 0");
    }
    // for tau0
    if (nu00 <= 0) {
      throw std::invalid_argument("Degrees of freedom parameter must be > 0");
    }
    bayesmix::check_spd(tau00);
    // check nu0
    if (nu0 <= dim - 1) {
      throw std::invalid_argument("Degrees of freedom parameter is not valid");
    }
    // Set initial values
    hypers->mean = mu00;
    hypers->var_scaling = alpha00 / beta00;
    hypers->scale = tau00 / (nu00 + dim + 1);
    hypers->scale_inv = stan::math::inverse_spd(hypers->scale);
    hypers->deg_free = nu0;
  }

  else {
    throw std::invalid_argument("Unrecognized hierarchy prior");
  }
}

void NNWHierarchy::write_state_to_proto(google::protobuf::Message *out) const {
  bayesmix::MultiLSState state_;
  bayesmix::to_proto(state.mean, state_.mutable_mean());
  bayesmix::to_proto(state.prec, state_.mutable_prec());
  auto *out_cast = google::protobuf::internal::down_cast<
      bayesmix::MarginalState::ClusterState *>(out);
  out_cast->mutable_multi_ls_state()->CopyFrom(state_);
  out_cast->set_cardinality(card);
}

void NNWHierarchy::write_hypers_to_proto(
    google::protobuf::Message *out) const {
  bayesmix::NNWPrior hypers_;
  bayesmix::to_proto(hypers->mean,
                     hypers_.mutable_fixed_values()->mutable_mean());
  hypers_.mutable_fixed_values()->set_var_scaling(hypers->var_scaling);
  hypers_.mutable_fixed_values()->set_deg_free(hypers->deg_free);
  bayesmix::to_proto(hypers->scale,
                     hypers_.mutable_fixed_values()->mutable_scale());

  google::protobuf::internal::down_cast<bayesmix::NNWPrior *>(out)
      ->mutable_fixed_values()
      ->CopyFrom(hypers_.fixed_values());
}


// Standard Libraries
#include <iostream>

// 3rd Parties Libraries
//PCL
#include <pcl/common/common_headers.h>

//Eigen
#include <Eigen/Dense>

//Local Libraries
#include <point_cloud_utils.h>
#include <distance_geometry_registration.h>

int main(int argc, char** argv)	{
	if(argc < 4)	{
		std::cerr << "Usage: distance_geometry_registration <SourceCorrespondence> <SourcePointCloud> <TargetCorrespondence>\n";
		return -1;
	}

	pcl::PointCloud<pcl::PointXYZRGB>::Ptr sourceCorrespondence(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr sourcePointCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr targetCorrespondence(new pcl::PointCloud<pcl::PointXYZRGB>);

	// Import Point Cloud
	importPointCloud<pcl::PointXYZRGB>(argv[1], sourceCorrespondence);
	importPointCloud<pcl::PointXYZRGB>(argv[2], sourcePointCloud);
	importPointCloud<pcl::PointXYZRGB>(argv[3], targetCorrespondence);

	DistanceGeometryRegistration<pcl::PointXYZRGB>* dgr = new DistanceGeometryRegistration<pcl::PointXYZRGB>();
	dgr->pointCloudRegistration(sourceCorrespondence, targetCorrespondence, sourcePointCloud, THREAD_MODE::THREAD_DISABLE);
	std::cout << "Transformation Error: " << dgr->estimateError(sourceCorrespondence, targetCorrespondence);

	exportPointCloud<pcl::PointXYZRGB>("transformatedPointCloud.txt", sourcePointCloud);

	delete dgr;
	return 0;
}


#include <stan/math/mix/scal.hpp>
#include <gtest/gtest.h>
#include <boost/math/special_functions/digamma.hpp>
#include <test/unit/math/rev/scal/fun/util.hpp>
#include <test/unit/math/mix/scal/fun/nan_util.hpp>

TEST(AgradFwdBinomialCoefficientLog,FvarVar_Double_1stDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using stan::math::binomial_coefficient_log;
  using boost::math::digamma;

  fvar<var> x(2004.0,1.0);
  double z(1002);
  fvar<var> a = binomial_coefficient_log(x,z);

  EXPECT_FLOAT_EQ(binomial_coefficient_log(2004.0,1002.0), a.val_.val());
  EXPECT_FLOAT_EQ(0.69289774, a.d_.val());

  AVEC y = createAVEC(x.val_);
  VEC g;
  a.val_.grad(y,g);
  EXPECT_FLOAT_EQ(0.69289774, g[0]);
}
TEST(AgradFwdBinomialCoefficientLog,Double_FvarVar_1stDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using stan::math::binomial_coefficient_log;
  using boost::math::digamma;

  double x(2004.0);
  fvar<var> z(1002.0,2.0);
  fvar<var> a = binomial_coefficient_log(x,z);

  EXPECT_FLOAT_EQ(binomial_coefficient_log(2004.0,1002.0), a.val_.val());
  EXPECT_NEAR(0, a.d_.val(),1e-8);

  AVEC y = createAVEC(z.val_);
  VEC g;
  a.val_.grad(y,g);
  EXPECT_NEAR(0, g[0],1e-8);
}
TEST(AgradFwdBinomialCoefficientLog,FvarVar_Double_2ndDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using stan::math::binomial_coefficient_log;
  using boost::math::digamma;

  fvar<var> x(2004.0,1.0);
  double z(1002);
  fvar<var> a = binomial_coefficient_log(x,z);

  EXPECT_FLOAT_EQ(binomial_coefficient_log(2004.0,1002.0), a.val_.val());
  EXPECT_FLOAT_EQ(0.69289774, a.d_.val());

  AVEC y = createAVEC(x.val_);
  VEC g;
  a.d_.grad(y,g);
  EXPECT_FLOAT_EQ(-0.00049862865, g[0]);
}
TEST(AgradFwdBinomialCoefficientLog,Double_FvarVar_2ndDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using stan::math::binomial_coefficient_log;
  using boost::math::digamma;

  double x(2004.0);
  fvar<var> z(1002.0,2.0);
  fvar<var> a = binomial_coefficient_log(x,z);

  EXPECT_FLOAT_EQ(binomial_coefficient_log(2004.0,1002.0), a.val_.val());
  EXPECT_NEAR(0, a.d_.val(),1e-8);

  AVEC y = createAVEC(z.val_);
  VEC g;
  a.d_.grad(y,g);
  EXPECT_FLOAT_EQ(-0.00399002460681026, g[0]);
}

TEST(AgradFwdBinomialCoefficientLog,FvarVar_FvarVar_1stDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using stan::math::binomial_coefficient_log;
  using boost::math::digamma;

  fvar<var> x(2004.0,1.0);
  fvar<var> z(1002.0,2.0);
  fvar<var> a = binomial_coefficient_log(x,z);

  EXPECT_FLOAT_EQ(binomial_coefficient_log(2004.0,1002.0), a.val_.val());
  EXPECT_FLOAT_EQ(0.69289774, a.d_.val());

  AVEC y = createAVEC(x.val_,z.val_);
  VEC g;
  a.val_.grad(y,g);
  EXPECT_FLOAT_EQ(0.69289774, g[0]);
  EXPECT_NEAR(0, g[1],1e-8);
}
TEST(AgradFwdBinomialCoefficientLog,FvarVar_FvarVar_2ndDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using stan::math::binomial_coefficient_log;
  using boost::math::digamma;

  fvar<var> x(2004.0,1.0);
  fvar<var> z(1002.0,2.0);
  fvar<var> a = binomial_coefficient_log(x,z);

  EXPECT_FLOAT_EQ(binomial_coefficient_log(2004.0,1002.0), a.val_.val());
  EXPECT_FLOAT_EQ(0.69289774, a.d_.val());

  AVEC y = createAVEC(x.val_,z.val_);
  VEC g;
  a.d_.grad(y,g);
  EXPECT_FLOAT_EQ(0.0014963837, g[0]);
  EXPECT_FLOAT_EQ(-0.0029925184551076781, g[1]);
}

TEST(AgradFwdBinomialCoefficientLog,FvarFvarVar_FvarFvarVar_1stDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using stan::math::binomial_coefficient_log;
  using stan::math::binomial_coefficient_log;

  fvar<fvar<var> > x;
  x.val_.val_ = 2004.0;
  x.val_.d_ = 1.0;

  fvar<fvar<var> > y;
  y.val_.val_ = 1002.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = binomial_coefficient_log(x,y);

  EXPECT_FLOAT_EQ(binomial_coefficient_log(2004.0,1002.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(0.69289774, a.val_.d_.val());
  EXPECT_NEAR(0, a.d_.val_.val(),1e-8);
  EXPECT_FLOAT_EQ(0.0009975062, a.d_.d_.val());

  AVEC p = createAVEC(x.val_.val_,y.val_.val_);
  VEC g;
  a.val_.val_.grad(p,g);
  EXPECT_FLOAT_EQ(0.69289774, g[0]);
  EXPECT_NEAR(0, g[1],1e-8);
}
TEST(AgradFwdBinomialCoefficientLog,FvarFvarVar_Double_1stDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using stan::math::binomial_coefficient_log;
  using stan::math::binomial_coefficient_log;

  fvar<fvar<var> > x;
  x.val_.val_ = 2004.0;
  x.val_.d_ = 1.0;

  double y(1002.0);

  fvar<fvar<var> > a = binomial_coefficient_log(x,y);

  EXPECT_FLOAT_EQ(binomial_coefficient_log(2004.0,1002.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(0.69289774, a.val_.d_.val());
  EXPECT_NEAR(0, a.d_.val_.val(),1e-8);
  EXPECT_FLOAT_EQ(0, a.d_.d_.val());

  AVEC p = createAVEC(x.val_.val_);
  VEC g;
  a.val_.val_.grad(p,g);
  EXPECT_FLOAT_EQ(0.69289774, g[0]);
}
TEST(AgradFwdBinomialCoefficientLog,Double_FvarFvarVar_1stDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using stan::math::binomial_coefficient_log;
  using stan::math::binomial_coefficient_log;

  double x(2004.0);

  fvar<fvar<var> > y;
  y.val_.val_ = 1002.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = binomial_coefficient_log(x,y);

  EXPECT_FLOAT_EQ(binomial_coefficient_log(2004.0,1002.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(0, a.val_.d_.val());
  EXPECT_NEAR(0, a.d_.val_.val(),1e-8);
  EXPECT_FLOAT_EQ(0, a.d_.d_.val());

  AVEC p = createAVEC(y.val_.val_);
  VEC g;
  a.val_.val_.grad(p,g);
  EXPECT_NEAR(0, g[0],1e-8);
}

TEST(AgradFwdBinomialCoefficientLog,FvarFvarVar_FvarFvarVar_2ndDeriv_x) {
  using stan::math::fvar;
  using stan::math::var;
  using stan::math::binomial_coefficient_log;
  using stan::math::binomial_coefficient_log;

  fvar<fvar<var> > x;
  x.val_.val_ = 2004.0;
  x.val_.d_ = 1.0;

  fvar<fvar<var> > y;
  y.val_.val_ = 1002.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = binomial_coefficient_log(x,y);

  EXPECT_FLOAT_EQ(binomial_coefficient_log(2004.0,1002.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(0.69289774, a.val_.d_.val());
  EXPECT_NEAR(0, a.d_.val_.val(),1e-8);
  EXPECT_FLOAT_EQ(0.0009975062, a.d_.d_.val());

  AVEC p = createAVEC(x.val_.val_,y.val_.val_);
  VEC g;
  a.val_.d_.grad(p,g);
  EXPECT_FLOAT_EQ(-0.00049862865, g[0]);
  EXPECT_FLOAT_EQ(0.00099750615170258105, g[1]);
}
TEST(AgradFwdBinomialCoefficientLog,FvarFvarVar_FvarFvarVar_2ndDeriv_y) {
  using stan::math::fvar;
  using stan::math::var;
  using stan::math::binomial_coefficient_log;
  using stan::math::binomial_coefficient_log;

  fvar<fvar<var> > x;
  x.val_.val_ = 2004.0;
  x.val_.d_ = 1.0;

  fvar<fvar<var> > y;
  y.val_.val_ = 1002.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = binomial_coefficient_log(x,y);

  EXPECT_FLOAT_EQ(binomial_coefficient_log(2004.0,1002.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(0.69289774, a.val_.d_.val());
  EXPECT_NEAR(0, a.d_.val_.val(),1e-8);
  EXPECT_FLOAT_EQ(0.0009975062, a.d_.d_.val());

  AVEC p = createAVEC(x.val_.val_,y.val_.val_);
  VEC g;
  a.d_.val_.grad(p,g);
  EXPECT_FLOAT_EQ(0.0009975062, g[0]);
  EXPECT_FLOAT_EQ(-0.0019950123034051291, g[1]);
}
TEST(AgradFwdBinomialCoefficientLog,Double_FvarFvarVar_2ndDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using stan::math::binomial_coefficient_log;
  using stan::math::binomial_coefficient_log;

  double x(2004.0);

  fvar<fvar<var> > y;
  y.val_.val_ = 1002.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = binomial_coefficient_log(x,y);

  EXPECT_FLOAT_EQ(binomial_coefficient_log(2004.0,1002.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(0, a.val_.d_.val());
  EXPECT_NEAR(0, a.d_.val_.val(),1e-8);
  EXPECT_FLOAT_EQ(0, a.d_.d_.val());

  AVEC p = createAVEC(y.val_.val_);
  VEC g;
  a.d_.val_.grad(p,g);
  EXPECT_NEAR(-0.00199501230340513, g[0],1e-8);
}
TEST(AgradFwdBinomialCoefficientLog,FvarFvarVar_Double_2ndDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using stan::math::binomial_coefficient_log;
  using stan::math::binomial_coefficient_log;

  fvar<fvar<var> > x;
  x.val_.val_ = 2004.0;
  x.val_.d_ = 1.0;

  double y(1002.0);

  fvar<fvar<var> > a = binomial_coefficient_log(x,y);

  EXPECT_FLOAT_EQ(binomial_coefficient_log(2004.0,1002.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(0.69289774, a.val_.d_.val());
  EXPECT_NEAR(0, a.d_.val_.val(),1e-8);
  EXPECT_FLOAT_EQ(0, a.d_.d_.val());

  AVEC p = createAVEC(x.val_.val_);
  VEC g;
  a.val_.d_.grad(p,g);
  EXPECT_FLOAT_EQ(-0.00049862863648177515, g[0]);
}
TEST(AgradFwdBinomialCoefficientLog,FvarFvarVar_FvarFvarVar_3rdDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using stan::math::binomial_coefficient_log;
  using stan::math::binomial_coefficient_log;

  fvar<fvar<var> > x;
  x.val_.val_ = 2004.0;
  x.val_.d_ = 1.0;

  fvar<fvar<var> > y;
  y.val_.val_ = 1002.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = binomial_coefficient_log(x,y);

  EXPECT_FLOAT_EQ(binomial_coefficient_log(2004.0,1002.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(0.69289774, a.val_.d_.val());
  EXPECT_NEAR(0, a.d_.val_.val(),1e-8);
  EXPECT_FLOAT_EQ(0.0009975062, a.d_.d_.val());

  AVEC p = createAVEC(x.val_.val_,y.val_.val_);
  VEC g;
  a.d_.d_.grad(p,g);
  EXPECT_FLOAT_EQ(-9.9501847e-07, g[0]);
  EXPECT_FLOAT_EQ(9.9501847e-07, g[1]);
}
TEST(AgradFwdBinomialCoefficientLog,Double_FvarFvarVar_3rdDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using stan::math::binomial_coefficient_log;
  using stan::math::binomial_coefficient_log;

  double x(2004.0);

  fvar<fvar<var> > y;
  y.val_.val_ = 1002.0;
  y.d_.val_ = 1.0;
  y.val_.d_ = 1.0;

  fvar<fvar<var> > a = binomial_coefficient_log(x,y);

  EXPECT_FLOAT_EQ(binomial_coefficient_log(2004.0,1002.0), a.val_.val_.val());
  EXPECT_NEAR(0, a.val_.d_.val(),1e-8);
  EXPECT_NEAR(0, a.d_.val_.val(),1e-8);
  EXPECT_FLOAT_EQ(-0.0019950124, a.d_.d_.val());

  AVEC p = createAVEC(y.val_.val_);
  VEC g;
  a.d_.d_.grad(p,g);
  EXPECT_NEAR(0, g[0],1e-8);
}
TEST(AgradFwdBinomialCoefficientLog,FvarFvarVar_Double_3rdDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using stan::math::binomial_coefficient_log;
  using stan::math::binomial_coefficient_log;

  fvar<fvar<var> > x;
  x.val_.val_ = 2004.0;
  x.val_.d_ = 1.0;
  x.d_.val_ = 1.0;

  double y(1002.0);

  fvar<fvar<var> > a = binomial_coefficient_log(x,y);

  EXPECT_FLOAT_EQ(binomial_coefficient_log(2004.0,1002.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(0.69289774, a.val_.d_.val());
  EXPECT_FLOAT_EQ(0.69289774181268948, a.d_.val_.val());
  EXPECT_FLOAT_EQ(-0.00049862865, a.d_.d_.val());

  AVEC p = createAVEC(x.val_.val_);
  VEC g;
  a.d_.d_.grad(p,g);
  EXPECT_FLOAT_EQ(7.4613968e-07, g[0]);
}

struct binomial_coefficient_log_fun {
  template <typename T0, typename T1>
  inline
  typename boost::math::tools::promote_args<T0,T1>::type
  operator()(const T0 arg1,
             const T1 arg2) const {
    return binomial_coefficient_log(arg1,arg2);
  }
};

TEST(AgradFwdBinomialCoefficientLog, nan) {
  binomial_coefficient_log_fun binomial_coefficient_log_;
  test_nan_mix(binomial_coefficient_log_,3.0,5.0,false);
}


// boost\math\distributions\poisson.hpp

// Copyright John Maddock 2006.
// Copyright Paul A. Bristow 2007.

// 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)

// Poisson distribution is a discrete probability distribution.
// It expresses the probability of a number (k) of
// events, occurrences, failures or arrivals occurring in a fixed time,
// assuming these events occur with a known average or mean rate (lambda)
// and are independent of the time since the last event.
// The distribution was discovered by Simeon-Denis Poisson (1781-1840).

// Parameter lambda is the mean number of events in the given time interval.
// The random variate k is the number of events, occurrences or arrivals.
// k argument may be integral, signed, or unsigned, or floating point.
// If necessary, it has already been promoted from an integral type.

// Note that the Poisson distribution
// (like others including the binomial, negative binomial & Bernoulli)
// is strictly defined as a discrete function:
// only integral values of k are envisaged.
// However because the method of calculation uses a continuous gamma function,
// it is convenient to treat it as if a continous function,
// and permit non-integral values of k.
// To enforce the strict mathematical model, users should use floor or ceil functions
// on k outside this function to ensure that k is integral.

// See http://en.wikipedia.org/wiki/Poisson_distribution
// http://documents.wolfram.com/v5/Add-onsLinks/StandardPackages/Statistics/DiscreteDistributions.html

#ifndef BOOST_MATH_SPECIAL_POISSON_HPP
#define BOOST_MATH_SPECIAL_POISSON_HPP

#include <boost/math/distributions/fwd.hpp>
#include <boost/math/special_functions/gamma.hpp> // for incomplete gamma. gamma_q
#include <boost/math/special_functions/trunc.hpp> // for incomplete gamma. gamma_q
#include <boost/math/distributions/complement.hpp> // complements
#include <boost/math/distributions/detail/common_error_handling.hpp> // error checks
#include <boost/math/special_functions/fpclassify.hpp> // isnan.
#include <boost/math/special_functions/factorials.hpp> // factorials.
#include <boost/math/tools/roots.hpp> // for root finding.
#include <boost/math/distributions/detail/inv_discrete_quantile.hpp>

#include <utility>

namespace boost
{
  namespace math
  {
     namespace detail{
      template <class Dist>
      inline typename Dist::value_type
         inverse_discrete_quantile(
            const Dist& dist,
            const typename Dist::value_type& p,
            const typename Dist::value_type& guess,
            const typename Dist::value_type& multiplier,
            const typename Dist::value_type& adder,
            const policies::discrete_quantile<policies::integer_round_nearest>&,
            boost::uintmax_t& max_iter);
      template <class Dist>
      inline typename Dist::value_type
         inverse_discrete_quantile(
            const Dist& dist,
            const typename Dist::value_type& p,
            const typename Dist::value_type& guess,
            const typename Dist::value_type& multiplier,
            const typename Dist::value_type& adder,
            const policies::discrete_quantile<policies::integer_round_up>&,
            boost::uintmax_t& max_iter);
      template <class Dist>
      inline typename Dist::value_type
         inverse_discrete_quantile(
            const Dist& dist,
            const typename Dist::value_type& p,
            const typename Dist::value_type& guess,
            const typename Dist::value_type& multiplier,
            const typename Dist::value_type& adder,
            const policies::discrete_quantile<policies::integer_round_down>&,
            boost::uintmax_t& max_iter);
      template <class Dist>
      inline typename Dist::value_type
         inverse_discrete_quantile(
            const Dist& dist,
            const typename Dist::value_type& p,
            const typename Dist::value_type& guess,
            const typename Dist::value_type& multiplier,
            const typename Dist::value_type& adder,
            const policies::discrete_quantile<policies::integer_round_outwards>&,
            boost::uintmax_t& max_iter);
      template <class Dist>
      inline typename Dist::value_type
         inverse_discrete_quantile(
            const Dist& dist,
            const typename Dist::value_type& p,
            const typename Dist::value_type& guess,
            const typename Dist::value_type& multiplier,
            const typename Dist::value_type& adder,
            const policies::discrete_quantile<policies::integer_round_inwards>&,
            boost::uintmax_t& max_iter);
      template <class Dist>
      inline typename Dist::value_type
         inverse_discrete_quantile(
            const Dist& dist,
            const typename Dist::value_type& p,
            const typename Dist::value_type& guess,
            const typename Dist::value_type& multiplier,
            const typename Dist::value_type& adder,
            const policies::discrete_quantile<policies::real>&,
            boost::uintmax_t& max_iter);
     }
    namespace poisson_detail
    {
      // Common error checking routines for Poisson distribution functions.
      // These are convoluted, & apparently redundant, to try to ensure that
      // checks are always performed, even if exceptions are not enabled.

      template <class RealType, class Policy>
      inline bool check_mean(const char* function, const RealType& mean, RealType* result, const Policy& pol)
      {
        if(!(boost::math::isfinite)(mean) || (mean < 0))
        {
          *result = policies::raise_domain_error<RealType>(
            function,
            "Mean argument is %1%, but must be >= 0 !", mean, pol);
          return false;
        }
        return true;
      } // bool check_mean

      template <class RealType, class Policy>
      inline bool check_mean_NZ(const char* function, const RealType& mean, RealType* result, const Policy& pol)
      { // mean == 0 is considered an error.
        if( !(boost::math::isfinite)(mean) || (mean <= 0))
        {
          *result = policies::raise_domain_error<RealType>(
            function,
            "Mean argument is %1%, but must be > 0 !", mean, pol);
          return false;
        }
        return true;
      } // bool check_mean_NZ

      template <class RealType, class Policy>
      inline bool check_dist(const char* function, const RealType& mean, RealType* result, const Policy& pol)
      { // Only one check, so this is redundant really but should be optimized away.
        return check_mean_NZ(function, mean, result, pol);
      } // bool check_dist

      template <class RealType, class Policy>
      inline bool check_k(const char* function, const RealType& k, RealType* result, const Policy& pol)
      {
        if((k < 0) || !(boost::math::isfinite)(k))
        {
          *result = policies::raise_domain_error<RealType>(
            function,
            "Number of events k argument is %1%, but must be >= 0 !", k, pol);
          return false;
        }
        return true;
      } // bool check_k

      template <class RealType, class Policy>
      inline bool check_dist_and_k(const char* function, RealType mean, RealType k, RealType* result, const Policy& pol)
      {
        if((check_dist(function, mean, result, pol) == false) ||
          (check_k(function, k, result, pol) == false))
        {
          return false;
        }
        return true;
      } // bool check_dist_and_k

      template <class RealType, class Policy>
      inline bool check_prob(const char* function, const RealType& p, RealType* result, const Policy& pol)
      { // Check 0 <= p <= 1
        if(!(boost::math::isfinite)(p) || (p < 0) || (p > 1))
        {
          *result = policies::raise_domain_error<RealType>(
            function,
            "Probability argument is %1%, but must be >= 0 and <= 1 !", p, pol);
          return false;
        }
        return true;
      } // bool check_prob

      template <class RealType, class Policy>
      inline bool check_dist_and_prob(const char* function, RealType mean,  RealType p, RealType* result, const Policy& pol)
      {
        if((check_dist(function, mean, result, pol) == false) ||
          (check_prob(function, p, result, pol) == false))
        {
          return false;
        }
        return true;
      } // bool check_dist_and_prob

    } // namespace poisson_detail

    template <class RealType = double, class Policy = policies::policy<> >
    class poisson_distribution
    {
    public:
      typedef RealType value_type;
      typedef Policy policy_type;

      poisson_distribution(RealType mean = 1) : m_l(mean) // mean (lambda).
      { // Expected mean number of events that occur during the given interval.
        RealType r;
        poisson_detail::check_dist(
           "boost::math::poisson_distribution<%1%>::poisson_distribution",
          m_l,
          &r, Policy());
      } // poisson_distribution constructor.

      RealType mean() const
      { // Private data getter function.
        return m_l;
      }
    private:
      // Data member, initialized by constructor.
      RealType m_l; // mean number of occurrences.
    }; // template <class RealType, class Policy> class poisson_distribution

    typedef poisson_distribution<double> poisson; // Reserved name of type double.

    // Non-member functions to give properties of the distribution.

    template <class RealType, class Policy>
    inline const std::pair<RealType, RealType> range(const poisson_distribution<RealType, Policy>& /* dist */)
    { // Range of permissible values for random variable k.
       using boost::math::tools::max_value;
       return std::pair<RealType, RealType>(static_cast<RealType>(0), max_value<RealType>()); // Max integer?
    }

    template <class RealType, class Policy>
    inline const std::pair<RealType, RealType> support(const poisson_distribution<RealType, Policy>& /* dist */)
    { // Range of supported values for random variable k.
       // This is range where cdf rises from 0 to 1, and outside it, the pdf is zero.
       using boost::math::tools::max_value;
       return std::pair<RealType, RealType>(static_cast<RealType>(0),  max_value<RealType>());
    }

    template <class RealType, class Policy>
    inline RealType mean(const poisson_distribution<RealType, Policy>& dist)
    { // Mean of poisson distribution = lambda.
      return dist.mean();
    } // mean

    template <class RealType, class Policy>
    inline RealType mode(const poisson_distribution<RealType, Policy>& dist)
    { // mode.
      BOOST_MATH_STD_USING // ADL of std functions.
      return floor(dist.mean());
    }

    //template <class RealType, class Policy>
    //inline RealType median(const poisson_distribution<RealType, Policy>& dist)
    //{ // median = approximately lambda + 1/3 - 0.2/lambda
    //  RealType l = dist.mean();
    //  return dist.mean() + static_cast<RealType>(0.3333333333333333333333333333333333333333333333)
    //   - static_cast<RealType>(0.2) / l;
    //} // BUT this formula appears to be out-by-one compared to quantile(half)
    // Query posted on Wikipedia.
    // Now implemented via quantile(half) in derived accessors.

    template <class RealType, class Policy>
    inline RealType variance(const poisson_distribution<RealType, Policy>& dist)
    { // variance.
      return dist.mean();
    }

    // RealType standard_deviation(const poisson_distribution<RealType, Policy>& dist)
    // standard_deviation provided by derived accessors.

    template <class RealType, class Policy>
    inline RealType skewness(const poisson_distribution<RealType, Policy>& dist)
    { // skewness = sqrt(l).
      BOOST_MATH_STD_USING // ADL of std functions.
      return 1 / sqrt(dist.mean());
    }

    template <class RealType, class Policy>
    inline RealType kurtosis_excess(const poisson_distribution<RealType, Policy>& dist)
    { // skewness = sqrt(l).
      return 1 / dist.mean(); // kurtosis_excess 1/mean from Wiki & MathWorld eq 31.
      // http://mathworld.wolfram.com/Kurtosis.html explains that the kurtosis excess
      // is more convenient because the kurtosis excess of a normal distribution is zero
      // whereas the true kurtosis is 3.
    } // RealType kurtosis_excess

    template <class RealType, class Policy>
    inline RealType kurtosis(const poisson_distribution<RealType, Policy>& dist)
    { // kurtosis is 4th moment about the mean = u4 / sd ^ 4
      // http://en.wikipedia.org/wiki/Curtosis
      // kurtosis can range from -2 (flat top) to +infinity (sharp peak & heavy tails).
      // http://www.itl.nist.gov/div898/handbook/eda/section3/eda35b.htm
      return 3 + 1 / dist.mean(); // NIST.
      // http://mathworld.wolfram.com/Kurtosis.html explains that the kurtosis excess
      // is more convenient because the kurtosis excess of a normal distribution is zero
      // whereas the true kurtosis is 3.
    } // RealType kurtosis

    template <class RealType, class Policy>
    RealType pdf(const poisson_distribution<RealType, Policy>& dist, const RealType& k)
    { // Probability Density/Mass Function.
      // Probability that there are EXACTLY k occurrences (or arrivals).
      BOOST_FPU_EXCEPTION_GUARD

      BOOST_MATH_STD_USING // for ADL of std functions.

      RealType mean = dist.mean();
      // Error check:
      RealType result = 0;
      if(false == poisson_detail::check_dist_and_k(
        "boost::math::pdf(const poisson_distribution<%1%>&, %1%)",
        mean,
        k,
        &result, Policy()))
      {
        return result;
      }

      // Special case of mean zero, regardless of the number of events k.
      if (mean == 0)
      { // Probability for any k is zero.
        return 0;
      }
      if (k == 0)
      { // mean ^ k = 1, and k! = 1, so can simplify.
        return exp(-mean);
      }
      return boost::math::gamma_p_derivative(k+1, mean, Policy());
    } // pdf

    template <class RealType, class Policy>
    RealType cdf(const poisson_distribution<RealType, Policy>& dist, const RealType& k)
    { // Cumulative Distribution Function Poisson.
      // The random variate k is the number of occurrences(or arrivals)
      // k argument may be integral, signed, or unsigned, or floating point.
      // If necessary, it has already been promoted from an integral type.
      // Returns the sum of the terms 0 through k of the Poisson Probability Density or Mass (pdf).

      // But note that the Poisson distribution
      // (like others including the binomial, negative binomial & Bernoulli)
      // is strictly defined as a discrete function: only integral values of k are envisaged.
      // However because of the method of calculation using a continuous gamma function,
      // it is convenient to treat it as if it is a continous function
      // and permit non-integral values of k.
      // To enforce the strict mathematical model, users should use floor or ceil functions
      // outside this function to ensure that k is integral.

      // The terms are not summed directly (at least for larger k)
      // instead the incomplete gamma integral is employed,

      BOOST_MATH_STD_USING // for ADL of std function exp.

      RealType mean = dist.mean();
      // Error checks:
      RealType result = 0;
      if(false == poisson_detail::check_dist_and_k(
        "boost::math::cdf(const poisson_distribution<%1%>&, %1%)",
        mean,
        k,
        &result, Policy()))
      {
        return result;
      }
      // Special cases:
      if (mean == 0)
      { // Probability for any k is zero.
        return 0;
      }
      if (k == 0)
      { // return pdf(dist, static_cast<RealType>(0));
        // but mean (and k) have already been checked,
        // so this avoids unnecessary repeated checks.
       return exp(-mean);
      }
      // For small integral k could use a finite sum -
      // it's cheaper than the gamma function.
      // BUT this is now done efficiently by gamma_q function.
      // Calculate poisson cdf using the gamma_q function.
      return gamma_q(k+1, mean, Policy());
    } // binomial cdf

    template <class RealType, class Policy>
    RealType cdf(const complemented2_type<poisson_distribution<RealType, Policy>, RealType>& c)
    { // Complemented Cumulative Distribution Function Poisson
      // The random variate k is the number of events, occurrences or arrivals.
      // k argument may be integral, signed, or unsigned, or floating point.
      // If necessary, it has already been promoted from an integral type.
      // But note that the Poisson distribution
      // (like others including the binomial, negative binomial & Bernoulli)
      // is strictly defined as a discrete function: only integral values of k are envisaged.
      // However because of the method of calculation using a continuous gamma function,
      // it is convenient to treat it as is it is a continous function
      // and permit non-integral values of k.
      // To enforce the strict mathematical model, users should use floor or ceil functions
      // outside this function to ensure that k is integral.

      // Returns the sum of the terms k+1 through inf of the Poisson Probability Density/Mass (pdf).
      // The terms are not summed directly (at least for larger k)
      // instead the incomplete gamma integral is employed,

      RealType const& k = c.param;
      poisson_distribution<RealType, Policy> const& dist = c.dist;

      RealType mean = dist.mean();

      // Error checks:
      RealType result = 0;
      if(false == poisson_detail::check_dist_and_k(
        "boost::math::cdf(const poisson_distribution<%1%>&, %1%)",
        mean,
        k,
        &result, Policy()))
      {
        return result;
      }
      // Special case of mean, regardless of the number of events k.
      if (mean == 0)
      { // Probability for any k is unity, complement of zero.
        return 1;
      }
      if (k == 0)
      { // Avoid repeated checks on k and mean in gamma_p.
         return -boost::math::expm1(-mean, Policy());
      }
      // Unlike un-complemented cdf (sum from 0 to k),
      // can't use finite sum from k+1 to infinity for small integral k,
      // anyway it is now done efficiently by gamma_p.
      return gamma_p(k + 1, mean, Policy()); // Calculate Poisson cdf using the gamma_p function.
      // CCDF = gamma_p(k+1, lambda)
    } // poisson ccdf

    template <class RealType, class Policy>
    inline RealType quantile(const poisson_distribution<RealType, Policy>& dist, const RealType& p)
    { // Quantile (or Percent Point) Poisson function.
      // Return the number of expected events k for a given probability p.
      static const char* function = "boost::math::quantile(const poisson_distribution<%1%>&, %1%)";
      RealType result = 0; // of Argument checks:
      if(false == poisson_detail::check_prob(
        function,
        p,
        &result, Policy()))
      {
        return result;
      }
      // Special case:
      if (dist.mean() == 0)
      { // if mean = 0 then p = 0, so k can be anything?
         if (false == poisson_detail::check_mean_NZ(
         function,
         dist.mean(),
         &result, Policy()))
        {
          return result;
        }
      }
      if(p == 0)
      {
         return 0; // Exact result regardless of discrete-quantile Policy
      }
      if(p == 1)
      {
         return policies::raise_overflow_error<RealType>(function, 0, Policy());
      }
      typedef typename Policy::discrete_quantile_type discrete_type;
      boost::uintmax_t max_iter = policies::get_max_root_iterations<Policy>();
      RealType guess, factor = 8;
      RealType z = dist.mean();
      if(z < 1)
         guess = z;
      else
         guess = boost::math::detail::inverse_poisson_cornish_fisher(z, p, RealType(1-p), Policy());
      if(z > 5)
      {
         if(z > 1000)
            factor = 1.01f;
         else if(z > 50)
            factor = 1.1f;
         else if(guess > 10)
            factor = 1.25f;
         else
            factor = 2;
         if(guess < 1.1)
            factor = 8;
      }

      return detail::inverse_discrete_quantile(
         dist,
         p,
         1-p,
         guess,
         factor,
         RealType(1),
         discrete_type(),
         max_iter);
   } // quantile

    template <class RealType, class Policy>
    inline RealType quantile(const complemented2_type<poisson_distribution<RealType, Policy>, RealType>& c)
    { // Quantile (or Percent Point) of Poisson function.
      // Return the number of expected events k for a given
      // complement of the probability q.
      //
      // Error checks:
      static const char* function = "boost::math::quantile(complement(const poisson_distribution<%1%>&, %1%))";
      RealType q = c.param;
      const poisson_distribution<RealType, Policy>& dist = c.dist;
      RealType result = 0;  // of argument checks.
      if(false == poisson_detail::check_prob(
        function,
        q,
        &result, Policy()))
      {
        return result;
      }
      // Special case:
      if (dist.mean() == 0)
      { // if mean = 0 then p = 0, so k can be anything?
         if (false == poisson_detail::check_mean_NZ(
         function,
         dist.mean(),
         &result, Policy()))
        {
          return result;
        }
      }
      if(q == 0)
      {
         return policies::raise_overflow_error<RealType>(function, 0, Policy());
      }
      if(q == 1)
      {
         return 0;  // Exact result regardless of discrete-quantile Policy
      }
      typedef typename Policy::discrete_quantile_type discrete_type;
      boost::uintmax_t max_iter = policies::get_max_root_iterations<Policy>();
      RealType guess, factor = 8;
      RealType z = dist.mean();
      if(z < 1)
         guess = z;
      else
         guess = boost::math::detail::inverse_poisson_cornish_fisher(z, RealType(1-q), q, Policy());
      if(z > 5)
      {
         if(z > 1000)
            factor = 1.01f;
         else if(z > 50)
            factor = 1.1f;
         else if(guess > 10)
            factor = 1.25f;
         else
            factor = 2;
         if(guess < 1.1)
            factor = 8;
      }

      return detail::inverse_discrete_quantile(
         dist,
         1-q,
         q,
         guess,
         factor,
         RealType(1),
         discrete_type(),
         max_iter);
   } // quantile complement.

  } // namespace math
} // namespace boost

// This include must be at the end, *after* the accessors
// for this distribution have been defined, in order to
// keep compilers that support two-phase lookup happy.
#include <boost/math/distributions/detail/derived_accessors.hpp>
#include <boost/math/distributions/detail/inv_discrete_quantile.hpp>

#endif // BOOST_MATH_SPECIAL_POISSON_HPP


/**
 * @file upwindfinitevolume.cc
 * @brief NPDE homework UpwindFiniteVolume code
 * @author Philipp Egg
 * @date 08.09.2020
 * @copyright Developed at ETH Zurich
 */

#include "upwindfinitevolume.h"

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <cmath>
#include <stdexcept>

namespace UpwindFiniteVolume {

/* SAM_LISTING_BEGIN_1 */
Eigen::Matrix<double, 2, 3> gradbarycoordinates(
    const Eigen::Matrix<double, 2, 3> &triangle) {
  Eigen::Matrix3d X;
  // Solve for the coefficients of the barycentric coordinate functions
  X.block<3, 1>(0, 0) = Eigen::Vector3d::Ones();
  X.block<3, 2>(0, 1) = triangle.transpose();
  return X.inverse().block<2, 3>(1, 0);
}
/* SAM_LISTING_END_1 */

/* SAM_LISTING_BEGIN_2 */
Eigen::Vector2d computeCircumcenters(const Eigen::Vector2d &a1,
                                     const Eigen::Vector2d &a2,
                                     const Eigen::Vector2d &a3) {
  //====================
  // Your code goes here
  //====================
  return Eigen::Vector2d::Zero();
}
/* SAM_LISTING_END_2 */

}  // namespace UpwindFiniteVolume


/**
 * \ file DerivativeFilter.cpp
 */

#include <array>
#include <fstream>

#include <ATK/config.h>

#include <ATK/Tools/DerivativeFilter.h>
#include <ATK/Mock/SimpleSinusGeneratorFilter.h>

#include <boost/math/constants/constants.hpp>

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

constexpr gsl::index PROCESSSIZE = 1000;
constexpr gsl::index SAMPLING_RATE = 2*1024*1024;
constexpr gsl::index freq = 1024;
constexpr gsl::index offset = SAMPLING_RATE / (4 * freq);

BOOST_AUTO_TEST_CASE( DerivativeFilter_const_sin1k )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(SAMPLING_RATE);
  generator.set_amplitude(1);
  generator.set_frequency(freq);

  ATK::DerivativeFilter<double> filter;
  filter.set_input_sampling_rate(SAMPLING_RATE);
  filter.set_output_sampling_rate(SAMPLING_RATE);

  filter.set_input_port(0, generator, 0);
  filter.process(PROCESSSIZE);

  auto sin = generator.get_output_array(0);
  auto array = filter.get_output_array(0);

  auto coeff = 2 * freq * boost::math::constants::pi<double>() / SAMPLING_RATE;

  for(size_t i = 1; i < PROCESSSIZE - offset; ++i)
  {
    BOOST_CHECK_CLOSE(array[i], coeff * sin[i + offset], 5);
  }
}


#include <Eigen/Geometry>
#include <pybind11/eigen.h>
#include <pybind11/pybind11.h>

namespace py = pybind11;

template <typename T>
void initEigenQuaternion(py::module &m, const char *name) {
  // TODO: Make this useful, not just a fancy tuple
  using QuaternionT = Eigen::Quaternion<T>;
  using Vector3T = Eigen::Vector3<T>;
  py::class_<QuaternionT>(m, name, py::module_local())
      .def(py::init<const T &, const T &, const T &, const T &>())
      .def_property("w", [](const QuaternionT &self) { return self.w(); },
                    [](QuaternionT &self, T val) { self.w() = val; })
      .def_property("x", [](const QuaternionT &self) { return self.x(); },
                    [](QuaternionT &self, T val) { self.x() = val; })
      .def_property("y", [](const QuaternionT &self) { return self.y(); },
                    [](QuaternionT &self, T val) { self.y() = val; })
      .def_property("z", [](const QuaternionT &self) { return self.z(); },
                    [](QuaternionT &self, T val) { self.z() = val; })
      .def("to_rotation_matrix", &QuaternionT::toRotationMatrix)
      .def_static("from_two_vectors", [](const Vector3T &a, const Vector3T &b) {
        return QuaternionT::FromTwoVectors(a, b);
      });
}

void initEigenGeometry(py::module &m) {
  initEigenQuaternion<float>(m, "Quaternionf");
  initEigenQuaternion<double>(m, "Quaterniond");
}


/**
 * @copyright   Copyright (c) 2021, Swift Engineering Inc.
 * @license     Licensed under the MIT license. See LICENSE for details.
 */
#include <gtest/gtest.h>

#include <string>
#include <sstream>
#include <boost/array.hpp>
#include <stdio.h>
#include <ignition/math/Pose3.hh>
#include <ignition/math/Vector3.hh>
#include <ignition/math/Quaternion.hh>

#include "Coordinate_Utils.hpp"

// #define ASSERT_PERCENT_NEAR(value, tolerance)

TEST(CoordUtilsTest, RotationFromBasis) {
    // Given
    ignition::math::Vector3d forward(0, 0, 1);
    ignition::math::Vector3d upward(-1, 0, 0);

    ignition::math::Vector3d aVec(0.81165, -0.90368, 9.9767);

    ignition::math::Quaterniond rotAtoB = avionics_sim::Coordinate_Utils::QuatFromBasis(forward, upward);

    ignition::math::Vector3d bVec = rotAtoB.RotateVector(aVec);

    ignition::math::Vector3d expectedBVec(-0.81165, 0.90368, 9.9767);

    for (int i = 0; i < 3; i++) {
        EXPECT_NEAR(bVec[i], expectedBVec[i], 0.01);
    }
}

struct CoordUtilsParams {
    std::string case_name;
    ignition::math::Pose3d poseInWorld;
    ignition::math::Vector3d vecToRotate;
    ignition::math::Vector3d vecExpected;
};

class CoordUtilsParamTest : public ::testing::TestWithParam<CoordUtilsParams> {
  protected:
    // Some expensive resource shared by all tests.
    // static T* shared_resource_;

    /**
     * Sets up the test suite, called before all test cases are run
     */
    static void SetUpTestSuite() {
    }

    /**
     * Tears down the test suite, called after all test cases are run
     */
    static void TearDownTestSuite() {
    }

    // You can define per-test set-up logic as usual.
    virtual void SetUp() {
    }

    // You can define per-test tear-down logic as usual.
    virtual void TearDown() {
    }
};

TEST_P(CoordUtilsParamTest, ProjectVectorGlobalTest) {
    CoordUtilsParams params = GetParam();

    ignition::math::Vector3d vecResult;
    avionics_sim::Coordinate_Utils::project_vector_global(params.poseInWorld, params.vecToRotate, &vecResult);

    for (int i = 0; i < 3; i++) {
        EXPECT_NEAR(vecResult[i], params.vecExpected[i], 0.01);
    }
}

const std::vector<CoordUtilsParams> params{
    {
        "Test 1",
        ignition::math::Pose3d(0, 0, 0, -0.09, 1.48, 0.1),
        ignition::math::Vector3d(10, 1, 0.1),
        ignition::math::Vector3d(0.81165, -0.90368, 9.97673)
    }
};

INSTANTIATE_TEST_CASE_P(CoordUtilsTest,
                        CoordUtilsParamTest,
                        ::testing::ValuesIn(params));


#include <iostream>
#include <cmath>
#include <stdlib.h>
#include <vector>

#include <Eigen/Core>
#include <gtest/gtest.h>

#include "test/utils.hpp"

#include "ldaplusplus/optimization/MultinomialLogisticRegression.hpp"
#include "ldaplusplus/optimization/GradientDescent.hpp"
#include "ldaplusplus/optimization/SecondOrderLogisticRegressionApproximation.hpp"

using namespace Eigen;
using namespace ldaplusplus::optimization;

template <typename T>
class TestSecondOrderMultinomialLogisticRegression : public ParameterizedTest<T> {};

TYPED_TEST_CASE(TestSecondOrderMultinomialLogisticRegression, ForFloatAndDouble);

/**
  * In this test we check if the gradient is correct by appling
  * a finite difference method.
  */
TYPED_TEST(TestSecondOrderMultinomialLogisticRegression, Gradient) {
    // Gradient checking should only be made with a double type
    if (is_float<TypeParam>::value) {
        return;
    }

    // eta is typically of size KxC, where K is the number of topics and C the
    // number of different classes.
    // Here we choose randomly for conviency K=10 and C=5
    MatrixX<TypeParam> eta = MatrixX<TypeParam>::Random(10, 5);
    // X is of size KxD, where D is the total number of documents.
    // In our case we have chosen D=15
    MatrixX<TypeParam> X = MatrixX<TypeParam>::Random(10, 1);
    // y is vector of size Dx1
    VectorXi y(1);
    for (int i=0; i<1; i++) {
        y(i) = rand() % (int)5;
    }
    std::vector<MatrixX<TypeParam> > X_var = {MatrixX<TypeParam>::Random(10, 10).array().abs()};

    TypeParam L = 1;
    SecondOrderLogisticRegressionApproximation<TypeParam> mlr(X, X_var, y, L);

    // grad is the gradient according to the equation
    // implemented in MultinomialLogisticRegression.cpp
    // gradient function
    // grad is of same size as eta, which is KxC
    MatrixX<TypeParam> grad(10, 5);

    // Calculate the gradients
    mlr.gradient(eta, grad);

    // Grad's approximation
    TypeParam grad_hat;
    TypeParam t = 1e-6;

    for (int i=0; i < eta.rows(); i++) {
        for (int j=0; j < eta.cols(); j++) {
            eta(i, j) += t;
            TypeParam ll1 = mlr.value(eta);
            eta(i, j) -= 2*t;
            TypeParam ll2 = mlr.value(eta);

            // Compute gradients approximation
            grad_hat = (ll1 - ll2) / (2 * t);

            auto absolute_error = std::abs(grad(i, j) - grad_hat);
            if (grad_hat != 0) {
                auto relative_error = absolute_error / std::abs(grad_hat);
                EXPECT_TRUE(
                    relative_error < 1e-4 ||
                    absolute_error < 1e-5
                ) << relative_error << " " << absolute_error;
            }
            else {
                EXPECT_LT(absolute_error, 1e-5);
            }
        }
    }
}

TYPED_TEST(TestSecondOrderMultinomialLogisticRegression, MinimizerOverfitSmall) {
    MatrixX<TypeParam> X(2, 10);
    VectorXi y(10);

    X << 0.6097662 ,  0.53395565,  0.9499446 ,  0.67289898,  0.94173948,
         0.56675891,  0.80363783,  0.85303565,  0.15903886,  0.99518533,
         0.41655682,  0.29256121,  0.36103228,  0.29899503,  0.4957268 ,
         -0.04277318, -0.28038614, -0.12334621, -0.17497722,  0.1492248;
    y << 0, 0, 0, 0, 0, 1, 1, 1, 1, 1;
    std::vector<MatrixX<TypeParam> > X_var;
    for (int i=0; i<10; i++) {
        //X_var.push_back(MatrixX<TypeParam>::Random(2, 2).array().abs() * 0.01);
        //X_var.push_back(MatrixX<TypeParam>::Zero(2, 2));
        VectorX<TypeParam> a = VectorX<TypeParam>::Random(2).array() * 0.01;
        X_var.push_back(
            a * a.transpose()
        );
    }

    SecondOrderLogisticRegressionApproximation<TypeParam> mlr(X, X_var, y, 0);
    MatrixX<TypeParam> eta = MatrixX<TypeParam>::Zero(2, 3);

    GradientDescent<SecondOrderLogisticRegressionApproximation<TypeParam>, MatrixX<TypeParam>> minimizer(
        std::make_shared<
            ArmijoLineSearch<
                SecondOrderLogisticRegressionApproximation<TypeParam>,
                MatrixX<TypeParam>
            >
        >(),
        [](TypeParam value, TypeParam gradNorm, size_t iterations) {
            return iterations < 5000;
        }
    );
    minimizer.minimize(mlr, eta);

    EXPECT_GT(0.1, mlr.value(eta));
}


//==================================================================================================
/*!
  @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_FUNCTION_NEXTPOW2_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_NEXTPOW2_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-ieee
    This function object returns the smallest integer n such that
    `saturated_(abs)(x)` is less or equal to \f$2^n\f$

    @par Header <boost/simd/function/nextpow2.hpp>

    @par Example:

      @snippet nextpow2.cpp nextpow2

    @par Possible output:

      @snippet nextpow2.txt nextpow2

  **/
  as_integer_t<Value> nextpow2(Value const& x);
} }
#endif

#include <boost/simd/function/scalar/nextpow2.hpp>
#include <boost/simd/function/simd/nextpow2.hpp>

#endif