#include <boost/assert.hpp>
#include <boost/foreach.hpp>
#include <boost/filesystem.hpp>
#include <boost/format.hpp>
//#include <boost/math/special_functions/binomial.hpp>
#include <cmath>
#include <Eigen/Dense>
#include <limits>
#include <vector>

#include <fstream>

#include "kalman_filter.hpp"
#include "kalman_filter_cache.hpp"
#include "model.hpp"
#include "partition.hpp"
#include "io.hpp"

//using boost::math::binomial_coefficient;

namespace biggles
{

namespace model
{

namespace detail
{

// log multivariate gamma function with p == 2
inline float lgamma_2(float x)
{
    // see http://en.wikipedia.org/wiki/Multivariate_gamma_function
    return 0.5f*logf(M_PI) + lgammaf(x) + lgammaf(x-0.5f);
}

// log of the inverse Wishart PDF.
inline float log_inverse_wishart_pdf(const Eigen::Matrix2f& B, const Eigen::Matrix2f& Phi, int m)
{
    // see http://en.wikipedia.org/wiki/Inverse-Wishart_distribution

    float log_det_B = logf(B.determinant());
    float log_det_Phi = logf(Phi.determinant());
    float trace_Phi_B_inv = (Phi * B.inverse()).trace();

    return 0.5f*m*log_det_Phi - 0.5f*(m+2+1)*log_det_B - 0.5f*trace_Phi_B_inv - 0.5f*(2*m)*logf(2.f) - lgamma_2(0.5f*m);
}

}

/// \brief calculates p(partition | clutter rate)
///
/// This function seems to be uncontroversial
float log_partition_given_clutter_rate_density(const partition& part_sample, const model::parameters& para_sample) {
    const time_stamp part_first_ts = part_sample.first_time_stamp();
    const time_stamp part_last_ts = part_sample.last_time_stamp();
    const time_stamp part_duration = part_last_ts - part_first_ts;
    /*
    std::vector<size_t> clutter_counts(part_duration);
    BOOST_FOREACH(const observation& obs, part_sample.clutter()) {
        ++clutter_counts[t(obs) - part_first_ts];
    }
    */
    float lambda_f = clutter_rate(para_sample);
    /*
    if (not std::isfinite(lambda_f)) {
        std::stringstream sstr;
        sstr << "clutter rate is not finite: " << lambda_f;
        throw std::runtime_error(sstr.str()) ;
    }
    */
    BOOST_ASSERT(std::isfinite(lambda_f));
    BOOST_ASSERT(lambda_f >= 0);
    float log_part_clutter = part_sample.clutter().size() * logf(lambda_f) - part_duration * lambda_f;
    for (clutter_t::const_iterator it = part_sample.clutter().begin(); it not_eq part_sample.clutter().end(); ++it) {
        log_part_clutter += -lgammaf(1.f + it->second.size());
    }
    return log_part_clutter;
}

float log_partition_given_survival_prob_density(const partition& part_sample, const model::parameters& para_sample) {
    binomial_coefficient& binom_coeff = binomial_coefficient::get();
    const time_stamp part_first_ts = part_sample.first_time_stamp();
    const time_stamp part_last_ts = part_sample.last_time_stamp();
    const time_stamp part_duration = part_last_ts - part_first_ts;
    const int min_surv = 0; // number of guaranteed survivals
    std::vector<size_t> survival_events(part_duration);
    std::vector<size_t> death_events(part_duration);
    BOOST_FOREACH(const boost::shared_ptr<const track>& t_ptr, part_sample.tracks()) {
        BOOST_ASSERT(t_ptr->duration() > min_surv);
        time_stamp track_first_ts = t_ptr->first_time_stamp();
        time_stamp track_last_ts = t_ptr->last_time_stamp();
        for (time_stamp ts = track_first_ts - part_first_ts + 1 + min_surv; ts < track_last_ts - part_first_ts; ++ts)
            survival_events[ts]++;
        if (track_last_ts < part_last_ts)
            death_events[track_last_ts - part_first_ts]++;
    }
    float log_part_surv = 0.f;
    float log_p_s = logf(survival_probability(para_sample));
    float log_1m_p_s = logf(1.f - survival_probability(para_sample));
    for ( time_stamp i = 0; i < part_duration; ++i) {
        log_part_surv += logf(binom_coeff(survival_events[i] + death_events[i], survival_events[i])) +
            survival_events[i] * log_p_s + death_events[i] * log_1m_p_s;
    }
    return log_part_surv;
}

float log_partition_given_observation_prob_density(const partition& part_sample, const model::parameters& para_sample) {
    binomial_coefficient& binom_coeff = binomial_coefficient::get();
    const time_stamp part_first_ts = part_sample.first_time_stamp();
    const time_stamp part_last_ts = part_sample.last_time_stamp();
    const time_stamp part_duration = part_last_ts - part_first_ts;
    const size_t min_obs = 0; //number of guaranteed observations
    std::vector<size_t> track_events(part_duration);
    std::vector<size_t> observation_events(part_duration);
    BOOST_FOREACH(const boost::shared_ptr<const track>& t_ptr, part_sample.tracks()) {
        time_stamp track_first_ts = t_ptr->first_time_stamp();
        time_stamp track_last_ts = t_ptr->last_time_stamp();
        for (time_stamp ts = track_first_ts - part_first_ts; ts < track_last_ts - part_first_ts; ++ts)
            track_events[ts]++;
        BOOST_ASSERT(t_ptr->observations().size() >= min_obs);
        observation_collection::const_iterator obs_iter = t_ptr->observations().begin();
        for (size_t i = 0; i < min_obs; ++i) {
            track_events[t(*obs_iter) - part_first_ts]--;
            obs_iter++;
        }
        for (; obs_iter != t_ptr->observations().end(); ++obs_iter)
            observation_events[t(*obs_iter) - part_first_ts]++;
    }
    float log_part_obs = 0.f;
    float log_p_o = logf(observation_probability(para_sample));
    float log_1m_p_o = logf(1.f - observation_probability(para_sample));
    for (time_stamp i = 0; i < part_duration; ++i) {
        log_part_obs += logf(binom_coeff(track_events[i], observation_events[i])) +
            observation_events[i] * log_p_o + (track_events[i] - observation_events[i]) * log_1m_p_o;
    }
    return log_part_obs;
}

float log_partition_given_birth_rate_density(const partition& part_sample, const model::parameters& para_sample) {
    const time_stamp part_first_ts = part_sample.first_time_stamp();
    const time_stamp part_last_ts = part_sample.last_time_stamp();
    const time_stamp part_duration = part_last_ts - part_first_ts;
    const int min_surv = 0; // number of guaranteed survivals
    std::vector<size_t> birth_events(part_duration);
    BOOST_FOREACH(const boost::shared_ptr<const track>& t_ptr, part_sample.tracks()) {
        BOOST_ASSERT(t_ptr->duration() > min_surv);
        time_stamp track_first_ts = t_ptr->first_time_stamp();
        birth_events[track_first_ts - part_first_ts]++;
    }
    float lambda_b = birth_rate(para_sample);
    size_t n_total_born(part_sample.tracks().size());
    float log_part_birth = n_total_born * logf(lambda_b) - (part_duration - min_surv) * lambda_b;
    // TODO: the value for lgammaf may be stored and recalled rather than recalculated; could be faster.
    for (time_stamp i = 0; i < part_duration; ++i)
        log_part_birth += -lgammaf(1.f + birth_events[i]);
    return log_part_birth;
}

float log_partition_given_parameters_density_orig(const partition& partition, const model::parameters& parameters)
{
    float log_pdf = 0.f;

    off_t n_frames = partition.duration();

    // ensemble statistics over all time stamps
    off_t n_total_born(partition.tracks().size());
    off_t n_total_false(partition.clutter().size());
    off_t n_total_died(partition.tracks().size()); // everything that has a beginning has an end, Neo.
    off_t n_total_survived(0);
    off_t n_total_generated(0);

    // calculate n_total_survived and n_total_generated. record birth times for tracks
    std::vector<size_t> birth_counts(partition.last_time_stamp() - partition.first_time_stamp());
    BOOST_FOREACH(const boost::shared_ptr<const track>& t_ptr, partition.tracks())
    {
        BOOST_ASSERT(t_ptr->duration() > 1);
        // a track 'survives' for one fewer ticks than it's duration
        n_total_survived += t_ptr->duration() - 1;

        // the track's size is the number of observations within it
        n_total_generated += t_ptr->size();

        // record birth time
        BOOST_ASSERT(t_ptr->first_time_stamp() >= partition.first_time_stamp());
        BOOST_ASSERT(t_ptr->first_time_stamp() - partition.first_time_stamp() < static_cast<time_stamp>(birth_counts.size()));
        ++birth_counts[t_ptr->first_time_stamp() - partition.first_time_stamp()];
    }

    // record clutter counts
    std::vector<size_t> clutter_counts(partition.last_time_stamp() - partition.first_time_stamp());
    for (clutter_t::const_iterator it = partition.clutter().begin(); it not_eq partition.clutter().end(); ++it) {
        clutter_counts[it->first - partition.first_time_stamp()] = it->second.size();
    }

    // p_s term
    float p_s = frame_to_frame_survival_probability(parameters);
    BOOST_ASSERT(std::isfinite(p_s) and p_s >= 0);
    log_pdf += n_total_survived * logf(p_s) + n_total_died * logf(1.f - p_s);

    // p_d term
    float p_d = generate_observation_probability(parameters);
    BOOST_ASSERT(std::isfinite(p_d) and p_d >= 0);
    log_pdf += n_total_generated * logf(p_d) + (n_total_survived + n_total_born - n_total_generated) * logf(1.f - p_d);

    // start of lambda_b term
    float lambda_b = mean_new_tracks_per_frame(parameters);
    BOOST_ASSERT(std::isfinite(lambda_b) and lambda_b >= 0);
    log_pdf += n_total_born * logf(lambda_b) - n_frames * lambda_b; // ... still to calculate log gamma terms

    // start of lambda_f term
    float lambda_f = mean_false_observations_per_frame(parameters);
    if (not std::isfinite(lambda_f)) {
        std::stringstream sstr;
        sstr << "clutter rate is not finite: " << lambda_f;
        throw std::runtime_error(sstr.str()) ; // TODO debug cleaning up
    }
    BOOST_ASSERT(std::isfinite(lambda_f));
    BOOST_ASSERT(lambda_f >= 0);
    log_pdf += n_total_false * logf(lambda_f) - n_frames * lambda_f; // ... still to calculate log gamma terms

    // finish lambda_b and lambda_f terms
    for(time_stamp ts = partition.first_time_stamp(); ts < partition.last_time_stamp(); ++ts)
    {
        // finish lambda_b term
        log_pdf += -lgammaf(1.f + birth_counts[ts - partition.first_time_stamp()]);

        // finish lambda_f term
        log_pdf += -lgammaf(1.f + clutter_counts[ts - partition.first_time_stamp()]);
    }

    return log_pdf; // the rest is just a repeat of P(T|clutter_rate)

}

float log_partition_given_parameters_density(
        const partition& partition_sample, const model::parameters& parameter_sample)
{
    return log_partition_given_parameters_density_orig(partition_sample, parameter_sample);
}

float log_clutter_given_parameters_density(const partition& part, const model::parameters& parameters) {
    return -logf(part.volume())*part.clutter().size();
}

float log_track_given_parameters_density(const boost::shared_ptr<const track>& track_p, const model::parameters& parameters)
{
    return track_p->log_posterior(parameters);
}

float log_parameters_prior_density(const model::parameters& parameters)
{
    // finite regions of support:
    if(mean_new_tracks_per_frame(parameters) <= 0.f)
        return -std::numeric_limits<float>::max();
    if(mean_false_observations_per_frame(parameters) <= 0.f)
        return -std::numeric_limits<float>::max();
    if(frame_to_frame_survival_probability(parameters) < 0.f)
        return -std::numeric_limits<float>::max();
    if(frame_to_frame_survival_probability(parameters) > 1.f)
        return -std::numeric_limits<float>::max();
    if(generate_observation_probability(parameters) < 0.f)
        return -std::numeric_limits<float>::max();
    if(generate_observation_probability(parameters) > 1.f)
        return -std::numeric_limits<float>::max();

    /*
    if (mean_false_observations_per_frame(parameters) != 0.1f) { // FIXME PRIOR calculation
        return -std::numeric_limits<float>::max();
    }
    if (mean_new_tracks_per_frame(parameters) != .5f) { // FIXME PRIOR calculation
        return -std::numeric_limits<float>::max();
    }
    if (generate_observation_probability(parameters) != .9f) { // FIXME PRIOR calculation
        return -std::numeric_limits<float>::max();
    }
    if (frame_to_frame_survival_probability(parameters) != .9f) { // FIXME PRIOR calculation
        return -std::numeric_limits<float>::max();
    }
    */
    // prior on R:
    // FIXME PRIOR calculation
    /*
    if ( observation_error_covariance(parameters) == Eigen::Matrix2f::Identity()* 0.09f ) {
        return 0.f;
    } else {
        return -std::numeric_limits<float>::max();
    }
    */
    return detail::log_inverse_wishart_pdf(observation_error_covariance(parameters), 2.f * Eigen::Matrix2f::Identity(), 5);
}

float log_likelihood(const partition& part_sample, const model::parameters& para_sample) {
    float log_pdf(0.f);

    // track PDFs
    BOOST_FOREACH(const boost::shared_ptr<const track>& t_ptr, part_sample.tracks())
    {
        log_pdf += log_track_given_parameters_density(t_ptr, para_sample);
    }

    // clutter PDF
    log_pdf += log_clutter_given_parameters_density(part_sample, para_sample);
    return log_pdf;
}

float log_tracks_given_parameters_density(const partition& part, const model::parameters& parameters) {
    // track PDFs
    float log_pdf(0.f);
    BOOST_FOREACH(const boost::shared_ptr<const track>& t_ptr, part.tracks())
    {
        float track_log_pdf = log_track_given_parameters_density(t_ptr, parameters);
        if (not std::isfinite(track_log_pdf)) {
            std::cerr << "track duration " << t_ptr->duration() << std::endl;
            std::cerr << "track size " << t_ptr->size() << std::endl;
        }
        log_pdf += track_log_pdf;
    }
    return log_pdf;
}

void dump_part_para(const partition& part, const model::parameters& para) {
    size_t ctr = 0;
    std::string fname = boost::str(boost::format("no_finite_log_pdf.%04d.json") % ctr);
    while (boost::filesystem::exists(fname)) {
        ctr++;
        if (ctr > 9999) {
            std::cerr << "nothing dumped :(" << std::endl;
            return;
        }
        fname = boost::str(boost::format("no_finite_log_pdf.%04d.json") % ctr);
    }
    std::ofstream fh(fname.c_str());
    fh << "{ \"parameters\": ";
    io::write_model_parameters_to_json_stream(fh, para);
    fh << ", \"partition\": " << std::endl;
    io::write_partition_to_json_stream(fh, part);
    fh << "}" << std::endl;
}

float log_partition_given_parameters_and_data_density(const partition& part, const model::parameters& parameters)
{
    float log_pdf(0.f);
    float temp(0.f);

    // clutter PDF
    temp = log_clutter_given_parameters_density(part, parameters);
    if (not std::isfinite(temp)) {
        std::cerr << " *** clutter log density not finite" << std::endl;
        std::cerr << "     " << part.volume() << std::endl;
        dump_part_para(part, parameters);
    }
    log_pdf += temp;

    temp = log_tracks_given_parameters_density(part, parameters);
    if (not std::isfinite(temp)) {
        std::cerr << " *** track log density not finite" << std::endl;
        std::cerr << temp << std::endl;
        std::cerr << model::observation_error_covariance(parameters) << std::endl;
        dump_part_para(part, parameters);
    }
    log_pdf += temp;

    //log_pdf = 0.f; // FIXME PRIOR test get rid of this line again

    // data-independent track PDF
    temp = log_partition_given_parameters_density(part, parameters);
    if (not std::isfinite(temp)) {
        std::cerr << " *** partition log density not finite" << std::endl;
        dump_part_para(part, parameters);
    }
    log_pdf += temp;

    // prior
    // the prior is only need to record the log pdf and not for the sampling itself.
    // log_pdf += log_parameters_prior_density(parameters);

    return log_pdf;
}

}

}


#include<iostream>
#include"basis.hpp"
#include"operators.hpp"
#include"diag.h"
#include"files.hpp"
#include"timeev.hpp"
#include"ETH.hpp"
#include <boost/program_options.hpp>
int main(int argc, char *argv[])
{
  using namespace Eigen;
using namespace std;
using namespace Many_Body;
  using Fermi_HubbardBasis= TensorProduct<ElectronBasis, ElectronBasis>;
   using HolsteinBasis= TensorProduct<ElectronBasis, PhononBasis>;
  using Mat= Operators::Mat;
  using boost::program_options::value;
   size_t Llead{};
  size_t Lchain{};
  double tint{};
  double t0{};
  double tl{};
    double V{};
  double dt{};
  double tot{};
  bool PB{};
  try
  {
    boost::program_options::options_description desc{"Options"};
    desc.add_options()
      ("help,h", "Help screen")
      ("Ll", value(&Llead)->default_value(4), "Ll")
      ("Lc", value(&Lchain)->default_value(2), "Lc")
      ("t0", value(&t0)->default_value(1.), "t0")
      ("tl", value(&tl)->default_value(1.), "tl")
      ("V", value(&V)->default_value(1.), "V")
      ("tint", value(&tint)->default_value(1.), "tint")
      ("dt", value(&dt)->default_value(0.1), "dt")
      ("tot", value(&tot)->default_value(1.), "tot")
      ("pb", value(&PB)->default_value(false), "PB");

    boost::program_options::variables_map vm;
    boost::program_options::store(parse_command_line(argc, argv, desc), vm);
    boost::program_options::notify(vm);

    if (vm.count("help"))
      {std::cout << desc << '\n'; return 0;}
    else{
      if (vm.count("L"))
      {      std::cout << "L: " << vm["L"].as<size_t>() << '\n';

      }
     if (vm.count("M"))
      {
	std::cout << "M: " << vm["M"].as<size_t>() << '\n';

      }
      if (vm.count("t0"))
      {
	std::cout << "t0: " << vm["t0"].as<double>() << '\n';
      }
       if (vm.count("omg"))
      {
	std::cout << "omega: " << vm["omg"].as<double>() << '\n';
      }
       if (vm.count("V"))
      {
	std::cout << "V: " << vm["V"].as<double>() << '\n';
      }
       if (vm.count("dt"))
      {
	std::cout << "dt: " << vm["dt"].as<double>() << '\n';
      }
       if (vm.count("tot"))
      {
	std::cout << "total time: " << vm["tot"].as<double>() << '\n';
      }
       if (vm.count("pb"))
      {
	std::cout << "PB: " << vm["pb"].as<bool>() << '\n';
      }
    }
  }
  catch (const boost::program_options::error &ex)
  {
    std::cerr << ex.what() << '\n';
    return 0;
  }

  // ee[L-1]=1;

  int Ltot=2*Llead+Lchain;
  ElectronBasis e( Ltot, int(Ltot/2));

      //   std::cout<< e << std::endl;

            std::cout<< e.dim << std::endl;

  Mat E1=Operators::EKinOperator(e, tl, PB, 0, Llead-1);

   Mat E2=Operators::EKinOperator(e, tl, PB, Llead+Lchain, Ltot-1);
  Mat EI1=Operators::EKinOperator(e, tint, PB, Llead-1, Llead);
  Mat EI2=Operators::EKinOperator(e, tint, PB, Llead+Lchain-1, Llead+Lchain);
    Mat NI1=Operators::NumberOperator(e, -V/2, PB,  0, Llead-1);
  Mat NI2=Operators::NumberOperator(e, V/2, PB, Llead+Lchain, Ltot-1);
  // // Mat E4=Operators::EKinOperator(e, tl, PB, Llead+Lchain, Ltot-1);
   Mat E3=Operators::EKinOperator(e, t0, PB, Llead, Llead+Lchain-1);
   Mat C1=Operators::CurrOperator(e, tint, PB, Llead-1, Llead);
   Mat C2=Operators::CurrOperator(e, tint, PB, Llead+Lchain-1, Llead+Lchain);

       Eigen::VectorXd eigenVals1(e.dim);
       Eigen::VectorXd eigenVals2(e.dim);
       Mat H1=E1+E2+E3+ EI1+ EI2;
       Mat H2=E1+E2+E3+ EI1+ EI2 +NI2 +NI1;
       //;
      //

    Eigen::MatrixXd HH2=Eigen::MatrixXd(H2);
       Eigen::MatrixXd HH1=Eigen::MatrixXd(H1);
    //std::cout<< HH<<std::endl;
   //      Eigen::MatrixXd N=Eigen::MatrixXd(Eph);
      Many_Body::diagMat(HH1, eigenVals1);
      Many_Body::diagMat(HH2, eigenVals2);
      //      std::cout<< eigenVals[0]<< std::endl;
   //       Eigen::VectorXd energy(TP.dim);
   //  	   std::vector<double> obs(TP.dim);
   // 	   std::vector<double> obs2(TP.dim);
   // 	   std::vector<double> ensvec(TP.dim);
   // 	   Eigen::MatrixXd PHD2=HH.adjoint()*N.selfadjointView<Lower>()*HH;

      Eigen::MatrixXcd evExp=TimeEv::EigenvalExponent(eigenVals2, dt);
    Eigen::MatrixXcd cEVec=HH2.cast<std::complex<double>>();
    Eigen::VectorXcd newIn=HH1.col(0);
 int i=0;
 auto O=C1+C2;
        while(i*dt<tot)
       {

       	 TimeEv::timeev_exact(newIn, cEVec, evExp);

       	  	 std::complex<double> c=im*(newIn.adjoint()*(O*newIn))(0);
 		 std::complex<double> c2=(newIn.adjoint()*(H1*newIn))(0);
       		 //	 std::complex<double> c=(inistate.adjoint()*(newIn))(0);

       	 i++;

       			// 	outputVals(i)=real(c);
       			// 	outputVals2(i)=real(c2);
        		// outputTime(i)=i*dt;
       	 std::cout<< std::setprecision(8)<<c<< " dt "<< i*dt<< "  E  "<< c2<< std::endl;
       	 //	  	 BOOST_CHECK(std::abs(real(c2)-real(c))<Many_Body::err);
       	     	             }

 //std::cout<< MatrixXd(OBS2) << std::endl;
 // Eigen::MatrixXd OBS22=HH.adjoint()*(OBS2.selfadjointView<Lower>())*HH;
 // Eigen::VectorXd diagobs2=OBS22.diagonal();
 	// std::cout<< en(0) << std::endl;
 // std::cout<<diagobs2(0)<<std::endl;
    std::string filename=".bin";
    //       bin_write("E"+filename, en);
  return 0;
}


#define BOOST_TEST_MODULE shperical harmonic coefficients
#define BOOST_TEST_DYN_LINK
#include <cmath>
#include <random>
#include <vector>
#include <chrono>
#include <boost/test/unit_test.hpp>
#include <blackpearl/core/sph_hrm_coeffs.hpp>

template<typename real_scalar_type>
void test_shp_hrm_coeffs(){
    using namespace blackpearl::core;
    size_t l_max(6143);
    size_t m_max(l_max);
    size_t num_fields(6);
    sph_hrm_coeffs<real_scalar_type> sh(l_max,m_max,num_fields);
}

BOOST_AUTO_TEST_CASE(shp_hrm_coeffs){
    test_shp_hrm_coeffs<float>();
    test_shp_hrm_coeffs<double>();
}


// Copyright Louis Dionne 2013-2016
// 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_HANA_EXAMPLE_CPPCON_2014_MATRIX_MATRIX_HPP
#define BOOST_HANA_EXAMPLE_CPPCON_2014_MATRIX_MATRIX_HPP

#include <boost/hana/all_of.hpp>
#include <boost/hana/assert.hpp>
#include <boost/hana/at.hpp>
#include <boost/hana/drop_front.hpp>
#include <boost/hana/front.hpp>
#include <boost/hana/functional/on.hpp>
#include <boost/hana/functional/partial.hpp>
#include <boost/hana/fuse.hpp>
#include <boost/hana/length.hpp>
#include <boost/hana/mult.hpp>
#include <boost/hana/sum.hpp>
#include <boost/hana/tuple.hpp>
#include <boost/hana/unpack.hpp>
#include <boost/hana/value.hpp>
#include <boost/hana/zip.hpp>
#include <boost/hana/zip_with.hpp>

#include <utility>

namespace cppcon {
    template <unsigned Rows, unsigned Columns>
    struct Matrix { };

    template <unsigned Rows, unsigned Columns, typename Storage>
    struct matrix_type {
        using hana_tag = Matrix<Rows, Columns>;

        Storage rows_;
        constexpr auto ncolumns() const
        { return boost::hana::length(boost::hana::front(rows_)); }

        constexpr auto nrows() const
        { return boost::hana::length(rows_); }

        constexpr auto size() const
        { return nrows() * ncolumns(); }

        template <typename I, typename J>
        constexpr decltype(auto) at(I i, J j) const
        { return boost::hana::at(boost::hana::at(rows_, i), j); }
    };

    auto row = boost::hana::make_tuple;

    auto matrix = [](auto&& ...rows) -> decltype(auto) {
        namespace hana = boost::hana;
        auto storage = hana::make_tuple(std::forward<decltype(rows)>(rows)...);
        auto ncolumns = hana::length(hana::front(storage));
        BOOST_HANA_CONSTANT_CHECK(
            hana::all_of(hana::drop_front(storage), [&](auto const& row) {
                return hana::length(row) == ncolumns;
            })
        );

        return matrix_type<
            sizeof...(rows), hana::value(ncolumns), decltype(storage)
        >{std::move(storage)};
    };

    auto vector = boost::hana::on(matrix, row);

    // More operations
    auto rows = [](auto&& m) -> decltype(auto) {
        return std::forward<decltype(m)>(m).rows_;
    };

    auto transpose = [](auto&& m) -> decltype(auto) {
        return boost::hana::unpack(
            boost::hana::fuse(boost::hana::zip)(rows(std::forward<decltype(m)>(m))),
            matrix
        );
    };

    auto columns = [](auto&& m) -> decltype(auto) {
        return rows(transpose(std::forward<decltype(m)>(m)));
    };

    auto element_wise = [](auto&& f) -> decltype(auto) {
        namespace hana = boost::hana;
        return [f(std::forward<decltype(f)>(f))](auto&& ...m) -> decltype(auto) {
            return hana::unpack(
                hana::zip_with(hana::partial(hana::zip_with, f),
                    rows(std::forward<decltype(m)>(m))...
                ),
                matrix
            );
        };
    };

    namespace detail {
        auto tuple_scalar_product = [](auto&& u, auto&& v) -> decltype(auto) {
            namespace hana = boost::hana;
            return hana::sum<>(hana::zip_with(hana::mult,
                std::forward<decltype(u)>(u),
                std::forward<decltype(v)>(v)
            ));
        };
    }
} // end namespace cppcon

#endif // !BOOST_HANA_EXAMPLE_CPPCON_2014_MATRIX_MATRIX_HPP


// (C) Copyright 2007-2009 Andrew Sutton
//
// Use, modification and distribution are subject to the
// Boost Software License, Version 1.0 (See accompanying file
// LICENSE_1_0.txt or http://www.boost.org/LICENSE_1_0.txt)

#include <vector>

#include <boost/graph/undirected_graph.hpp>
#include <boost/graph/directed_graph.hpp>
#include <boost/graph/degree_centrality.hpp>
#include <boost/graph/exterior_property.hpp>

using namespace std;
using namespace boost;

// useful types
// number of vertices in the graph
static const unsigned N = 5;

template <typename Graph>
void build_graph(Graph& g,
                 vector<typename graph_traits<Graph>::vertex_descriptor>& v)
{
    typedef typename graph_traits<Graph>::vertex_descriptor Vertex;

    // add vertices
    for(size_t i = 0; i < N; ++i) {
        v[i] = add_vertex(g);
    }

    // add edges
    add_edge(v[0], v[1], g);
    add_edge(v[1], v[2], g);
    add_edge(v[2], v[0], g);
    add_edge(v[3], v[4], g);
    add_edge(v[4], v[0], g);
};

template <typename Graph>
void test_undirected()
{
    typedef typename graph_traits<Graph>::vertex_descriptor Vertex;

    typedef exterior_vertex_property<Graph, unsigned> CentralityProperty;
    typedef typename CentralityProperty::container_type CentralityContainer;
    typedef typename CentralityProperty::map_type CentralityMap;

    Graph g;
    vector<Vertex> v(N);
    build_graph(g, v);

    CentralityContainer cents(num_vertices(g));
    CentralityMap cm(cents, g);
    all_degree_centralities(g, cm);

    BOOST_ASSERT(cm[v[0]] == 3);
    BOOST_ASSERT(cm[v[1]] == 2);
    BOOST_ASSERT(cm[v[2]] == 2);
    BOOST_ASSERT(cm[v[3]] == 1);
    BOOST_ASSERT(cm[v[4]] == 2);
}

template <typename Graph>
void test_influence()
{
    typedef typename graph_traits<Graph>::vertex_descriptor Vertex;

    typedef exterior_vertex_property<Graph, unsigned> CentralityProperty;
    typedef typename CentralityProperty::container_type CentralityContainer;
    typedef typename CentralityProperty::map_type CentralityMap;

    Graph g;

    vector<Vertex> v(N);
    build_graph(g, v);

    CentralityContainer cents(num_vertices(g));
    CentralityMap cm(cents, g);
    all_influence_values(g, cm);

    BOOST_ASSERT(cm[v[0]] == 1);
    BOOST_ASSERT(cm[v[1]] == 1);
    BOOST_ASSERT(cm[v[2]] == 1);
    BOOST_ASSERT(cm[v[3]] == 1);
    BOOST_ASSERT(cm[v[4]] == 1);
}

template <typename Graph>
void test_prestige()
{
    typedef typename graph_traits<Graph>::vertex_descriptor Vertex;

    typedef exterior_vertex_property<Graph, unsigned> CentralityProperty;
    typedef typename CentralityProperty::container_type CentralityContainer;
    typedef typename CentralityProperty::map_type CentralityMap;

    Graph g;

    vector<Vertex> v(N);
    build_graph(g, v);

    CentralityContainer cents(num_vertices(g));
    CentralityMap cm(cents, g);
    all_prestige_values(g, cm);

    BOOST_ASSERT(cm[v[0]] == 2);
    BOOST_ASSERT(cm[v[1]] == 1);
    BOOST_ASSERT(cm[v[2]] == 1);
    BOOST_ASSERT(cm[v[3]] == 0);
    BOOST_ASSERT(cm[v[4]] == 1);
}

int
main(int argc, char *argv[])
{
    typedef undirected_graph<> Graph;
    typedef directed_graph<> Digraph;

    test_undirected<Graph>();
    test_influence<Digraph>();
    test_prestige<Digraph>();
}


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

// ---
// Author: Zachary Kann

#include <fstream>
#include <cmath>
#include <complex>
#include <armadillo>
#include "boost/program_options.hpp"
#include "z_constants.hpp"
#include "z_conversions.hpp"
#include "z_string.hpp"

namespace po = boost::program_options;

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

  enum Chromophore {kOH, kOD};

  std::string V_filename;
  po::options_description desc("Options");
  desc.add_options()
    ("help,h",  "Print help messages")
    ("V_file,c", po::value<std::string>(&V_filename),
     "Choice of chromophore (OH or OD)")
    ("chromophore,c", po::value<std::string>(),
     "Choice of chromophore (OH or OD)");

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

  if (vm.count("help")) {
    std::cout << desc << "\n";
    exit(EXIT_SUCCESS);
  }

  Chromophore chromophore;
  const std::string& vm_chromophore =
      vm["chromophore"].as<std::string>();
  if (vm_chromophore == "OH")
    chromophore = kOH;
  else if (vm_chromophore == "OD")
    chromophore = kOD;
  else
    assert(false && "Unrecognized chromophore option");

  double red_mass = (chromophore == kOH) ? MASS_H*MASS_OD : MASS_D*MASS_OH;
  red_mass *= AMU_TO_KG/MASS_HOD;
  double delta_x = 0.02e-10;
  int points = 56;

  arma::mat T = arma::zeros<arma::mat>(points, points);
  arma::mat V = arma::zeros<arma::mat>(points, points);
  arma::mat H = arma::zeros<arma::mat>(points, points);

  std::ifstream V_file(V_filename.c_str());

  for (int i=0; i<points; i++)
    V_file >> V(i,i);

  for (int i_col = 0; i_col < points; ++i_col) {
    for (int i_row = 0; i_row < points; ++i_row) {
      T(i_row, i_col) =
          H_BAR*H_BAR/2.0/red_mass/delta_x/delta_x/C_SPEED_CGS/PLANCK;
      V(i_row, i_col) *= 220000;
      if ((i_col-i_row)%2)
        T(i_row, i_col) *= -1.0;
      if (i_row == i_col)
        T(i_row, i_col) *= M_PI*M_PI/3.0;
      else
        T(i_row, i_col) *= 2.0/(i_col-i_row)/(i_col-i_row);
    }
  }
  H = T+V;
  arma::rowvec eigenvalues = arma::eig_sym(H);

  std::ofstream output_file("freqs.dat");
  output_file << eigenvalues(1) - eigenvalues(0);
}


#include <memory>

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

#include "ldaplusplus/Document.hpp"

using namespace Eigen;
using namespace ldaplusplus;

TEST(TestCorpus, TestDocument) {
    VectorXi X = VectorXi::Random(10).array().abs().matrix();

    auto doc = std::make_shared<corpus::EigenDocument>(X);

    for (int i=0; i<10; i++) {
        ASSERT_EQ(X[i], doc->get_words()[i]);
    }
}

TEST(TestCorpus, TestClassificationDecorator) {
    VectorXi X = VectorXi::Random(10).array().abs().matrix();

    auto doc = std::make_shared<corpus::ClassificationDecorator>(
        std::make_shared<corpus::EigenDocument>(X),
        13
    );

    for (int i=0; i<10; i++) {
        ASSERT_EQ(X[i], doc->get_words()[i]);
    }
    ASSERT_EQ(13, doc->get_class());
}

TEST(TestCorpus, TestEigenCorpus) {
    MatrixXi X = MatrixXi::Random(10, 100).array().abs().matrix();

    auto corpus = std::make_shared<corpus::EigenCorpus>(X);

    ASSERT_EQ(100, corpus->size());

    for (int i=0; i<100; i++) {
        for (int j=0; j<10; j++) {
            ASSERT_EQ(X(j, i), corpus->at(i)->get_words()[j]);
        }
    }
}


/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2015, Daichi Yoshikawa
 *  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 Daichi Yoshikawa 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.
 *
 * Author: Daichi Yoshikawa
 *
 *********************************************************************/

#ifndef __AHL_ROBOT_TRANSFORMATION_HPP
#define __AHL_ROBOT_TRANSFORMATION_HPP

#include <memory>
#include <Eigen/Dense>

namespace ahl_robot
{

  class Transformation
  {
  public:
    explicit Transformation()
      : T_(Eigen::Matrix4d::Identity()), axis_(Eigen::Vector3d::UnitX()) {}
    virtual ~Transformation() {}
    virtual const Eigen::Matrix4d& T(double q) = 0;
    virtual void transform(double q, const Eigen::Matrix4d& T_org, Eigen::Matrix4d& T) = 0;
    virtual Eigen::Vector3d& axis()
    {
      return axis_;
    }
  protected:
    Eigen::Matrix4d T_;
    Eigen::Vector3d axis_;
  };
  using TransformationPtr = std::shared_ptr<Transformation>;

  class Fixed : public Transformation
  {
  public:
    virtual const Eigen::Matrix4d& T(double q) override
    {
      return T_;
    }
    virtual void transform(double q, const Eigen::Matrix4d& T_org, Eigen::Matrix4d& T) override
    {
    }
  };

  class RevoluteX : public Transformation
  {
  public:
    explicit RevoluteX();
    virtual const Eigen::Matrix4d& T(double q) override;
    virtual void transform(double q, const Eigen::Matrix4d& T_org, Eigen::Matrix4d& T) override;
  private:
    Eigen::Matrix3d R_;
  };

  class RevoluteY : public Transformation
  {
  public:
    explicit RevoluteY();
    virtual const Eigen::Matrix4d& T(double q) override;
    virtual void transform(double q, const Eigen::Matrix4d& T_org, Eigen::Matrix4d& T) override;
  private:
    Eigen::Matrix3d R_;
  };

  class RevoluteZ : public Transformation
  {
  public:
    explicit RevoluteZ();
    virtual const Eigen::Matrix4d& T(double q) override;
    virtual void transform(double q, const Eigen::Matrix4d& T_org, Eigen::Matrix4d& T) override;
  private:
    Eigen::Matrix3d R_;
  };

  class PrismaticX : public Transformation
  {
  public:
    explicit PrismaticX();
    virtual const Eigen::Matrix4d& T(double q) override;
    virtual void transform(double q, const Eigen::Matrix4d& T_org, Eigen::Matrix4d& T) override;
  };

  class PrismaticY : public Transformation
  {
  public:
    explicit PrismaticY();
    virtual const Eigen::Matrix4d& T(double q) override;
    virtual void transform(double q, const Eigen::Matrix4d& T_org, Eigen::Matrix4d& T) override;
  };

  class PrismaticZ : public Transformation
  {
  public:
    explicit PrismaticZ();
    virtual const Eigen::Matrix4d& T(double q) override;
    virtual void transform(double q, const Eigen::Matrix4d& T_org, Eigen::Matrix4d& T) override;
  };

} // namespace ahl_robot

#endif // __AHL_ROBOT_TRANSFORMATION_HPP


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE kpabe_test

#include <vector>
#include <string>
#include <iostream>

#include <boost/test/unit_test.hpp>
#include <pbc.h>

#include "kpabe.hpp"

using namespace std;

struct InitPolicy {
   Node root;
   vector<int> attributes;

   InitPolicy() : root(Node::Type::AND), attributes({1, 2, 3, 4}) {
      // (one or two) and (three or four)
      vector<Node> children1, children2;
      for(auto it = attributes.begin(); it != attributes.begin() + attributes.size() / 2; ++it) {
         children1.emplace_back(*it);
         children2.emplace_back(*(it + 2));
      }

      Node orNodeLeft(Node::Type::OR, children1);
      Node orNodeRight(Node::Type::OR, children2);
      root.addChild(orNodeLeft);
      root.addChild(orNodeRight);
   }
};

struct InitGenerator: InitPolicy {
   PrivateParams priv;
   PublicParams pub;

   InitGenerator() : InitPolicy() {
      setup({1, 2, 3, 4}, pub, priv);
   }
};

BOOST_AUTO_TEST_CASE(hashElement_test) {
   element_t el;
   element_init_G1(el, getPairing());
   element_random(el);

   uint8_t key[32];
   hashElement(el, key);
   //elementToKey(el, key, NULL);

   element_clear(el);
}

BOOST_FIXTURE_TEST_CASE(getLeafs_test, InitPolicy) {
   auto leafs = root.getLeafs();
   for(auto attr: attributes) {
      BOOST_CHECK(find(leafs.begin(), leafs.end(), attr) != leafs.end());
   }
}

BOOST_FIXTURE_TEST_CASE(splitShares_test, InitPolicy) {

   element_t rootSecret;
   element_init_Zr(rootSecret, getPairing());
   element_random(rootSecret);

   auto shares = root.splitShares(*rootSecret);

   for(auto& share: shares) {
      element_add_ui(&share, &share, 1ul); // Sanity check that the shares are initialized
      element_clear(&share);
   }

   element_clear(rootSecret);
}

BOOST_FIXTURE_TEST_CASE(getSecretShares_test, InitPolicy) {
   element_t rootSecret;
   element_init_Zr(rootSecret, getPairing());
   element_random(rootSecret);

   auto shares = root.getSecretShares(*rootSecret);

   for(auto& share: shares) {
      element_add_ui(&share, &share, 1ul); // Sanity check that the shares are initialized
      element_clear(&share);
   }

   element_clear(rootSecret);
}

BOOST_FIXTURE_TEST_CASE(recoverCoefficients_test, InitPolicy) {
   auto shares = root.recoverCoefficients();

   for(auto& share: shares) {
      element_add_ui(&share, &share, 1ul); // Sanity check that the shares are initialized
      element_clear(&share);
   }
}

BOOST_FIXTURE_TEST_CASE(satisfyingAttributes_test, InitPolicy) {
   element_t rootCoeff;
   element_init_Zr(rootCoeff, getPairing());
   element_set1(rootCoeff);

   vector<int> attr {1, 3};
   vector<int> expected {1, 3};
   auto sat = root.satisfyingAttributes(attr, *rootCoeff);

   BOOST_CHECK(expected.size() == sat.size());
   for(auto& s: sat) {
      BOOST_CHECK(find(expected.begin(), expected.end(), s.first) != expected.end());
      element_clear(&s.second);
   }

   element_clear(rootCoeff);
}

BOOST_FIXTURE_TEST_CASE(satisfyingAttributes_negative_test, InitPolicy) {
   element_t rootCoeff;
   element_init_Zr(rootCoeff, getPairing());
   element_set1(rootCoeff);

   vector<int> attr {1};
   vector<int> expected {};
   auto sat = root.satisfyingAttributes(attr, *rootCoeff);

   BOOST_CHECK(expected.size() == sat.size());
   // Just in case the test fails, clear all elements
   for(auto& s: sat) {
      const auto& a = s.first;
      BOOST_CHECK(find(expected.begin(), expected.end(), a) != expected.end());
      element_clear(&s.second);
   }

   element_clear(rootCoeff);
}

BOOST_AUTO_TEST_CASE(setupTest) {
   vector<int> attributes {1, 2, 3};
   PrivateParams priv;
   PublicParams pub;
   setup(attributes, pub, priv);
}

BOOST_FIXTURE_TEST_CASE(createSecretTest, InitPolicy) {
   vector<int> attrUniverse {1, 2, 3, 4};
   PrivateParams priv;
   PublicParams pub;
   setup(attributes, pub, priv);
   auto key = keyGeneration(priv, root);
   vector<int> expectedAttributes {1, 2, 3, 4};

   BOOST_CHECK(expectedAttributes.size() == key.Di.size());
   for(auto attr: expectedAttributes) {
      auto attrDuPairIter = key.Di.find(attr);
      BOOST_CHECK(attrDuPairIter != key.Di.end());
      element_clear(&attrDuPairIter->second);
   }
}

BOOST_FIXTURE_TEST_CASE(createSecretAndRecoverSecret, InitGenerator) {
   element_s CsEnc, CsDec;
   vector<int> encAttr {1, 3};
   auto Cw = createSecret(pub, encAttr, CsEnc);

   auto decKeyPolicy = keyGeneration(priv, root);
   recoverSecret(decKeyPolicy, Cw, encAttr, CsDec);

   BOOST_CHECK(!element_cmp(&CsEnc, &CsDec));

   for(auto& attrCiPair: Cw) {
      element_clear(&attrCiPair.second);
   }

   for(auto& attrDiPair: decKeyPolicy.Di) {
      element_clear(&attrDiPair.second);
   }

   element_clear(&CsEnc);
   element_clear(&CsDec);
}

BOOST_FIXTURE_TEST_CASE(encryptAndDecrypt, InitGenerator) {
   const string message("Hello World!");
   vector<int> attributes {1};

   Cw_t Cw;
   auto ciphertext = encrypt(pub, attributes, message, Cw);

   Node policy(Node::Type::OR);
   policy.addChild(Node(1));
   policy.addChild(Node(2));
   auto key = keyGeneration(priv, policy);

   auto msg = decrypt(key, Cw, attributes, ciphertext);

   for(auto& attrCiPair: Cw) {
      element_clear(&attrCiPair.second);
   }

   for(auto& attrDiPair: key.Di) {
      element_clear(&attrDiPair.second);
   }

   BOOST_CHECK(msg == message);
}


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

#ifndef NETKET_VARIATIONALMONTECARLO_HPP
#define NETKET_VARIATIONALMONTECARLO_HPP

#include <complex>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <string>
#include <vector>

#include <Eigen/Dense>
#include <Eigen/IterativeLinearSolvers>
#include <nonstd/optional.hpp>

#include "Machine/machine.hpp"
#include "Operator/abstract_operator.hpp"
#include "Optimizer/optimizer.hpp"
#include "Output/json_output_writer.hpp"
#include "Sampler/abstract_sampler.hpp"
#include "Stats/stats.hpp"
#include "Utils/parallel_utils.hpp"
#include "Utils/random_utils.hpp"
#include "common_types.hpp"
#include "matrix_replacement.hpp"

namespace netket {

// Variational Monte Carlo schemes to learn the ground state
// Available methods:
// 1) Stochastic reconfiguration optimizer
//   both direct and sparse version
// 2) Gradient Descent optimizer
class VariationalMonteCarlo {
  using GsType = Complex;
  using VectorT = Eigen::Matrix<Complex, Eigen::Dynamic, 1>;
  using MatrixT = Eigen::Matrix<Complex, Eigen::Dynamic, Eigen::Dynamic>;

  const AbstractOperator &ham_;
  AbstractSampler &sampler_;
  AbstractMachine &psi_;

  std::vector<std::vector<int>> connectors_;
  std::vector<std::vector<double>> newconfs_;
  std::vector<Complex> mel_;

  Eigen::VectorXcd elocs_;
  MatrixT Ok_;
  VectorT Okmean_;

  Eigen::MatrixXd vsamp_;

  Eigen::VectorXcd grad_;
  Eigen::VectorXcd gradprev_;

  double sr_diag_shift_;
  bool sr_rescale_shift_;
  bool use_iterative_;

  int totalnodes_;
  int mynode_;

  AbstractOptimizer &opt_;

  std::vector<AbstractOperator *> obs_;
  std::vector<std::string> obsnames_;
  ObsManager obsmanager_;

  bool dosr_;

  bool use_cholesky_;

  int nsamples_;
  int nsamples_node_;
  int ninitsamples_;
  int ndiscardedsamples_;

  Complex elocmean_;
  double elocvar_;
  int npar_;

 public:
  class Iterator {
   public:
    // typedefs required for iterators
    using iterator_category = std::input_iterator_tag;
    using difference_type = Index;
    using value_type = Index;
    using pointer_type = Index *;
    using reference_type = Index &;

   private:
    VariationalMonteCarlo &vmc_;
    Index step_size_;
    nonstd::optional<Index> n_iter_;

    Index cur_iter_;

   public:
    Iterator(VariationalMonteCarlo &vmc, Index step_size,
             nonstd::optional<Index> n_iter)
        : vmc_(vmc),
          step_size_(step_size),
          n_iter_(std::move(n_iter)),
          cur_iter_(0) {}

    Index operator*() const { return cur_iter_; }

    Iterator &operator++() {
      vmc_.Advance(step_size_);
      cur_iter_ += step_size_;
      return *this;
    }

    // TODO(C++17): Replace with comparison to special Sentinel type, since
    // C++17 allows end() to return a different type from begin().
    bool operator!=(const Iterator &) {
      return !n_iter_.has_value() || cur_iter_ < n_iter_.value();
    }
    // pybind11::make_iterator requires operator==
    bool operator==(const Iterator &other) { return !(*this != other); }

    Iterator begin() const { return *this; }
    Iterator end() const { return *this; }
  };

  VariationalMonteCarlo(const AbstractOperator &hamiltonian,
                        AbstractSampler &sampler, AbstractOptimizer &optimizer,
                        int nsamples, int discarded_samples = -1,
                        int discarded_samples_on_init = 0,
                        const std::string &method = "Sr",
                        double diag_shift = 0.01, bool rescale_shift = false,
                        bool use_iterative = false, bool use_cholesky = true)
      : ham_(hamiltonian),
        sampler_(sampler),
        psi_(sampler.GetMachine()),
        opt_(optimizer),
        elocvar_(0.) {
    Init(nsamples, discarded_samples, discarded_samples_on_init, method,
         diag_shift, rescale_shift, use_iterative, use_cholesky);
  }

  void Init(int nsamples, int discarded_samples, int discarded_samples_on_init,
            const std::string &method, double diagshift, bool rescale_shift,
            bool use_iterative, bool use_cholesky) {
    npar_ = psi_.Npar();

    opt_.Init(psi_.GetParameters());

    grad_.resize(npar_);
    Okmean_.resize(npar_);

    setSrParameters();

    MPI_Comm_size(MPI_COMM_WORLD, &totalnodes_);
    MPI_Comm_rank(MPI_COMM_WORLD, &mynode_);

    nsamples_ = nsamples;

    nsamples_node_ = int(std::ceil(double(nsamples_) / double(totalnodes_)));

    ninitsamples_ = discarded_samples_on_init;

    if (discarded_samples == -1) {
      ndiscardedsamples_ = 0.1 * nsamples_node_;
    } else {
      ndiscardedsamples_ = discarded_samples;
    }

    if (method == "Gd") {
      dosr_ = false;
    } else {
      setSrParameters(diagshift, rescale_shift, use_iterative, use_cholesky);
    }

    if (dosr_) {
      InfoMessage() << "Using the Stochastic reconfiguration method"
                    << std::endl;

      if (use_iterative_) {
        InfoMessage() << "With iterative solver" << std::endl;
      } else {
        if (use_cholesky_) {
          InfoMessage() << "Using Cholesky decomposition" << std::endl;
        }
      }
    } else {
      InfoMessage() << "Using a gradient-descent based method" << std::endl;
    }

    InfoMessage() << "Variational Monte Carlo running on " << totalnodes_
                  << " processes" << std::endl;

    MPI_Barrier(MPI_COMM_WORLD);
  }

  void AddObservable(AbstractOperator &ob, const std::string &obname) {
    obs_.push_back(&ob);
    obsnames_.push_back(obname);
  }

  void InitSweeps() {
    sampler_.Reset();

    for (int i = 0; i < ninitsamples_; i++) {
      sampler_.Sweep();
    }
  }

  void Sample() {
    sampler_.Reset();

    for (int i = 0; i < ndiscardedsamples_; i++) {
      sampler_.Sweep();
    }

    vsamp_.resize(nsamples_node_, psi_.Nvisible());

    for (int i = 0; i < nsamples_node_; i++) {
      sampler_.Sweep();
      vsamp_.row(i) = sampler_.Visible();
    }
  }

  /**
   * Computes the expectation values of observables from the currently stored
   * samples.
   */
  void ComputeObservables() {
    const Index nsamp = vsamp_.rows();
    for (const auto &obname : obsnames_) {
      obsmanager_.Reset(obname);
    }
    for (Index i_samp = 0; i_samp < nsamp; ++i_samp) {
      for (std::size_t i_obs = 0; i_obs < obs_.size(); ++i_obs) {
        const auto &op = obs_[i_obs];
        const auto &name = obsnames_[i_obs];
        obsmanager_.Push(name, ObsLocValue(*op, vsamp_.row(i_samp)).real());
      }
    }
  }

  void Gradient() {
    obsmanager_.Reset("Energy");
    obsmanager_.Reset("EnergyVariance");

    const int nsamp = vsamp_.rows();
    elocs_.resize(nsamp);
    Ok_.resize(nsamp, psi_.Npar());

    for (int i = 0; i < nsamp; i++) {
      elocs_(i) = ObsLocValue(ham_, vsamp_.row(i));
      Ok_.row(i) = psi_.DerLog(vsamp_.row(i));
      obsmanager_.Push("Energy", elocs_(i).real());
    }

    elocmean_ = elocs_.mean();
    SumOnNodes(elocmean_);
    elocmean_ /= double(totalnodes_);

    Okmean_ = Ok_.colwise().mean();
    SumOnNodes(Okmean_);
    Okmean_ /= double(totalnodes_);

    Ok_ = Ok_.rowwise() - Okmean_.transpose();

    elocs_ -= elocmean_ * Eigen::VectorXd::Ones(nsamp);

    for (int i = 0; i < nsamp; i++) {
      obsmanager_.Push("EnergyVariance", std::norm(elocs_(i)));
    }

    grad_ = 2. * (Ok_.adjoint() * elocs_);

    // Summing the gradient over the nodes
    SumOnNodes(grad_);
    grad_ /= double(totalnodes_ * nsamp);
  }

  /**
   * Computes the value of the local estimator of the operator `ob` in
   * configuration `v` which is defined by O_loc(v) = ⟨v|ob|Ψ⟩ / ⟨v|Ψ⟩.
   *
   * @param ob Operator representing the observable.
   * @param v Many-body configuration
   * @return The value of the local observable O_loc(v).
   */
  Complex ObsLocValue(const AbstractOperator &ob, const Eigen::VectorXd &v) {
    ob.FindConn(v, mel_, connectors_, newconfs_);

    assert(connectors_.size() == mel_.size());

    auto logvaldiffs = (psi_.LogValDiff(v, connectors_, newconfs_));

    assert(mel_.size() == std::size_t(logvaldiffs.size()));

    Complex obval = 0;

    for (int i = 0; i < logvaldiffs.size(); i++) {
      obval += mel_[i] * std::exp(logvaldiffs(i));
    }

    return obval;
  }

  double ElocMean() { return elocmean_.real(); }

  double Elocvar() { return elocvar_; }

  void Advance(Index steps = 1) {
    assert(steps > 0);
    for (Index i = 0; i < steps; ++i) {
      Sample();
      Gradient();
      UpdateParameters();
    }
  }

  Iterator Iterate(const nonstd::optional<Index> &n_iter = nonstd::nullopt,
                   Index step_size = 1) {
    assert(!n_iter.has_value() || n_iter.value() > 0);
    assert(step_size > 0);

    opt_.Reset();
    InitSweeps();

    Advance(step_size);
    return Iterator(*this, step_size, n_iter);
  }

  void Run(const std::string &output_prefix,
           nonstd::optional<Index> n_iter = nonstd::nullopt,
           Index step_size = 1, Index save_params_every = 50) {
    assert(n_iter > 0);
    assert(step_size > 0);
    assert(save_params_every > 0);

    nonstd::optional<JsonOutputWriter> writer;
    if (mynode_ == 0) {
      writer.emplace(output_prefix + ".log", output_prefix + ".wf",
                     save_params_every);
    }

    for (const auto step : Iterate(n_iter, step_size)) {
      ComputeObservables();

      // Note: This has to be called in all MPI processes, because converting
      // the ObsManager to JSON performs a MPI reduction.
      auto obs_data = json(obsmanager_);
      obs_data["Acceptance"] = sampler_.Acceptance();

      // writer.has_value() iff the MPI rank is 0, so the output is only
      // written once
      if (writer.has_value()) {
        writer->WriteLog(step, obs_data);
        writer->WriteState(step, psi_);
      }
      MPI_Barrier(MPI_COMM_WORLD);
    }
  }

  void UpdateParameters() {
    auto pars = psi_.GetParameters();

    if (dosr_) {
      const int nsamp = vsamp_.rows();

      Eigen::VectorXcd b = Ok_.adjoint() * elocs_;
      SumOnNodes(b);
      b /= double(nsamp * totalnodes_);

      if (!use_iterative_) {
        // Explicit construction of the S matrix
        Eigen::MatrixXcd S = Ok_.adjoint() * Ok_;
        SumOnNodes(S);
        S /= double(nsamp * totalnodes_);

        // Adding diagonal shift
        S += Eigen::MatrixXd::Identity(pars.size(), pars.size()) *
             sr_diag_shift_;

        Eigen::VectorXcd deltaP;
        if (use_cholesky_ == false) {
          Eigen::FullPivHouseholderQR<Eigen::MatrixXcd> qr(S.rows(), S.cols());
          qr.setThreshold(1.0e-6);
          qr.compute(S);
          deltaP = qr.solve(b);
        } else {
          Eigen::LLT<Eigen::MatrixXcd> llt(S.rows());
          llt.compute(S);
          deltaP = llt.solve(b);
        }
        // Eigen::VectorXcd deltaP=S.jacobiSvd(ComputeThinU |
        // ComputeThinV).solve(b);

        assert(deltaP.size() == grad_.size());
        grad_ = deltaP;

        if (sr_rescale_shift_) {
          Complex nor = (deltaP.dot(S * deltaP));
          grad_ /= std::sqrt(nor.real());
        }

      } else {
        Eigen::ConjugateGradient<MatrixReplacement, Eigen::Lower | Eigen::Upper,
                                 Eigen::IdentityPreconditioner>
            it_solver;
        // Eigen::GMRES<MatrixReplacement, Eigen::IdentityPreconditioner>
        // it_solver;
        it_solver.setTolerance(1.0e-3);
        MatrixReplacement S;
        S.attachMatrix(Ok_);
        S.setShift(sr_diag_shift_);
        S.setScale(1. / double(nsamp * totalnodes_));

        it_solver.compute(S);
        auto deltaP = it_solver.solve(b);

        grad_ = deltaP;
        if (sr_rescale_shift_) {
          auto nor = deltaP.dot(S * deltaP);
          grad_ /= std::sqrt(nor.real());
        }

        // if(mynode_==0){
        //   std::cerr<<it_solver.iterations()<<"
        //   "<<it_solver.error()<<std::endl;
        // }
        MPI_Barrier(MPI_COMM_WORLD);
      }
    }

    opt_.Update(grad_, pars);

    SendToAll(pars);

    psi_.SetParameters(pars);
    MPI_Barrier(MPI_COMM_WORLD);
  }

  void setSrParameters(double diagshift = 0.01, bool rescale_shift = false,
                       bool use_iterative = false, bool use_cholesky = true) {
    sr_diag_shift_ = diagshift;
    sr_rescale_shift_ = rescale_shift;
    use_iterative_ = use_iterative;
    dosr_ = true;
    use_cholesky_ = use_cholesky;
  }

  AbstractMachine &GetMachine() { return psi_; }
  const ObsManager &GetObsManager() const { return obsmanager_; }
};

}  // namespace netket

#endif


/*
 * QuadraticIntegralErrorTerm.cpp
 *
 *  Created on: Dec 4, 2013
 *      Author: hannes
 */

#include <sstream>
#include <boost/python.hpp>
#include <numpy_eigen/boost_python_headers.hpp>
#include <Eigen/Core>
#include <aslam/backend/QuadraticIntegralError.hpp>

using namespace boost::python;

template <int dim, typename Expression = aslam::backend::VectorExpression<dim>>
inline void addQuadraticIntegralExpressionErrorTerms(aslam::backend::OptimizationProblem & problem, double a, double b, int numberOfPoints, const object & expressionFactory, const Eigen::Matrix<double, dim, dim> & sqrtInvR){
  using namespace aslam::backend::integration;

  struct ExpressionFactoryAdapter {
    ExpressionFactoryAdapter(const object & o) : o(o){}

    Expression operator()(double time) const {
      return extract<Expression>(o(time));
    }
   private:
    const object & o;
  };

  addQuadraticIntegralExpressionErrorTerms(problem, a, b, numberOfPoints, ExpressionFactoryAdapter(expressionFactory), sqrtInvR);
}

const char * BaseName = "addQuadraticIntegralExpressionErrorTermsToProblem";

template <int dimMax>
void defAddQuadraticIntegralExpressionErrorTerms(){
  std::stringstream s;
  s << BaseName << dimMax;

  boost::python::def(
      s.str().c_str(),
      &addQuadraticIntegralExpressionErrorTerms<dimMax>,
      boost::python::args("problem, timeA, timeB, nIntegrationPoints, expressionFactory, sqrtInvR")
   );
  defAddQuadraticIntegralExpressionErrorTerms<dimMax -1>();
}

template <>
void defAddQuadraticIntegralExpressionErrorTerms<0>(){
}

void exportAddQuadraticIntegralExpressionErrorTerms(){
  defAddQuadraticIntegralExpressionErrorTerms<3>();

  boost::python::def(
      "addQuadraticIntegralEuclideanExpressionErrorTermsToProblem",
      &addQuadraticIntegralExpressionErrorTerms<3, aslam::backend::EuclideanExpression>,
      boost::python::args("problem, timeA, timeB, nIntegrationPoints, expressionFactory, sqrtInvR")
   );
}


/*
 *                                                                                * Note: This license has also been called the "New BSD License"
 * or "Modified BSD License".
 *
 * Copyright (c) 2021 Electronics and Telecommunications Research
 * Institute All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * 2. 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.
 *
 * 3. Neither the name of the copyright holder 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 HOLDER 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.
*/

/**
 * CityGeodeticCalculator.cpp
 *
 * $Revision: 625 $
 * $LastChangedDate: 2019-07-19 15:18:25 +0900 (Fri, 19 Jul 2019) $
 */

#include <boost/math/constants/constants.hpp>
#include <string>
#include <vector>
#include <cmath>
#include "City.h"
#include "CityGeodeticCalculator.h"
#include "CityUtils.h"

namespace dtsim {

const double CityGeodeticCalculator::WGS84_a = 6378137;
const double CityGeodeticCalculator::WGS84_b = 6356752.314245;
const double CityGeodeticCalculator::WGS84_f = 1/298.257223563;
const double CityGeodeticCalculator::RADIUS_EARTH = 6371000.0;

CityGeodeticCalculator::CityGeodeticCalculator()
{
	if (City::instance()->containProperty("geodetic.distance.formula")) {
		std::string formulaStr = City::instance()->getStringProperty("geodetic.distance.formula");

		if ((formulaStr.compare("haversine") == 0) || (formulaStr.compare("Haversine") == 0))
			formula = GEODETIC_HAVERSINE;
		else if ((formulaStr.compare("vincenty") == 0) || (formulaStr.compare("Vincenty") == 0))
			formula = GEODETIC_VINCENTY;
		else
			formula = GEODETIC_HAVERSINE;
	} else {
		formula = GEODETIC_HAVERSINE;
	}
}

CityGeodeticCalculator::~CityGeodeticCalculator()
{
	_instance = 0;
}

CityGeodeticCalculator* CityGeodeticCalculator::_instance = 0;

CityGeodeticCalculator* CityGeodeticCalculator::instance()
{
	if (_instance == 0)
		_instance = new CityGeodeticCalculator();

	return _instance;
}

void CityGeodeticCalculator::haversine_getDestinationPoint(double startLon, double startLat,
														   double distance, double azimuth,
														   double& destLon, double& destLat)
{
	double delta = distance / RADIUS_EARTH;
	double theta = degreesToRadians(azimuth);

	double lambda1 = degreesToRadians(startLon);
	double phi1 = degreesToRadians(startLat);

	double sinPhi2 = (std::sin(phi1) * std::cos(delta)) +
					 (std::cos(phi1) * std::sin(delta) * std::cos(theta));
	double phi2 = std::asin(sinPhi2);

	double x = std::cos(delta) - (std::sin(phi1) * sinPhi2);
	double y = std::sin(theta) * std::sin(delta) * std::cos(phi1);
	double lambda2 = lambda1 + std::atan2(y, x);

	destLon = radiansToDegrees(lambda2);
	destLat = radiansToDegrees(phi2);
}

void CityGeodeticCalculator::vincenty_getDestinationPoint(double startLon, double startLat,
														  double distance, double azimuth,
														  double& destLon, double& destLat)
{
	double lambda1 = degreesToRadians(startLon);
	double phi1 = degreesToRadians(startLat);
	double alpha1 = degreesToRadians(azimuth);
	double s = distance;

	double sinAlpha1 = std::sin(alpha1);
	double cosAlpha1 = std::cos(alpha1);

	double tanU1 = (1 - WGS84_f) * std::tan(phi1);
	double cosU1 = 1 / std::sqrt((1 + tanU1*tanU1));
	double sinU1 = tanU1 * cosU1;

	double sigma1 = std::atan2(tanU1, cosAlpha1);
	double sinAlpha = cosU1 * sinAlpha1;
	double cosSqAlpha = 1 - sinAlpha*sinAlpha;
	double uSq = cosSqAlpha * (WGS84_a*WGS84_a - WGS84_b*WGS84_b) / (WGS84_b*WGS84_b);
	double A = 1 + uSq/16384*(4096+uSq*(-768+uSq*(320-175*uSq)));
	double B = uSq/1024 * (256+uSq*(-128+uSq*(74-47*uSq)));

	double cos2SigmaM, sinSigma, cosSigma, deltaSigma;

	double sigma = s / (WGS84_b*A);
	double sigmaP;
	int iterations = 0;
	do {
		cos2SigmaM = std::cos(2*sigma1 + sigma);
		sinSigma = std::sin(sigma);
		cosSigma = std::cos(sigma);
		deltaSigma = B * sinSigma * (cos2SigmaM + B/4*(cosSigma*(-1 + 2*cos2SigmaM*cos2SigmaM)-
					 B / 6*cos2SigmaM*(-3 + 4*sinSigma*sinSigma)*(-3 + 4*cos2SigmaM*cos2SigmaM)));
		sigmaP = sigma;
		sigma = s / (WGS84_b*A) + deltaSigma;
	} while ((std::abs(sigma - sigmaP) > 1e-12) && (++iterations < 1000));

	double chi = sinU1*sinSigma - cosU1*cosSigma*cosAlpha1;
	double phi2 = std::atan2(sinU1*cosSigma + cosU1*sinSigma*cosAlpha1, (1-WGS84_f)*std::sqrt(sinAlpha*sinAlpha + chi*chi));
	double lambda = std::atan2(sinSigma*sinAlpha1, cosU1*cosSigma - sinU1*sinSigma*cosAlpha1);
	double C = WGS84_f/16*cosSqAlpha*(4 + WGS84_f*(4 - 3*cosSqAlpha));
	double L = lambda - (1-C) * WGS84_f * sinAlpha * (sigma + C*sinSigma*(cos2SigmaM+C*cosSigma*(-1+2*cos2SigmaM*cos2SigmaM)));
	double lambda2 = lambda1 + L;

	double alpha2 = std::atan2(sinAlpha, -chi);

	destLon = radiansToDegrees(lambda2);
	destLat = radiansToDegrees(phi2);
}

void CityGeodeticCalculator::getDestinationPoint(double startLon, double startLat,
												 double distance, double azimuth,
												 double& destLon, double& destLat)
{
	if (formula == GEODETIC_HAVERSINE)
		haversine_getDestinationPoint(startLon, startLat, distance, azimuth, destLon, destLat);
	else
		vincenty_getDestinationPoint(startLon, startLat, distance, azimuth, destLon, destLat);
}

void CityGeodeticCalculator::getDestinationPoint(CityCoordinate& startPoint,
												 double distance, double azimuth,
												 CityCoordinate& destPoint)
{
	getDestinationPoint(startPoint.x, startPoint.y, distance, azimuth, destPoint.x, destPoint.y);
}

void CityGeodeticCalculator::getMidPoint(double startLon, double startLat,
										 double destLon, double destLat,
										 double& midLon, double& midLat)
{
	double lambda1 = degreesToRadians(startLon);
	double phi1 = degreesToRadians(startLat);
	double phi2 = degreesToRadians(destLat);

	double deltaLambda = degreesToRadians(destLon - startLon);

	std::vector<double> A;
	std::vector<double> B;
	std::vector<double> C;
	double x, y, z;

	x = std::cos(phi1);
	y = 0;
	z = std::sin(phi1);
	A.push_back(x);
	A.push_back(y);
	A.push_back(z);

	x = std::cos(phi2) * std::cos(deltaLambda);
	y = std::cos(phi2) * std::sin(deltaLambda);
	z = std::sin(phi2);
	B.push_back(x);
	B.push_back(y);
	B.push_back(z);

	x = A[0] + B[0];
	y = A[1] + B[1];
	z = A[2] + B[2];
	C.push_back(x);
	C.push_back(y);
	C.push_back(z);

	double phiM = std::atan2(C[2], std::sqrt((C[0]*C[0]) + (C[1]*C[1])));
	double lambdaM = lambda1 + std::atan2(C[1], C[0]);

	midLon = radiansToDegrees(lambdaM);
	midLat = radiansToDegrees(phiM);
}

void CityGeodeticCalculator::getMidPoint(CityCoordinate& startPoint, CityCoordinate& destPoint,
										 CityCoordinate& midPoint)
{
	getMidPoint(startPoint.x, startPoint.y, destPoint.x, destPoint.y, midPoint.x, midPoint.y);
}

void CityGeodeticCalculator::getIntermediatePoint(double startLon, double startLat, double fraction,
												  double destLon, double destLat,
												  double& intermediateLon, double& intermediateLat)
{
	if ((startLon == destLon) && (startLat == destLat)) {
		intermediateLon = startLon;
		intermediateLat = startLat;
		return;
	}

	double lambda1 = degreesToRadians(startLon);
	double phi1 = degreesToRadians(startLat);
	double lambda2 = degreesToRadians(destLon);
	double phi2 = degreesToRadians(destLat);

	double deltaPhi = phi2 - phi1;
	double deltaLambda = lambda2 - lambda1;

	double a = (std::sin(deltaPhi*0.5)*std::sin(deltaPhi*0.5)) +
			   (std::cos(phi1)*std::cos(phi2)*std::sin(deltaLambda*0.5)*std::sin(deltaLambda*0.5));
	double delta = 2 * std::atan2(std::sqrt(a), std::sqrt(1-a));

	double A = std::sin((1-fraction)*delta) / std::sin(delta);
	double B = std::sin(fraction*delta) / std::sin(delta);

	double x = (A * std::cos(phi1) * std::cos(lambda1)) + (B * std::cos(phi2) * std::cos(lambda2));
	double y = (A * std::cos(phi1) * std::sin(lambda1)) + (B * std::cos(phi2) * std::sin(lambda2));
	double z = (A * std::sin(phi1)) + (B * std::sin(phi2));

	double phi3 = std::atan2(z, std::sqrt((x*x) + (y*y)));
	double lambda3 = std::atan2(y, x);

	intermediateLon = radiansToDegrees(lambda3);
	intermediateLat = radiansToDegrees(phi3);
}

void CityGeodeticCalculator::getIntermediatePoint(CityCoordinate& startPoint, double fraction,
												  CityCoordinate& destPoint,
												  CityCoordinate& intermediatePoint)
{
	getIntermediatePoint(startPoint.x, startPoint.y, fraction, destPoint.x, destPoint.y,
						 intermediatePoint.x, intermediatePoint.y);
}

double CityGeodeticCalculator::haversine_getDistance(double startLon, double startLat,
													 double destLon, double destLat)
{
	double lambda1 = degreesToRadians(startLon);
	double phi1 = degreesToRadians(startLat);
	double lambda2 = degreesToRadians(destLon);
	double phi2 = degreesToRadians(destLat);

	double deltaPhi = phi2 - phi1;
	double deltaLambda = lambda2 - lambda1;

	double a = (std::sin(deltaPhi*0.5) * std::sin(deltaPhi*0.5)) +
			   (std::cos(phi1)*std::cos(phi2)*std::sin(deltaLambda*0.5)*std::sin(deltaLambda*0.5));
	double c = 2.0 * std::atan2(std::sqrt(a), std::sqrt(1-a));

	return RADIUS_EARTH * c;
}

double CityGeodeticCalculator::vincenty_getDistance(double startLon, double startLat,
													double destLon, double destLat, double& azimuth)
{
	double lambda1 = degreesToRadians(startLon);
	double phi1 = degreesToRadians(startLat);
	double lambda2 = degreesToRadians(destLon);
	double phi2 = degreesToRadians(destLat);

	double L = lambda2 - lambda1;
	double tanU1 = (1 - WGS84_f) * std::tan(phi1);
	double cosU1 = 1 / std::sqrt((1 + tanU1*tanU1));
	double sinU1 = tanU1 * cosU1;
	double tanU2 = (1 - WGS84_f) * std::tan(phi2);
	double cosU2 = 1 / std::sqrt((1 + tanU2*tanU2));
	double sinU2 = tanU2 * cosU2;

	double sinLambda, cosLambda, sinSigma = 0, cosSigma = 0, sinAlpha;
	double sinSqSigma, cosSqAlpha = 0, cos2SigmaM = 0, sigma = 0, C;

	double lambda = L;
	double lambdaP;
	int iterations = 0;

	bool antimeridian = std::abs(L) > boost::math::constants::pi<double>();

	do {
		sinLambda = std::sin(lambda);
		cosLambda = std::cos(lambda);

		sinSqSigma = (cosU2*sinLambda)*(cosU2*sinLambda) +
					 (cosU1*sinU2 - sinU1*cosU2*cosLambda) *
					 (cosU1*sinU2 - sinU1*cosU2*cosLambda);

		if (std::abs(sinSqSigma) < std::numeric_limits<double>::epsilon())
			break;

		sinSigma = std::sqrt(sinSqSigma);
		cosSigma = sinU1*sinU2 + cosU1*cosU2*cosLambda;

		sigma = std::atan2(sinSigma, cosSigma);
		sinAlpha = cosU1*cosU2*sinLambda / sinSigma;
		cosSqAlpha = 1 - sinAlpha*sinAlpha;
		cos2SigmaM = (cosSqAlpha != 0) ? (cosSigma - 2*sinU1*sinU2/cosSqAlpha) : 0;

		C = WGS84_f/16*cosSqAlpha*(4 + WGS84_f*(4 - 3*cosSqAlpha));
		lambdaP = lambda;
		lambda = L + (1-C)*WGS84_f*sinAlpha*(sigma + C*sinSigma*(cos2SigmaM + C*cosSigma*(-1 + 2*cos2SigmaM*cos2SigmaM)));
	} while ((std::abs(lambda - lambdaP) > 1e-12) && (++iterations < 1000));

	double uSq = cosSqAlpha * (WGS84_a*WGS84_a - WGS84_b*WGS84_b) / (WGS84_b*WGS84_b);
	double A = 1 + uSq/16384*(4096 + uSq*(-768 + uSq*(320  -175*uSq)));
	double B = uSq/1024 * (256 + uSq*(-128 + uSq*(74 - 47*uSq)));
	double deltaSigma = B*sinSigma*(cos2SigmaM + B/4*(cosSigma*(-1 + 2*cos2SigmaM*cos2SigmaM) -
						B/6*cos2SigmaM*(-3 + 4*sinSigma*sinSigma)*(-3 + 4*cos2SigmaM*cos2SigmaM)));

	double distance = WGS84_b*A*(sigma - deltaSigma);

	double alpha = std::atan2(cosU2*sinLambda, cosU1*sinU2 - sinU1*cosU2*cosLambda);
	double degrees = radiansToDegrees(alpha);
	if ((0 > degrees) || (degrees >= 360))
		degrees = std::fmod((std::fmod(degrees, 360.0) + 360.0), 360.0);

	azimuth = std::abs(distance) < std::numeric_limits<double>::epsilon() ? NAN : degrees;

	return distance;
}

double CityGeodeticCalculator::getDistance(double startLon, double startLat,
										   double destLon, double destLat)
{
	if (formula == GEODETIC_HAVERSINE) {
		return haversine_getDistance(startLon, startLat, destLon, destLat);
	} else {
		double azimuth;
		return vincenty_getDistance(startLon, startLat, destLon, destLat, azimuth);
	}
}

double CityGeodeticCalculator::getDistance(CityCoordinate& startPoint, CityCoordinate& destPoint)
{
	return getDistance(startPoint.x, startPoint.y, destPoint.x, destPoint.y);
}

double CityGeodeticCalculator::haversine_getAzimuth(double startLon, double startLat,
													double destLon, double destLat)
{
	double phi1 = degreesToRadians(startLat);
	double phi2 = degreesToRadians(destLat);

	double deltaLambda = degreesToRadians(destLon - startLon);

	double x = (std::cos(phi1) * std::sin(phi2)) -
			   (std::sin(phi1) * std::cos(phi2) * std::cos(deltaLambda));
	double y = std::sin(deltaLambda) * std::cos(phi2);
	double theta = std::atan2(y, x);

	double azimuth = radiansToDegrees(theta);

	if ((0 > azimuth) || (azimuth >= 360))
		azimuth = std::fmod((std::fmod(azimuth, 360.0) + 360.0), 360.0);

	return azimuth;
}

double CityGeodeticCalculator::vincenty_getAzimuth(double startLon, double startLat,
												   double destLon, double destLat)
{
	double azimuth;
	vincenty_getDistance(startLon, startLat, destLon, destLat, azimuth);

	return azimuth;
}

double CityGeodeticCalculator::getAzimuth(double startLon, double startLat,
										  double destLon, double destLat)
{
	if ((startLon == destLon) && (startLat == destLat))
		return NAN;

	if (formula == GEODETIC_HAVERSINE)
		return haversine_getAzimuth(startLon, startLat, destLon, destLat);
	else
		return vincenty_getAzimuth(startLon, startLat, destLon, destLat);
}

double CityGeodeticCalculator::getAzimuth(CityCoordinate& startPoint, CityCoordinate& destPoint)
{
	return getAzimuth(startPoint.x, startPoint.y, destPoint.x, destPoint.y);
}

} /* namespace dtsim */


// Copyright (C) 2013 The Regents of the University of California (Regents)
// and Google, Inc. 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 Regents or University of California, Google,
//       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 HOLDERS 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.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu), John Flynn (jflynn@google.com)

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <glog/logging.h>
#include <cmath>

#include "gtest/gtest.h"

#include "theia/math/util.h"
#include "theia/sfm/pose/test_util.h"
#include "theia/sfm/pose/two_point_pose_partial_rotation.h"
#include "theia/test/test_utils.h"
#include "theia/util/random.h"
#include "theia/util/util.h"

namespace theia {

using Eigen::Map;
using Eigen::Quaterniond;
using Eigen::Vector3d;

namespace {

void TestTwoPointPoseWithNoise(
    const Vector3d points_3d[2],
    const Vector3d& axis,
    const double model_noise,
    const double projection_noise,
    const Quaterniond& test_rotation,
    const Vector3d& test_translation,
    const double max_rotation_difference,
    const double max_difference_between_translation) {
  // Sets up the image rays, since the points_3d are in the image's coordinate
  // space they are just these points after normalization.
  Vector3d image_rays[2] = { points_3d[0].normalized(),
                             points_3d[1].normalized() };

  // Sets up the model points by transforming the points_3d by the inverse
  // of the the test translation and rotation.
  Vector3d model_points[2];
  const Quaterniond inverse_test_rotation = test_rotation.inverse();
  const Vector3d inverse_test_translation =
      -1.0 * (inverse_test_rotation * test_translation);
  for (int i = 0; i < 2; ++i) {
    model_points[i] =
        inverse_test_rotation * points_3d[i] + inverse_test_translation;
  }

  // Adds projection noise if required.
  if (projection_noise) {
    for (int i = 0; i < 2; ++i) {
      AddNoiseToRay(projection_noise, &image_rays[i]);
    }
  }

  // Adds model noise if required.
  if (model_noise) {
    for (int i = 0; i < 2; ++i) {
      AddNoiseToPoint(model_noise, &model_points[i]);
    }
  }

  Quaterniond soln_rotations[2];
  Vector3d soln_translations[2];

  // Computes the pose.
  int num_solutions = TwoPointPosePartialRotation(axis,
                                                  model_points[0],
                                                  model_points[1],
                                                  image_rays[0],
                                                  image_rays[1],
                                                  soln_rotations,
                                                  soln_translations);

  // Checks that one of the solutions matches the test rotation and
  // translations.
  bool matched_transform = false;
  for (int n = 0; n < num_solutions; ++n) {
    const Quaterniond& solved_rotation = soln_rotations[n];
    const Vector3d& solved_translation = soln_translations[n];

    // Checks that the calculated quaternions are unit length.
    ASSERT_NEAR(solved_rotation.norm(), 1.0, 1e-5);

    const double angle_between_rotations =
        test_rotation.angularDistance(solved_rotation);

    bool matched_rotation = (angle_between_rotations < max_rotation_difference);

    const double translation_difference =
        (test_translation - solved_translation).norm();

    bool matched_translation =
        (translation_difference < max_difference_between_translation);

    if (matched_translation && matched_rotation) {
      matched_transform = true;
    }

    // Checks that the solution is valid, i.e. that it gives a very
    // small reprojection error.
    for (int i = 0; i < 2; ++i) {
      const Vector3d proj_3d =
          solved_rotation * model_points[i] + solved_translation;
      static const double kMaxProjectionError = 1e-2;
      for (int d = 0; d < 2; ++d) {
        EXPECT_NEAR(image_rays[i][d] / image_rays[i].z(),
                    proj_3d[d] / proj_3d.z(), kMaxProjectionError);
      }
    }
  }
  EXPECT_TRUE(matched_transform);
}

// Checks with several points, optionally adds model and projection noise.
void ManyPointsTest(const double model_noise,
                    const double projection_noise,
                    const double max_rotation_difference,
                    const double max_difference_between_translation) {
  // Sets some test rotations and translations.
  static const Vector3d kAxes[] = {
      Vector3d(0.0, 0.0, 1.0).normalized(),
      Vector3d(0.0, 1.0, 0.0).normalized(),
      Vector3d(1.0, 0.0, 0.0).normalized(),
      Vector3d(1.0, 0.0, 1.0).normalized(),
      Vector3d(0.0, 1.0, 1.0).normalized(),
      Vector3d(1.0, 1.0, 1.0).normalized(),
      Vector3d(0.0, 1.0, 1.0).normalized(),
      Vector3d(1.0, 1.0, 1.0).normalized()
  };

  static const double kRotationAngles[THEIA_ARRAYSIZE(kAxes)] = {
      DegToRad(7.0),
      DegToRad(12.0),
      DegToRad(15.0),
      DegToRad(20.0),
      DegToRad(11.0),
      DegToRad(0.0),  // Tests no rotation.
      DegToRad(5.0),
      DegToRad(0.0)  // Tests no rotation and no translation.
  };

  static const Vector3d kTranslations[THEIA_ARRAYSIZE(kAxes)] = {
      Vector3d(1.0, 1.0, 1.0),
      Vector3d(3.0, 2.0, 13.0),
      Vector3d(4.0, 5.0, 11.0),
      Vector3d(1.0, 2.0, 15.0),
      Vector3d(3.0, 1.5, 91.0),
      Vector3d(1.0, 7.0, 11.0),
      Vector3d(0.0, 0.0, 0.0),  // Tests no translation.
      Vector3d(0.0, 0.0, 0.0)  // Tests no translation and no rotation.
  };

  // Sets up some test points.
  static const double kTestPoints[][6] = {
      {-1.62, -2.99, 6.12, 4.42, -1.53, 9.83},
      {1.45, -0.59, 5.29, 1.89, -1.10, 8.22},
      {-0.21, 2.38, 5.63, 0.61, -0.97, 7.49},
      {0.48, 0.70, 8.94, 1.65, -2.56, 8.63},
      {2.44, -0.20, 7.78, 2.84, -2.58, 7.35},
      {-1.35, -2.84, 7.33, -0.42, 1.54, 8.86},
      {2.56, 1.72, 7.86, 1.75, -1.39, 5.73},
      {2.08, -3.91, 8.37, -0.91, 1.36, 9.16},
      {2.84, 1.54, 8.74, -1.01, 3.02, 8.18},
      {-3.73, -0.62, 7.81, -2.98, -1.88, 6.23},
      {2.39, -0.19, 6.47, -0.63, -1.05, 7.11},
      {-1.76, -0.55, 5.18, -3.19, 3.27, 8.18},
      {0.31, -2.77, 7.54, 0.54, -3.77, 9.77},
  };

  InitRandomGenerator();
  for (int i = 0; i < THEIA_ARRAYSIZE(kTestPoints); ++i) {
    const Vector3d points_3d[2] = {
      Vector3d(kTestPoints[i][0], kTestPoints[i][1], kTestPoints[i][2]),
      Vector3d(kTestPoints[i][3], kTestPoints[i][4], kTestPoints[i][5]),
    };

    for (int transform_index = 0;
         transform_index < THEIA_ARRAYSIZE(kAxes);
         ++transform_index) {
      const Quaterniond test_rotation(Eigen::AngleAxisd(
          kRotationAngles[transform_index], kAxes[transform_index]));

      TestTwoPointPoseWithNoise(points_3d,
                                kAxes[transform_index],
                                model_noise,
                                projection_noise,
                                test_rotation,
                                kTranslations[transform_index],
                                max_rotation_difference,
                                max_difference_between_translation);
    }
  }
}

// Tests a single set of model to image correspondences and a single
// transformation consisting of a translation and a rotation around kAxis.
void BasicTest() {
  // Sets up some points in the 3D scene
  const Vector3d points_3d[2] = { Vector3d(5.0, 20.0, 23.0),
                                  Vector3d(-6.0, 16.0, 33.0) };

  const Vector3d kAxis(0.0, 1.0, 0.0);
  const Quaterniond kExpectedRotation(Eigen::AngleAxisd(0.15, kAxis));
  const Vector3d kExpectedTranslation(-3.0, 1.5, 11.0);

  static const double kModelNoise = 0.0;
  static const double kProjectionNoise = 0.0 / 512;
  static const double kMaxAllowedRotationDifference = DegToRad(1.0e-5);
  static const double kMaxAllowedTranslationDifference = 1.0e-5;
  TestTwoPointPoseWithNoise(points_3d,
                            kAxis,
                            kModelNoise,
                            kProjectionNoise,
                            kExpectedRotation,
                            kExpectedTranslation,
                            kMaxAllowedRotationDifference,
                            kMaxAllowedTranslationDifference);
}

TEST(TwoPointPoseTest, BasicTest) {
  BasicTest();
}

// Tests a single set of model to image correspondences and multiple
// transformations consisting of translation and rotations around different
// axes.
TEST(TwoPointPoseTest, DifferentAxesTest) {
  const Vector3d points_3d[2] = {
      Vector3d(5.0, 20.0, 23.0),
      Vector3d(-6.0, 16.0, 33.0)
  };

  static const Vector3d kAxes[] = {
      Vector3d(0.0, 0.0, 1.0).normalized(),
      Vector3d(0.0, 1.0, 0.0).normalized(),
      Vector3d(1.0, 0.0, 0.0).normalized(),
      Vector3d(1.0, 0.0, 1.0).normalized(),
      Vector3d(0.0, 1.0, 1.0).normalized(),
      Vector3d(1.0, 1.0, 1.0).normalized(),
      Vector3d(0.0, 1.0, 1.0).normalized(),
      Vector3d(1.0, 1.0, 1.0).normalized()
  };

  static const double kRotationAngles[THEIA_ARRAYSIZE(kAxes)] = {
      DegToRad(7.0),
      DegToRad(12.0),
      DegToRad(15.0),
      DegToRad(20.0),
      DegToRad(11.0),
      DegToRad(0.0),  // Tests no rotation.
      DegToRad(5.0),
      DegToRad(0.0)  // Tests no rotation and no translation.
  };

  static const Vector3d kTranslations[THEIA_ARRAYSIZE(kAxes)] = {
      Vector3d(1.0, 1.0, 1.0),
      Vector3d(3.0, 2.0, 13.0),
      Vector3d(4.0, 5.0, 11.0),
      Vector3d(1.0, 2.0, 15.0),
      Vector3d(3.0, 1.5, 21.0),
      Vector3d(1.0, 7.0, 11.0),
      Vector3d(0.0, 0.0, 0.0),  // Tests no translation.
      Vector3d(0.0, 0.0, 0.0)  // Tests no translation and no rotation.
  };

  for (int i = 0; i < THEIA_ARRAYSIZE(kAxes); ++i) {
    Quaterniond rotation(Eigen::AngleAxisd(kRotationAngles[i], kAxes[i]));
    static const double kModelNoise = 0.0;
    static const double kProjectionNoise = 0.0 / 512;
    static const double kMaxAllowedRotationDifference = DegToRad(1.0e-5);
    static const double kMaxAllowedTranslationDifference = 1.0e-5;
    TestTwoPointPoseWithNoise(points_3d,
                              kAxes[i],
                              kModelNoise,
                              kProjectionNoise,
                              rotation,
                              kTranslations[i],
                              kMaxAllowedRotationDifference,
                              kMaxAllowedTranslationDifference);
  }
}

// Tests many sets of model to image correspondences and multiple
// transformations consisting of translation and rotations around different
// axes, does not add any noise.
TEST(TwoPointPoseTest, ManyPointsTest) {
  static const double kModelNoise = 0.0;
  static const double kProjectionNoise = 0.0;
  static const double kMaxAllowedRotationDifference = DegToRad(1.0e-5);
  static const double kMaxAllowedTranslationDifference = 1.0e-5;

  ManyPointsTest(kModelNoise,
                 kProjectionNoise,
                 kMaxAllowedRotationDifference,
                 kMaxAllowedTranslationDifference);
}

// Tests many sets of model to image correspondences and multiple
// transformations consisting of translation and rotations around different
// axes, with noise on the image correspondeces.
TEST(TwoPointPoseTest, ManyPointsAndCorrespondenceNoiseTest) {
  static const double kModelNoise = 0.0;
  static const double kProjectionNoise = 0.5 / 512;
  static const double kMaxAllowedRotationDifference = DegToRad(10.0);
  static const double kMaxAllowedTranslationDifference = 5.0;

  ManyPointsTest(kModelNoise,
                 kProjectionNoise,
                 kMaxAllowedRotationDifference,
                 kMaxAllowedTranslationDifference);
}

// Tests many sets of model to image correspondences and multiple
// transformations consisting of translation and rotations around different
// axes, with noise on the model points.
TEST(TwoPointPoseTest, ManyPointsAndModelNoiseTest) {
  static const double kModelNoise = 0.01;
  static const double kProjectionNoise = 0.0;
  static const double kMaxAllowedRotationDifference = DegToRad(15.0);
  static const double kMaxAllowedTranslationDifference = 10.0;

  ManyPointsTest(kModelNoise,
                 kProjectionNoise,
                 kMaxAllowedRotationDifference,
                 kMaxAllowedTranslationDifference);
}

}  // namespace
}  // namespace theia


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

#include <Eigen/Dense>
#include <iostream>

#include <boost/random/mersenne_twister.hpp>
#include <boost/shared_ptr.hpp>

#include "sphere.hpp"
#include "clusterer.hpp"
#include "dir.hpp"
#include "cat.hpp"

using namespace Eigen;
using std::cout;
using std::endl;

template<class T>
class KMeans : public Clusterer<T>
{
public:
  KMeans(const boost::shared_ptr<Matrix<T,Dynamic,Dynamic> >& spx, uint32_t K,
    boost::mt19937* pRndGen);
  virtual ~KMeans();

//  void initialize(const Matrix<T,Dynamic,Dynamic>& x);

  virtual void updateLabels();
  virtual void updateCenters();
  virtual MatrixXu mostLikelyInds(uint32_t n, Matrix<T,Dynamic,Dynamic>& deviates);
  virtual T avgIntraClusterDeviation();

  virtual T dist(const Matrix<T,Dynamic,1>& a, const Matrix<T,Dynamic,1>& b);
  virtual bool closer(T a, T b);
  virtual uint32_t indOfClosestCluster(int32_t i);
  virtual Matrix<T,Dynamic,1> computeCenter(uint32_t k);

protected:
  Sphere<T> S_;  // TODO this should not be here - needed for the empty cluster
};

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

template<class T>
KMeans<T>::KMeans(
    const boost::shared_ptr<Matrix<T,Dynamic,Dynamic> >& spx, uint32_t K,
    boost::mt19937* pRndGen)
  : Clusterer<T>(spx,K, pRndGen), S_(this->D_)
{
  Matrix<T,Dynamic,1> alpha(this->K_);
  alpha.setOnes(this->K_);
  Dir<Cat<T>,T> dir(alpha,this->pRndGen_);
  Cat<T> pi = dir.sample();
  cout<<"init pi="<<pi.pdf().transpose()<<endl;
  pi.sample(this->z_);

  this->ps_.setZero();
//  updateCenters();
//  for(uint32_t k=0; k<this->K_; ++k)
//    this->ps_.col(k) = S_.sampleUnif(this->pRndGen_);
//  cout<<"init centers"<<endl<<this->ps_<<endl;
}

template<class T>
KMeans<T>::~KMeans()
{}

template<class T>
T KMeans<T>::dist(const Matrix<T,Dynamic,1>& a, const Matrix<T,Dynamic,1>& b)
{
  return (a-b).norm();
};

template<class T>
bool KMeans<T>::closer(T a, T b)
{
  return a<b; // if dist a is smaller than dist b a is closer than b (Eucledian)
};

template<class T>
uint32_t KMeans<T>::indOfClosestCluster(int32_t i)
{
  T sim_closest = dist(this->ps_.col(0), this->spx_->col(i));
  uint32_t z_i = 0;
  for(uint32_t k=1; k<this->K_; ++k)
  {
    T sim_k = dist(this->ps_.col(k), this->spx_->col(i));
    if( closer(sim_k, sim_closest))
    {
      sim_closest = sim_k;
      z_i = k;
    }
  }
  return z_i;
};

template<class T>
void KMeans<T>::updateLabels()
{
#pragma omp parallel for
  for(uint32_t i=0; i<this->N_; ++i)
  {
    this->z_(i) = indOfClosestCluster(i);
  }
}

template<class T>
Matrix<T,Dynamic,1> KMeans<T>::computeCenter(uint32_t k)
{
  this->Ns_(k) = 0.0;
  Matrix<T,Dynamic,1> mean_k(this->D_);
  mean_k.setZero(this->D_);
  for(uint32_t i=0; i<this->N_; ++i)
    if(this->z_(i) == k)
    {
      mean_k += this->spx_->col(i);
      this->Ns_(k) ++;
    }
  return mean_k/this->Ns_(k);
}

template<class T>
void KMeans<T>::updateCenters()
{
#pragma omp parallel for
  for(uint32_t k=0; k<this->K_; ++k)
  {
    this->ps_.col(k) = computeCenter(k);
    if (this->Ns_(k) <= 0)
      this->ps_.col(k) = S_.sampleUnif(this->pRndGen_);
  }
}

template<class T>
MatrixXu KMeans<T>::mostLikelyInds(uint32_t n,
    Matrix<T,Dynamic,Dynamic>& deviates)
{
  MatrixXu inds = MatrixXu::Zero(n,this->K_);
  deviates = Matrix<T,Dynamic,Dynamic>::Ones(n,this->K_);

#pragma omp parallel for
  for (uint32_t k=0; k<this->K_; ++k)
  {
    for (uint32_t i=0; i<this->N_; ++i)
      if(this->z_(i) == k)
      {
        T deviate = dist(this->ps_.col(k), this->spx_->col(i));
//        T deviate = (this->ps_.col(k) - this->spx_->col(i)).norm();
        for (uint32_t j=0; j<n; ++j)
          if(closer(deviate, deviates(j,k)))
          {
            for(uint32_t l=n-1; l>j; --l)
            {
              deviates(l,k) = deviates(l-1,k);
              inds(l,k) = inds(l-1,k);
            }
            deviates(j,k) = deviate;
            inds(j,k) = i;
//            cout<<"after update "<<logLike<<endl;
//            Matrix<T,Dynamic,Dynamic> out(n,this->K_*2);
//            out<<logLikes.cast<T>(),inds.cast<T>();
//            cout<<out<<endl;
            break;
          }
      }
  }
  cout<<"::mostLikelyInds: deviates"<<endl;
  cout<<deviates<<endl;
  cout<<"::mostLikelyInds: inds"<<endl;
  cout<<inds<<endl;
  return inds;
};

template<class T>
T KMeans<T>::avgIntraClusterDeviation()
{
  Matrix<T,Dynamic,1> deviates(this->K_);
  deviates.setZero(this->K_);
#pragma omp parallel for
  for (uint32_t k=0; k<this->K_; ++k)
  {
    this->Ns_(k) = 0.0;
    for (uint32_t i=0; i<this->N_; ++i)
      if(this->z_(i) == k)
      {
        deviates(k) += dist(this->ps_.col(k), this->spx_->col(i));
//        deviates(k) += (this->ps_.col(k) - this->spx_->col(i)).norm();
        this->Ns_(k) ++;
      }
    if(this->Ns_(k) > 0.0) deviates(k) /= this->Ns_(k);
  }
  return deviates.sum()/static_cast<T>(this->K_);
}


#include <boost/geometry/algorithms/convex_hull.hpp>


#include <boost/functional/hash.hpp>

#include "memoryrange.hh"

using namespace std;
using namespace boost::accumulators;

std::vector< MemoryRange > MemoryRange::bisect( void ) const
{
  vector< MemoryRange > ret { *this };

  /* bisect in each axis */
  for ( unsigned int i = 0; i < Memory::datasize; i++ ) {
    vector< MemoryRange > doubled;
    for ( const auto &x : ret ) {
      auto ersatz_lower( x._lower ), ersatz_upper( x._upper );
      ersatz_lower.mutable_field( i ) = ersatz_upper.mutable_field( i ) = median( _acc[ i ] );

      if ( x._lower == ersatz_upper ) {
  /* try range midpoint instead */
  ersatz_lower.mutable_field( i ) = ersatz_upper.mutable_field( i ) = (x._lower.field( i ) + x._upper.field( i )) / 2;
      }

      if ( x._lower == ersatz_upper ) {
	assert( !(ersatz_lower == x._upper) );
	assert( x._lower == ersatz_lower );
	/* cannot double on this axis */
	doubled.push_back( x );
      } else {
	doubled.emplace_back( x._lower, ersatz_upper );
	doubled.emplace_back( ersatz_lower, x._upper );
      }
    }

    ret = doubled;
  }

  assert( !ret.empty());

  return ret;
}

Memory MemoryRange::range_median( void ) const
{
  Memory median_data( _lower );
  for ( unsigned int i = 0; i < Memory::datasize; i++ ) {
    median_data.mutable_field( i ) = (_lower.field( i ) + _upper.field( i )) / 2;
  }
  return median_data;
}

bool MemoryRange::contains( const Memory & query ) const
{
  return (query >= _lower) && (query < _upper);
}

void MemoryRange::track( const Memory & query ) const
{
  /* log it */
  for ( unsigned int i = 0; i < Memory::datasize; i++ ) {
    _acc[ i ]( query.field( i ) );
  }
}

bool MemoryRange::operator==( const MemoryRange & other ) const
{
  return (_lower == other._lower) && (_upper == other._upper); /* ignore median estimator for now */
}

string MemoryRange::str( void ) const
{
  char tmp[ 256 ];
  snprintf( tmp, 256, "(lo=<%s>, hi=<%s>)",
	    _lower.str().c_str(),
	    _upper.str().c_str() );
  return tmp;
}

RemyBuffers::MemoryRange MemoryRange::DNA( void ) const
{
  RemyBuffers::MemoryRange ret;

  ret.mutable_lower()->CopyFrom( _lower.DNA() );
  ret.mutable_upper()->CopyFrom( _upper.DNA() );

  return ret;
}

MemoryRange::MemoryRange( const RemyBuffers::MemoryRange & dna )
  : _lower( true, dna.lower() ),
    _upper( false, dna.upper() ),
    _acc( Memory::datasize ),
    _count( 0 )
{}

size_t hash_value( const MemoryRange & mr )
{
  size_t seed = 0;
  boost::hash_combine( seed, mr._lower );
  boost::hash_combine( seed, mr._upper );

  return seed;
}