// Copyright (c) 2022 The BITRUBCOIN developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or https://www.opensource.org/licenses/mit-license.php.

#include "test/test_bitrubcoin.h"

#include "evo/deterministicmns.h"
#include "llmq/quorums_connections.h"

#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(net_quorums_tests)

std::vector<CDeterministicMNCPtr> createMNList(unsigned int size)
{
    std::vector<CDeterministicMNCPtr> mns;
    for (size_t i = 0; i < size; i++) {
        CDeterministicMN dmn(i);
        uint256 newProTxHash;
        do {
            newProTxHash = g_insecure_rand_ctx.rand256();
        } while (std::find_if(mns.begin(), mns.end(),
                [&newProTxHash](CDeterministicMNCPtr mn){ return mn->proTxHash == newProTxHash; }) != mns.end());
        dmn.proTxHash = newProTxHash;
        mns.emplace_back(std::make_shared<const CDeterministicMN>(dmn));
    }
    return mns;
}

void checkQuorumRelayMembers(const std::vector<CDeterministicMNCPtr>& list, unsigned int expectedResSize)
{
    for (size_t i = 0; i < list.size(); i++) {
        const auto& set = llmq::GetQuorumRelayMembers(list, i);
        BOOST_CHECK_MESSAGE(set.size() == expectedResSize,
                            strprintf("size %d, expected ret size %d, ret size %d ", list.size(), expectedResSize, set.size()));
        BOOST_CHECK(set.count(list[i]->proTxHash) == 0);
    }
}

BOOST_FIXTURE_TEST_CASE(get_quorum_relay_members, BasicTestingSetup)
{
    size_t list_size = 2000;    // n
    size_t relay_memb = 10;     // floor(log2(n-1))

    std::vector<CDeterministicMNCPtr> masternodes = createMNList(list_size);

    // Test quorum sizes 2000 to 2
    while (true) {
        checkQuorumRelayMembers(masternodes, relay_memb);

        masternodes.resize(--list_size);
        if (list_size == 1) break;
        // n=2 is a special case (1 relay member)
        // Otherwise relay members are 1 + max(1, floor(log2(n-1))-1)
        else if (list_size == 2 ||
                (list_size > 4 && (1 << relay_memb) >= (int)list_size)) relay_memb--;
    }
}

BOOST_AUTO_TEST_SUITE_END()


#ifdef DO_NOT_RUN_THIS_TEST
#include "test_data.hpp"
#include "../src/shared_operations.hpp"
#include <boost/test/unit_test.hpp>
#include <iostream>
#include <opencv2/ml.hpp>
#include <opencv/cxcore.hpp>
BOOST_AUTO_TEST_CASE(foo)
{
    using namespace cv;
    const auto& samples = ut_data::unittest_data();
    const auto& classes = ut_data::classes4ut();
    // insert code here...
    Ptr<ml::KNearest>  knn(ml::KNearest::create());
    //Mat_<float> trainFeatures(7,4);
    auto trainFeatures{mlmodels::detail::prepare_samples(samples)};
    auto trainLabels{mlmodels::detail::prepare_labels(classes)};
    /*trainFeatures << 0.2,0.2,0.2,0.2,
    0.3,0.3,0.3,0.3,
    0.4,0.4,0.4,0.4,
    0.5,0.5,0.5,0.5,
    0.6,0.6,0.6,0.6,
    1.7,1.7,1.7,1.7,
    1.3,3,2,2;

    Mat_<int> trainLabels(1,7);
    trainLabels << 0,1,2,3,4,5,0;*/

    knn->train(trainFeatures, ml::ROW_SAMPLE, trainLabels);

    const auto& test_samples = ut_data::samples4test();
    auto testFeature{mlmodels::detail::prepare_samples(test_samples)};
    //Mat_<float> testFeature(1,4);
    //testFeature<< 0.2,1.7,1.1,0.3;

    int K=8;
    Mat response,dist,p;
    knn->findNearest(testFeature, K, p, response, dist);
    std::cout <<"predictions:\n"<< p << std::endl;
    std::cout <<"reponses\n"<< response << std::endl;
    std::cout << "distances:\n"<<dist<< std::endl;
}

BOOST_AUTO_TEST_CASE(foo2)
{
    using namespace cv;
    Ptr<ml::KNearest>  knn(ml::KNearest::create());
    int rows = ut_data::rows_number - ut_data::rows4testing;
    Mat_<float> trainFeatures(rows, 3);
    const auto& samples = ut_data::unittest_data();
    const auto& classes = ut_data::classes4ut();
    //std::copy(std::begin(classes), std::end(classes), std::ostream_iterator<mlmodels::class_data::value_type>(std::cout, ", "));
    //return;
    /*trainFeatures << 0.2,0.2,0.2,0.2,
    0.3,0.3,0.3,0.3,
    0.4,0.4,0.4,0.4,
    0.5,0.5,0.5,0.5,
    0.6,0.6,0.6,0.6,
    1.7,1.7,1.7,1.7,
    1.3,3,2,2;
    */
    Mat_<int> trainLabels(rows, 1);

    /*trainLabels << 0,1,2,3,4,5,0;*/
    for (auto i = 0; i < rows; ++i) {
        trainLabels.at<int>(i, 0) = classes[i];
    }
    for (auto i = 0; i < rows; i++) {
        for (auto j = 0; j < 3; j++) {
            trainFeatures.at<float>(i, j) = samples[i][j];
        }
    }
    //std::cout<<"train data:\n"<<trainFeatures<<"\nclasses\n"<<trainLabels<<std::endl;
    knn->train(trainFeatures, ml::ROW_SAMPLE, trainLabels);

    const auto& test_samples = ut_data::samples4test();
    //auto testFeature{mlmodels::detail::prepare_samples(test_samples)};
    Mat_<float> testFeature((int)ut_data::rows4testing, 3);
    for (auto i = 0; i < testFeature.rows; i++) {
        for (auto j = 0; j < 3; j++) {
            testFeature.at<float>(i, j) = test_samples[i][j];
        }
    }
    //std::cout<<"test data:\n"<<testFeature<<std::endl;
    //testFeature<< 0.2,1.7,1.1,0.3;
    int K=8;
    Mat response, dist, p;
    knn->findNearest(testFeature, K, p, response, dist);
    std::cout <<"predictions:\n"<< p << std::endl;
    std::cout <<"reponses\n"<< response << std::endl;
    std::cout << "distances:\n"<<dist<< std::endl;
}
#endif


/***
* Copyright 2017 Marc Stevens <marc@marc-stevens.nl>, Dan Shumow <danshu@microsoft.com>
* Distributed under the MIT Software License.
* See accompanying file LICENSE.txt or copy at
* https://opensource.org/licenses/MIT
***/

#ifndef DISTURBANCEVECTOR_HPP
#define DISTURBANCEVECTOR_HPP

#include <string>
#include <vector>
#include <cstdint>
#include <boost/lexical_cast.hpp>
#include <boost/container/flat_map.hpp>

class disturbancevector {
public:
	typedef ::uint32_t uint32;
	uint32 DV[80];
	uint32 DW[80];
	int dvtype;
	int dvk;
	int dvb;

	// specifify DVtype=1,2 K=0-79, b=0-31
	explicit disturbancevector(int DVtype, int K, int b)
	{
		assign(DVtype, K, b);
	}

	// specificy 16 consecutive words DV[offset+0],...,DV[offset+15], offset=0,...,64
	explicit disturbancevector(uint32 disturbances[16], int offset)
	{
		assign(disturbances, offset);
	}

	// specify name: I(K,b) or II(K,b)
	explicit disturbancevector(std::string DVstr) {
		// DVstr = "I(K,b)" or "II(K,b)"
		int DVtype = 0;
		while (DVstr.size() > 0 && DVstr[0] == 'I') {
			++DVtype;
			DVstr.erase(0, 1);
		}
		if (DVtype == 0 || DVtype > 2) throw std::runtime_error("DV string incorrect: " + DVstr);
		// DVstr = "(K,b)"
		if (DVstr.size() == 0 || (DVstr[0] != '(' && DVstr[0] != '_')) throw std::runtime_error("DV string incorrect: " + DVstr);
		DVstr.erase(0,1);
		// DVstr = "K,b)"
		std::string::size_type pos = DVstr.find_first_of(",_");
		if (pos == std::string::npos) throw std::runtime_error("DV string incorrect: " + DVstr);
		int K = boost::lexical_cast<int>(DVstr.substr(0, pos));
		DVstr.erase(0,pos+1);
		// DVstr = "b)"
		pos = DVstr.find_first_not_of("0123456789");
		int b = boost::lexical_cast<int>(DVstr.substr(0, pos));

		assign(DVtype, K, b);
	}

	std::string name() const {
		for (int K = 0; K <= 64; ++K) {
			bool possible = true;
			for (int i = 4; i <= 14; ++i)
				if (DV[K+i] != 0) {
					possible = false;
					break;
				}
			if (!possible || hammingweight(DV[K+15]) != 1) continue;
			int b = 0;
			while ((DV[K+15]&(1<<b))==0) ++b;
			if (DV[K+1] == 0) { // check if it is I(K,b)
				if (DV[K+0]==0 && DV[K+1]==0 && DV[K+2]==0 && DV[K+3]==0)
					return "I(" + boost::lexical_cast<std::string>(K) + "," + boost::lexical_cast<std::string>(b) + ")";
			} else { // check if it is II(K,b)
				uint32 rb = rotate_left(uint32(1<<31),b);
				if (DV[K+0]==0 && DV[K+1]==rb && DV[K+2]==0 && DV[K+3]==rb)
					return "II(" + boost::lexical_cast<std::string>(K) + "," + boost::lexical_cast<std::string>(b) + ")";
			}
			// other unknown type with parameters K and b
			return "unknown(" + boost::lexical_cast<std::string>(K) + "," + boost::lexical_cast<std::string>(b) + ")";
		}
		return "unknown";
	}

	void assign(int DVtype, int K, int b)
	{
		dvtype = DVtype; dvk = K; dvb = b;
		if (DVtype < 1 || DVtype > 2 || K < 0 || K > 64 || b < 0 || b >= 32) throw std::runtime_error("bad disturbancevector specification");
		for (unsigned i = unsigned(K); i < unsigned(K + 16); ++i)
			DV[i] = 0;
		DV[K+15] = rotate_left(uint32(1),b);
		if (DVtype == 2) {
			DV[K+1] = rotate_left(uint32(1<<31),b);
			DV[K+3] = rotate_left(uint32(1<<31),b);
		}
		expand_me(DV, K);
		initDW();
	}
	void assign(uint32 disturbances[16], int offset)
	{
		dvtype = dvk = dvb = 0;
		if (offset < 0 || offset > 64) throw std::runtime_error("bad disturbances offset specification");
		for (unsigned i = 0; i < 16; ++i)
			DV[offset+i] = disturbances[i];
		expand_me(DV, offset);
		initDW();
	}
	// determine DW from DV
	void initDW() {
		// determine DW
		for (int i = 16; i < 32; ++i)
			DW[i] = DV[i-0] ^ rotate_left(DV[i-1],5) ^ DV[i-2] ^ rotate_left(DV[i-3],30) ^ rotate_left(DV[i-4],30) ^ rotate_left(DV[i-5],30);
		expand_me(DW, 16);
	}

	// make V an expanded message using the message expansion relation from the 16 sequential words starting at offset in V
	void expand_me(uint32 V[80], int offset) {
		for (int i = offset - 1; i >= 0; --i)
			V[i]=rotate_right(V[i+16], 1) ^ V[i+13] ^ V[i+8] ^ V[i+2];
		for (int i = offset + 16; i < 80; ++i)
			V[i]=rotate_left(V[i-3] ^ V[i-8] ^ V[i-14] ^ V[i-16], 1);
	}

	unsigned hammingweight(uint32 x) const {
		unsigned i = 0;
		while (x) {
			++i;
			x &= x-1;
		}
		return i;
	}
	uint32 rotate_left(uint32 x, unsigned n) const {
		return (x<<n)|(x>>(32-n));
	}
	uint32 rotate_right(uint32 x, unsigned n) const {
		return (x>>n)|(x<<(32-n));
	}
};

#endif // DISTURBANCEVECTOR_HPP


#include "benchmarker.h"

#include <vector>
#include <memory>

#include <QVector2D>
#include <QVector3D>
#include <QTime>

#include "spline_library/utils/arclength.h"
#include "spline_library/splines/generic_b_spline.h"
#include "spline_library/splines/uniform_cr_spline.h"
#include "spline_library/splines/natural_spline.h"

#include <boost/math/tools/roots.hpp>

Benchmarker::Benchmarker(QObject *parent)
    :QObject(parent), repeats(100)
{

}

QMap<QString, float> Benchmarker::runBenchmark(void)
{
    canceled = false;

    //create a distribution of random numbers, and a parameterles function to generate them
    std::uniform_real_distribution<FloatingT> distribution(10,15);
    auto randomSource = [this, &distribution]() {
        return distribution(this->gen);
    };

    //create the functions used to generate the splines
    auto crSpline = [this, randomSource](size_t size) {
        auto points = randomPoints_Uniform<VectorT, D, FloatingT>(randomSource, size);
        std::unique_ptr<SplineType> result = std::make_unique<LoopingUniformCRSpline<VectorT,FloatingT>>(points);
        return result;
    };

    auto genericBSpline = [this, randomSource](size_t size) {
        auto points = randomPoints_Uniform<VectorT, D, FloatingT>(randomSource, size);
        std::unique_ptr<SplineType> result = std::make_unique<LoopingGenericBSpline<VectorT,FloatingT>>(points, 7);
        return result;
    };

    QMap<QString, float> results;
    timeSplineMemberFunction(results, &Benchmarker::testArcLength, crSpline, "uniform_cr[10]",    10000, 12);
    timeSplineMemberFunction(results, &Benchmarker::testArcLength, crSpline, "uniform_cr[1000]",  1000, 1002);
    timeSplineMemberFunction(results, &Benchmarker::testArcLength, genericBSpline, "bspline[10]",    1000, 16);
    timeSplineMemberFunction(results, &Benchmarker::testArcLength, genericBSpline, "bspline[1000]",  100, 1006);

    return results;
}

void Benchmarker::cancel(void)
{
    canceled = true;
}

void Benchmarker::timeSplineMemberFunction(
        QMap<QString, float>& results,
        void(Benchmarker::*testFunction)(int, const SplineType&) ,
        std::function<std::unique_ptr<SplineType>(size_t)> splineFunction,
        QString message, int queries, size_t size) {

    emit setProgressText(message);
    emit setProgressRange(0, repeats);

    int totalElapsed = 0;
    gen.seed(10);
    QTime t;

    for(int i = 0; i < repeats; i++) {
        if(canceled) return;

        emit setProgressValue(i);
        auto spline = splineFunction(size);

        t.start();
        (this->*testFunction)(queries, *spline);
        totalElapsed += t.elapsed();
    }
    emit setProgressValue(repeats);

    results[message] = 1000 * float(totalElapsed) / (repeats * queries);
}

void Benchmarker::testArcLength(int queries, const LoopingSpline<VectorT, FloatingT> &spline)
{
    std::uniform_real_distribution<FloatingT> dist(0, spline.getMaxT());

    for(int q = 0; q < queries; q++)
    {
        FloatingT a = dist(gen);
        FloatingT b = dist(gen);

        spline.cyclicArcLength(a,b);
    }
}


#pragma once

#include <vector>
#include <fstream>

#include <Eigen/Core>

#include <ipc/ccd/ccd.hpp>

namespace ipc {

/// Virtual class for candidates that support CCD.
struct ContinuousCollisionCandidate {
    virtual ~ContinuousCollisionCandidate() { }

    /// Perform narrow-phase CCD on the candidate.
    /// @param[in] V0 Mesh vertex positions at the start of the time step.
    /// @param[in] V1 Mesh vertex positions at the end of the time step.
    /// @param[in] E Mesh edges as rows of indicies into V.
    /// @param[in] F Mesh triangular faces as rows of indicies into V.
    /// @param[out] toi Computed time of impact (normalized).
    /// @param[in] tmax Maximum time (normalized) to look for collisions.
    ///                 Should be in [0, 1].
    /// @param[in] tolerance CCD tolerance used by Tight-Inclusion CCD.
    /// @param[in] max_iterations Maximum iterations used by Tight-Inclusion
    ///                           CCD.
    /// @param[in] conservative_rescaling Conservative rescaling value used to
    ///                                   avoid taking steps exactly to impact.
    /// @return If the candidate had a collision over the time interval.
    virtual bool
    ccd(const Eigen::MatrixXd& V0,
        const Eigen::MatrixXd& V1,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F,
        double& toi,
        const double tmax = 1.0,
        const double tolerance = DEFAULT_CCD_TOLERANCE,
        const long max_iterations = DEFAULT_CCD_MAX_ITERATIONS,
        const double conservative_rescaling =
            DEFAULT_CCD_CONSERVATIVE_RESCALING) const = 0;
};

struct VertexVertexCandidate {
    VertexVertexCandidate(long vertex0_index, long vertex1_index);

    bool operator==(const VertexVertexCandidate& other) const;
    bool operator!=(const VertexVertexCandidate& other) const
    {
        return !(*this == other);
    }

    /// @brief Compare EdgeVertexCandidates for sorting.
    bool operator<(const VertexVertexCandidate& other) const;

    long vertex0_index;
    long vertex1_index;
};

struct EdgeVertexCandidate : ContinuousCollisionCandidate {
    EdgeVertexCandidate(long edge_index, long vertex_index);

    bool operator==(const EdgeVertexCandidate& other) const;
    bool operator!=(const EdgeVertexCandidate& other) const
    {
        return !(*this == other);
    }

    /// @brief Compare EdgeVertexCandidates for sorting.
    bool operator<(const EdgeVertexCandidate& other) const;

    bool
    ccd(const Eigen::MatrixXd& V0,
        const Eigen::MatrixXd& V1,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F,
        double& toi,
        const double tmax = 1.0,
        const double tolerance = DEFAULT_CCD_TOLERANCE,
        const long max_iterations = DEFAULT_CCD_MAX_ITERATIONS,
        const double conservative_rescaling =
            DEFAULT_CCD_CONSERVATIVE_RESCALING) const override;

    long edge_index;
    long vertex_index;
};

struct EdgeEdgeCandidate : ContinuousCollisionCandidate {
    EdgeEdgeCandidate(long edge0_index, long edge1_index);

    bool operator==(const EdgeEdgeCandidate& other) const;
    bool operator!=(const EdgeEdgeCandidate& other) const
    {
        return !(*this == other);
    }

    /// @brief Compare EdgeEdgeCandidates for sorting.
    bool operator<(const EdgeEdgeCandidate& other) const;

    bool
    ccd(const Eigen::MatrixXd& V0,
        const Eigen::MatrixXd& V1,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F,
        double& toi,
        const double tmax = 1.0,
        const double tolerance = DEFAULT_CCD_TOLERANCE,
        const long max_iterations = DEFAULT_CCD_MAX_ITERATIONS,
        const double conservative_rescaling =
            DEFAULT_CCD_CONSERVATIVE_RESCALING) const override;

    long edge0_index;
    long edge1_index;
};

/// @brief Candidate for <b>intersection</b> between edge and face.
///
/// Not included in Candidates because it is not a collision candidate.
struct EdgeFaceCandidate {
    EdgeFaceCandidate(long edge_index, long face_index);

    bool operator==(const EdgeFaceCandidate& other) const;
    bool operator!=(const EdgeFaceCandidate& other) const
    {
        return !(*this == other);
    }

    /// @brief Compare EdgeFaceCandidate for sorting.
    bool operator<(const EdgeFaceCandidate& other) const;

    long edge_index;
    long face_index;
};

struct FaceVertexCandidate : ContinuousCollisionCandidate {
    FaceVertexCandidate(long face_index, long vertex_index);

    bool operator==(const FaceVertexCandidate& other) const;
    bool operator!=(const FaceVertexCandidate& other) const
    {
        return !(*this == other);
    }

    /// @brief Compare FaceVertexCandidate for sorting.
    bool operator<(const FaceVertexCandidate& other) const;

    bool
    ccd(const Eigen::MatrixXd& V0,
        const Eigen::MatrixXd& V1,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F,
        double& toi,
        const double tmax = 1.0,
        const double tolerance = DEFAULT_CCD_TOLERANCE,
        const long max_iterations = DEFAULT_CCD_MAX_ITERATIONS,
        const double conservative_rescaling =
            DEFAULT_CCD_CONSERVATIVE_RESCALING) const override;

    long face_index;
    long vertex_index;
};

struct Candidates {
    std::vector<EdgeVertexCandidate> ev_candidates;
    std::vector<EdgeEdgeCandidate> ee_candidates;
    std::vector<FaceVertexCandidate> fv_candidates;

    size_t size() const;

    bool empty() const;

    void clear();

    ContinuousCollisionCandidate& operator[](size_t idx);
    const ContinuousCollisionCandidate& operator[](size_t idx) const;

    bool save_obj(
        const std::string& filename,
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F);
};

////////////////////////////////////////////////////////////////////////////////
// Debugging functions

void save_obj(
    std::ofstream& out,
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const std::vector<EdgeVertexCandidate>& ev_candidates);
void save_obj(
    std::ofstream& out,
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const std::vector<EdgeEdgeCandidate>& ee_candidates);
void save_obj(
    std::ofstream& out,
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const std::vector<FaceVertexCandidate>& fv_candidates);
void save_obj(
    std::ofstream& out,
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const std::vector<EdgeFaceCandidate>& ef_candidates);

template <typename Candidate>
bool save_obj(
    const std::string& filename,
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const std::vector<Candidate>& candidates)
{
    std::ofstream obj(filename);
    if (!obj.is_open()) {
        return false;
    }
    save_obj(obj, V, E, F, candidates);
    return true;
}

} // namespace ipc


#include "day9.h"

#include <iostream>
#include <fstream>
#include <string>

#include <cmath>
//#include <ctype>

#include <vector>
#include <deque>
using namespace std;

#include <boost/multiprecision/cpp_int.hpp>  // uses boost's bigint
namespace mp = boost::multiprecision;

const int INSTRUCTION_END = -1;

// ---------------- Private Methods ---------------- //

// instruction % (10^2 * 10^n) // n > 0
// pos = 1 is 1st parameter's mode.
int IntcodeComputer::GetParamMode(int pos) {
    return (int)(_currentInstruction % (100 * (int)pow(10, pos))) / (10 * (int)pow(10, pos));
}

// This function supports parameter modes.
mp::int128_t IntcodeComputer::GetValueAt(int pos) {
    int index = _instructionPointer;  // index in the program

    int parameterMode = GetParamMode(pos);
    if(parameterMode == 0)  {
        return _program[ (int)_program[index + pos] ];  // position mode
    } else if(parameterMode == 1) {
        return _program[ index + pos ];  // immediate mode
    } else if(parameterMode == 2) {
        return _program[ _relativeBase + (int)_program[index + pos] ];  // relative mode
    } else {
        cout << "Error: invalid GetValueAt() parameter mode.\n";
        return -1;  // -1 just means error.
    }
}

// This function supports parameter modes.
void IntcodeComputer::SetValueAt(int pos, mp::int128_t value) {
    int index = _instructionPointer;  // index in the program

    int parameterMode = GetParamMode(pos);
    int address = -1;
    if(parameterMode == 0) {
        address = (int)_program[index + pos]; // position mode
    } else if(parameterMode == 1) {
        cout << "SetValueAt() parameter mode Error: not allowed setting parameter mode 1!\n";
        //int address = index + pos;  // immediate mode
    } else if(parameterMode == 2) {
        address = _relativeBase + (int)_program[index + pos]; // relative mode
    } else {
        cout << "Error: invalid SetValueAt() parameter mode.\n";
    }

    //if( address >  )
        _program[ address ] = value;
}

void IntcodeComputer::RunOpCode() {
    _currentInstruction = _program[(int)_instructionPointer];  // update current instruction

    int opcode = (int)(_currentInstruction % (mp::int128_t)100);  // gets last two digits of number
    switch (opcode) {
    case 1: {
        mp::int128_t value = GetValueAt(1) + GetValueAt(2);
        SetValueAt(3, value);
        _instructionPointer += 4;
        return;

    } case 2: {
        mp::int128_t value = GetValueAt(1) * GetValueAt(2);
        SetValueAt(3, value);
        _instructionPointer += 4;
        return;

    } case 3: { // in
        mp::int128_t value = _inputQueue.front();
        _inputQueue.pop_front();
        SetValueAt(1, value);
        _instructionPointer += 2;
        return;

    } case 4: { // out
        _outputQueue.push_back( GetValueAt(1) );
        _instructionPointer += 2;
        return;

    } case 5: { // jump if true
        if( GetValueAt(1) != 0)
            _instructionPointer = (int)GetValueAt(2);
        else
            _instructionPointer += 3;
        return;

    } case 6: { // jump if false
        if( GetValueAt(1) == 0)
            _instructionPointer = (int) GetValueAt(2);
        else
            _instructionPointer += 3;
        return;

    } case 7: { // lt
        mp::int128_t value = GetValueAt(1) < GetValueAt(2) ? 1 : 0;
        SetValueAt(3, value);
        _instructionPointer += 4;
        return;

    } case 8: {// equal
        mp::int128_t value = GetValueAt(1) == GetValueAt(2) ? 1 : 0;
        SetValueAt(3, value);
        _instructionPointer += 4;
        return;

    } case 9: { // adjust relative base
        _relativeBase += (int) GetValueAt(1);
        _instructionPointer += 2;
        return;

    } case 99: { // exit
        if(_isDebug) cout << "graceful exit\n";
        _instructionPointer = -1;
        return;

    } default: {
        cout << "something went wrong, _currentInstruction=" << _currentInstruction;
        cout << ", _instructionPointer=" << _instructionPointer << "\n";
        _instructionPointer = -1;
        return;
    }}
}

// Private printers
void IntcodeComputer::PrintData(deque<mp::int128_t> const& input) {
    for(unsigned int i = 0; i < input.size(); i++) {
        cout << input[i] << ", ";
    }
    cout << "\n";
}

void IntcodeComputer::PrintData(vector<mp::int128_t> const& input) {
    for(unsigned int i = 0; i < input.size(); i++) {
        cout << input[i] << ", ";
    }
    cout << "\n";
}

// ---------------- Public Methods ---------------- //

IntcodeComputer::IntcodeComputer(bool isDebug) {
    _isDebug = isDebug;
    _isReset = false;
}

// Can be called to re-init the program. (Flushes input and output)
void IntcodeComputer::LoadProgram(string filename) {
    // clear the program backup.
    vector<mp::int128_t> empty;
    swap( _programBackup, empty );

    ifstream file(filename);

    // fill vector
    string str;
    while (getline(file, str, ',')) {
        _programBackup.push_back( stoi(str) );
    }

    ResetProgram(true);
}

void IntcodeComputer::RunProgram() {
    if(!_isReset) ResetProgram();

    // loop until program exits. (will stop when iterations overflows ~a few million operations)
    int32_t iter = 0;
    while(_instructionPointer != INSTRUCTION_END && iter >= 0) {
        RunOpCode();
    }

    if(iter < 0) cout << "Error: Program counter overflowed; likely an infinite loop\n";

    _isReset = false;
}

void IntcodeComputer::FlushInput() {
    deque<mp::int128_t> empty;
    swap( _inputQueue, empty );
}

void IntcodeComputer::FlushOutput() {
    deque<mp::int128_t> empty;
    swap( _outputQueue, empty );
}

void IntcodeComputer::ResetProgram(bool flushQueues /*=true*/) {
    _program = _programBackup;

    _relativeBase = 0;  // move _relativeBase to start as well.
    _instructionPointer = 0;  // move instruction pointer to start.

    FlushInput();
    FlushOutput();

    _isReset = true;
}

void IntcodeComputer::EnqueInput(mp::int128_t val) {
    _inputQueue.push_back(val);
}

mp::int128_t IntcodeComputer::DequeueOutput() {
    mp::int128_t val = _inputQueue.front();
    _inputQueue.pop_front();
    return val;
}

// ---------------- Printing Methods (Public) ---------------- //

void IntcodeComputer::PrintInput() {
    PrintData(_inputQueue);
}

void IntcodeComputer::PrintOutput() {
    PrintData(_outputQueue);
}

void IntcodeComputer::PrintProgram(bool isBackup /*=false*/) {
    if(!isBackup)
        PrintData(_program);
    else
        PrintData(_programBackup);
}


////////////////////////////////////////////////////////////////////////////////////
// distribution::toolkit::distributions::students_t::include.hpp                   //
//                                                                                //
//  (C) Copyright 2009 Erwann Rogard                                              //
//  Use, modification and distribution are subject to the                         //
//  Boost Software License, Version 1.0. (See accompanying file                   //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)              //
////////////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_STUDENTS_T_INCLUDE_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_STUDENTS_T_INCLUDE_HPP_ER_2009

#include <boost/math/distributions/students_t.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/students_t/description.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/students_t/derivative_log_unnormalized_pdf.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/students_t/is_math_distribution.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/students_t/log_unnormalized_pdf.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/students_t/random.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/students_t/os.hpp>

#endif


// 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_HEISENBERG_HPP
#define NETKET_HEISENBERG_HPP

#include <mpi.h>
#include <Eigen/Dense>
#include <iostream>
#include <vector>
#include "Graph/graph.hpp"
#include "Hilbert/abstract_hilbert.hpp"
#include "abstract_operator.hpp"

namespace netket {

// Heisenberg model on an arbitrary graph

class Heisenberg : public AbstractOperator {
  const AbstractHilbert &hilbert_;
  const AbstractGraph &graph_;

  const int nspins_;
  double offdiag_;

  // list of bonds for the interaction part
  std::vector<std::vector<int>> bonds_;

 public:
  using VectorType = AbstractOperator::VectorType;
  using VectorRefType = AbstractOperator::VectorRefType;
  using VectorConstRefType = AbstractOperator::VectorConstRefType;

  explicit Heisenberg(const AbstractHilbert &hilbert)
      : hilbert_(hilbert), graph_(hilbert.GetGraph()), nspins_(hilbert.Size()) {
    Init();
  }

  void Init() {
    if (graph_.IsBipartite()) {
      offdiag_ = -2;
    } else {
      offdiag_ = 2;
    }

    GenerateBonds();

    InfoMessage() << "Heisenberg model created " << std::endl;
  }

  void GenerateBonds() {
    auto adj = graph_.AdjacencyList();

    bonds_.resize(nspins_);

    for (int i = 0; i < nspins_; i++) {
      for (auto s : adj[i]) {
        if (s > i) {
          bonds_[i].push_back(s);
        }
      }
    }
  }

  void FindConn(VectorConstRefType v, std::vector<std::complex<double>> &mel,
                std::vector<std::vector<int>> &connectors,
                std::vector<std::vector<double>> &newconfs) const override {
    connectors.clear();
    connectors.resize(1);
    newconfs.clear();
    newconfs.resize(1);
    mel.resize(1);

    // computing interaction part Sz*Sz
    mel[0] = 0.;
    connectors[0].resize(0);
    newconfs[0].resize(0);

    for (int i = 0; i < nspins_; i++) {
      for (auto bond : bonds_[i]) {
        // interaction part
        mel[0] += v(i) * v(bond);

        // spin flips
        if (v(i) != v(bond)) {
          connectors.push_back(std::vector<int>({i, bond}));
          newconfs.push_back(std::vector<double>({v(bond), v(i)}));
          mel.push_back(offdiag_);
        }
      }
    }
  }

  const AbstractHilbert &GetHilbert() const noexcept override {
    return hilbert_;
  }
};

}  // namespace netket

#endif


///////////////////////////////////////////////////////////////////////////////
// crtp.hpp                                                                  //
//                                                                           //
//  Copyright 2010 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_CONTINGENCY_TABLE_PEARSON_CHISQ_CRTP_HPP_ER_2010
#define BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_CONTINGENCY_TABLE_PEARSON_CHISQ_CRTP_HPP_ER_2010
#include <boost/math/policies/policy.hpp>
#include <boost/math/distributions/chi_squared.hpp>
#include <boost/statistics/detail/non_parametric/contingency_table/pearson_chisq/common/degrees_of_freedom.hpp>
#include <boost/statistics/detail/non_parametric/contingency_table/cells/cells_count.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace contingency_table{

    template<typename D,typename Keys,typename T1>
    struct crtp
    {
         crtp(){}

        typedef boost::math::policies::policy<> default_policy_;

        const D& derived()const{ return static_cast<const D&>(*this); }

        // statistic :
        template<typename AccSet,typename P>
        T1 statistic(const AccSet& acc,const P& policy)const{
            return this->derived().statistic_impl( acc, policy );
        }
        template<typename AccSet>
        T1 statistic(const AccSet& acc)const
        {
            return this->statistic( acc, default_policy_() );
        }

        // lost_degrees_of_freedom :
        template<typename AccSet,typename P>
        typename AccSet::weight_type
        lost_degrees_of_freedom(const AccSet& acc,const P& policy)const
        {
            return this->derived().lost_degrees_of_freedom_impl( acc, policy );
        }
        template<typename AccSet>
        typename AccSet::weight_type
        lost_degrees_of_freedom(const AccSet& acc)const
        {
            return this->lost_degrees_of_freedom( acc, default_policy_() );
        }

        // degrees_of_freedom
        template<typename AccSet,typename P>
        typename AccSet::weight_type
        degrees_of_freedom(const AccSet& acc,const P& policy)const
        {
            namespace ct = contingency_table;
            namespace ps = ct::pearson_chi_square_statistic;
            typedef typename AccSet::weight_type weight_;
            weight_ df =  ps::degrees_of_freedom<weight_>(
                ct::cells_count<Keys>( acc ),
                this->derived().lost_degrees_of_freedom_impl( acc, policy )
            );

        }

        template<typename AccSet>
        typename AccSet::weight_type
        degrees_of_freedom(const AccSet& acc)const
        {
            return this->degrees_of_freedom( acc, default_policy_() );
        }

        // asy_distribution

        template<typename P = default_policy_>
        struct result_of_asy_distribution
        {
            typedef boost::math::chi_squared_distribution<T1, P> type;
        };

        template<typename AccSet,typename P>
        typename result_of_asy_distribution<P>::type
        asy_distribution(const AccSet& acc,const P& policy)
        {
            typedef typename result_of_asy_distribution<P>::type result_;
            typedef typename AccSet::weight_type weight_;
            typedef boost::numeric::converter<T1, weight_> converter_;
            T1 df = converter_::convert(
                this->degrees_of_freedom( acc, policy ) );
            return result_( df );
        }

        template<typename AccSet>
        typename result_of_asy_distribution<>::type
        asy_distribution(const AccSet& acc)
        {
            return this->asy_distribution( acc, default_policy_() );
        }

    };

}// contingency_table
}// detail
}// statistics
}// boost

#endif


//=========================================================================
// Nyx Solver - Non Linear Problems Solver
//
// Copyright 2018 Pierre Guilbert
// Author: Pierre Guilbert (spguilbert@gmail.com)
// Data: 02-11-2018
//
// 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
//
// 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.
//=========================================================================

// LOCAL
#include "main_nyx_test.h"
#include "Function_test.h"
#include "Jacobian_test.h"
#include "NumericalDiff_test.h"
#include "NewtonRaphson_test.h"
#include "LevenbergMarquardt_test.h"

// STD
#include <iostream>

// Eigen
#include <Eigen/Dense>

//-------------------------------------------------------------------------
int main(int argc, char *argv[])
{
  int nbrErr = 0;

  // Function tests
  nbrErr += TestFunction();

  // Jacobian tests
  nbrErr += TestJacobian();

  // Numerical Differentiation tests
  nbrErr += NumericalDiffSquareRoot();
  nbrErr += NumericalDiffEulerAngleMapping();
  nbrErr += NumericalDiffNonLinearFunction();
  nbrErr += NumericalDiffMethods();

  // Newton Raphson methos tests
  nbrErr += TestNewtonRaphsonMethod();
  nbrErr += TestNewtonRaphsonMethod2();

  // Levenberg-Marquardt tests
  //nbrErr += TestLevenbergMarquardtCosineFunction();

  if (nbrErr == 0)
  {
    std::cout << __func__ << " SUCCEEDED" << std::endl;
    return EXIT_SUCCESS;
  }
  else
  {
    std::cout << __func__ << " FAILED, nbrErr: " << nbrErr << std::endl;
    return EXIT_FAILURE;
  }

  return EXIT_SUCCESS;
}


/**
 * Some common calculations are defined here
 */

#ifndef WAVE_TWIST_JACOBIANS_HPP
#define WAVE_TWIST_JACOBIANS_HPP

#include <Eigen/Core>

namespace wave_optimization {

template<typename Derived, typename MDerived, typename VDerived, typename V1Derived, typename JDerived>
inline void calcInFrameInterpolation(const Eigen::MatrixBase<Derived> &hat,
                                     const Eigen::MatrixBase<Derived> &candle,
                                     const Eigen::MatrixBase<MDerived> &Jlog,
                                     const Eigen::MatrixBase<MDerived> &AdTkp1,
                                     const Eigen::MatrixBase<VDerived> &Xk,
                                     const Eigen::MatrixBase<VDerived> &Xkp1,
                                     Eigen::MatrixBase<V1Derived> &Tint,
                                     Eigen::MatrixBase<JDerived> *JT_Xk = nullptr,
                                     Eigen::MatrixBase<JDerived> *JT_Xkp1 = nullptr) {
    EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Eigen::MatrixBase<Derived>, 1, 2)
    EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Eigen::MatrixBase<MDerived>, 6, 6)
    EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Eigen::MatrixBase<VDerived>, 12, 1)
    EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Eigen::MatrixBase<V1Derived>, 6, 1)
    EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Eigen::MatrixBase<JDerived>, 6, 12)

    Tint.noalias() = hat(0, 1) * Xk.template block<6, 1>(6, 0) +
    candle(0, 0) * Jlog * (Xkp1.template block<6, 1>(0, 0) - AdTkp1 * Xk.template block<6, 1>(0, 0)) +
    candle(0, 1) * Jlog * Xkp1.template block<6, 1>(6, 0);

    if(JT_Xk) {
        JT_Xk->template block<6, 6>(0,0).noalias() = (candle(0,0) * Jlog) *(-AdTkp1);
        JT_Xk->template block<6, 6>(0,6).noalias() = hat(0,1) * Eigen::MatrixBase<MDerived>::Identity();
    }
    if(JT_Xkp1) {
        JT_Xkp1->template block<6, 6>(0,0).noalias() = candle(0,0) * Jlog;
        JT_Xkp1->template block<6, 6>(0,6).noalias() = candle(0,1) * Jlog;
    }
}

}

#endif //WAVE_TWIST_JACOBIANS_HPP


/*!
 * \file ProjSpatialScheme.hpp
 * \author Jun Yoshida
 * \copyright (c) 2019 Jun Yoshida.
 * The project is released under the MIT License.
 * \date Descember 12, 2019: created
 */

#pragma once

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

#include "PathScheme.hpp"
#include "utils.hpp"

/*! An implementation of PathScheme for 3-dimensional paths based on 2-dimensional one.
 * \tparam T A base implementaion of PathScheme for 2-dimensional paths. Ensure that it is an implementation of
 */
template <
    class T,
    class = decltype(typename T::vertex_type{std::declval<double>(),std::declval<double>()})
    >
class ProjSpatialScheme : public AdapterScheme<T,Eigen::Vector3d>
{
    template <class, class>
    friend class ProjSpatialScheme;

public:
    using vertex_type = Eigen::Vector3d;
    using typename AdapterScheme<T,Eigen::Vector3d>::BaseScheme;
    using BaseVertexType = typename BaseScheme::vertex_type;

private:
    Eigen::Matrix<double,2,3> m_mat_proj;
    Eigen::Vector3d m_focus;

    // State stack for save/restore
    std::vector<std::pair<Eigen::Matrix<double,2,3>,Eigen::Vector3d>> m_ststack{};

public:
    //! Construct from an instance of the base scheme
    ProjSpatialScheme(Eigen::Matrix<double,2,3> const& projmat,
                      Eigen::Vector3d const& focus,
                      BaseScheme *base)
        : AdapterScheme<BaseScheme,Eigen::Vector3d>{base, std::bind(&ProjSpatialScheme::project, this, std::placeholders::_1)}, m_mat_proj(projmat),  m_focus(focus)
    {}

    //! Direct construction of an instance of the base scheme.
    template <class... Ts>
    ProjSpatialScheme(Eigen::Matrix<double,2,3> const& projmat,
                      Eigen::Vector3d const & focus, Ts&&... args)
        : ProjSpatialScheme(projmat, focus, new BaseScheme(std::forward<Ts>(args)...))
    {}

    virtual ~ProjSpatialScheme() = default;

    //! Create a new instance with the same state.
    template<class U, class... Args>
    ProjSpatialScheme<U> mimic(Args&&... args) const {
        auto scheme = ProjSpatialScheme<U>(m_focus, new U(std::forward<Args>(args)...));
        scheme.m_mat_proj = m_mat_proj;
        scheme.m_ststack = m_ststack;
        return scheme;
    }

    // Apply the translation and the projection to a given vector.
    BaseVertexType project(Eigen::Vector3d const &p)
    {
        std::array<double,2> center = PathScheme<Eigen::Vector3d>::getCenter();
        Eigen::Vector2d result
            = Eigen::Vector2d(center[0], center[1]) + m_mat_proj * (p-m_focus);

        return BaseVertexType{result(0), result(1)};
    }

    //! Compute the depth vector, i.e. the kernel of the projection matrix.
    Eigen::Vector3d getDepthVec() const noexcept {
        Eigen::Vector3d depth_vec = m_mat_proj.fullPivLu().kernel();
        depth_vec.normalize();
        return depth_vec;
    }

    /*
    //! Set the matrix to an orthoprojection.
    void ortho(double elev, double azim) {
        double t = M_PI*elev/180.0;
        double u = M_PI*azim/180.0;

        m_mat_proj <<
            cos(u),       -sin(u), 0,
         // cos(t)*sin(u), cos(t)*cos(u), -sin(t),
            sin(t)*sin(u), sin(t)*cos(u), cos(t);
    }

    //! Set the matrix to a cabinet projection.
    void cabinet(double angle, double depth_factor) {
        double t = M_PI*angle/180.0;

        m_mat_proj <<
            1.0, depth_factor*cos(t), 0.0,
            0.0, depth_factor*sin(t), 1.0;
    }
    */

    /*!
     * Overriding methods.
     */
    //@{
    virtual void translate(vertex_type const &p) noexcept override {
        //auto p2d = project(p);
        //mp_base->translate(BaseVertexType{p2d(0), p2d(1)});
        m_focus -= p;
    }

    virtual void save() override {
        m_ststack.emplace_back(m_mat_proj, m_focus);
        AdapterScheme<BaseScheme,Eigen::Vector3d>::save();
    }

    virtual void restore() override {
        m_mat_proj = m_ststack.back().first;
        m_focus = m_ststack.back().second;
        AdapterScheme<BaseScheme,Eigen::Vector3d>::restore();
    }
    //@}
};


#include <boost/math/tools/series.hpp>


#pragma once

/// @file

#include "linear_solver.hpp"

#include <Eigen/PaStiXSupport>

namespace neon
{
/// PaStiXLDLT is a supernodal direct solver with multithreading support.  This
/// performs the LDLT Cholesky factorisation of a symmetric system
class PaStiXLDLT : public direct_linear_solver
{
public:
    PaStiXLDLT();

    void solve(sparse_matrix const& A, vector& x, vector const& b) override final;

private:
    Eigen::PastixLDLT<Eigen::SparseMatrix<double>, Eigen::Upper> ldlt;
};

/// PaStiXLU is a supernodal direct solver with multithreading support.  This
/// performs the LU factorisation of a square matrix system
class PaStiXLU : public direct_linear_solver
{
public:
    PaStiXLU();

    void solve(sparse_matrix const& A, vector& x, vector const& b) override final;

private:
    // BUG Likely not going to work with unsymmetric matrix because of row and
    // column ordering change.  Should give the transpose of the matrix but
    // unsure why Eigen can't handle this
    Eigen::PastixLU<Eigen::SparseMatrix<double>> lu;
};
}


#ifndef ASLAM_CAMERAS_PINHOLE_PROJECTION_HPP
#define ASLAM_CAMERAS_PINHOLE_PROJECTION_HPP

#include <aslam/cameras/GridCalibrationTargetObservation.hpp>
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/split_member.hpp>
#include <boost/serialization/version.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/core/eigen.hpp>
#include <sm/PropertyTree.hpp>
#include <sm/boost/serialization.hpp>
#include <sm/kinematics/Transformation.hpp>
#include <sm/logging.hpp>
#include "StaticAssert.hpp"

namespace aslam {
namespace cameras {

/**
 * @brief
 *
 * @note NOTE by CC
 *  The frame defined in `kalibr`
 *    (1) Keypoint frame: the image frame F_uv
 *    (2) Euclidean frame: the camera frame F_C, sometime it means the normalized camera frame
 */
template <typename DISTORTION_T>
class PinholeProjection {
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    enum { KeypointDimension = 2 };

    enum { IntrinsicsDimension = 4 };
    enum { DesignVariableDimension = IntrinsicsDimension };

    typedef DISTORTION_T distortion_t;
    typedef Eigen::Matrix<double, KeypointDimension, 1> keypoint_t;
    typedef Eigen::Matrix<double, KeypointDimension, IntrinsicsDimension> jacobian_intrinsics_t;

    /// \brief Default constructor
    PinholeProjection();

    PinholeProjection(double focalLengthU, double focalLengthV, double imageCenterU, double imageCenterV,
                      int resolutionU, int resolutionV, distortion_t distortion);

    PinholeProjection(double focalLengthU, double focalLengthV, double imageCenterU, double imageCenterV,
                      int resolutionU, int resolutionV);

    PinholeProjection(const sm::PropertyTree& config);

    /// \brief destructor.
    virtual ~PinholeProjection();

    template <typename DERIVED_P, typename DERIVED_K>
    bool euclideanToKeypoint(const Eigen::MatrixBase<DERIVED_P>& p,
                             const Eigen::MatrixBase<DERIVED_K>& outKeypoint) const;

    /**
     * @brief This is this reprojection procedure, reproject point from world frame(p, 3x1 vector) to image
     * frame(outKeypoint, 2x1 vector), include distortion and projection. And then calculate the Jacobian of outKeypoint
     * w.r.t to p. The Jacobian calculation using the chain rule, first calculate the Jacobian(J1) of distorted point
     * w.r.t p, then calculate the Jacobians(J2) of outKeypoint w.r.t distorted point, so the final Jacobian J = J2 * J1
     */
    template <typename DERIVED_P, typename DERIVED_K, typename DERIVED_JP>
    bool euclideanToKeypoint(const Eigen::MatrixBase<DERIVED_P>& p, const Eigen::MatrixBase<DERIVED_K>& outKeypoint,
                             const Eigen::MatrixBase<DERIVED_JP>& outJp) const;

    template <typename DERIVED_P, typename DERIVED_K>
    bool homogeneousToKeypoint(const Eigen::MatrixBase<DERIVED_P>& p,
                               const Eigen::MatrixBase<DERIVED_K>& outKeypoint) const;

    template <typename DERIVED_P, typename DERIVED_K, typename DERIVED_JP>
    bool homogeneousToKeypoint(const Eigen::MatrixBase<DERIVED_P>& p, const Eigen::MatrixBase<DERIVED_K>& outKeypoint,
                               const Eigen::MatrixBase<DERIVED_JP>& outJp) const;

    // NOTE by CC: convert point in image frame p_uv to point in normalized camera frame. This procedure contains two
    // steps: (1) Back projection (2) Undistortion
    template <typename DERIVED_K, typename DERIVED_P>
    bool keypointToEuclidean(const Eigen::MatrixBase<DERIVED_K>& keypoint,
                             const Eigen::MatrixBase<DERIVED_P>& outPoint) const;

    template <typename DERIVED_K, typename DERIVED_P, typename DERIVED_JK>
    bool keypointToEuclidean(const Eigen::MatrixBase<DERIVED_K>& keypoint, const Eigen::MatrixBase<DERIVED_P>& outPoint,
                             const Eigen::MatrixBase<DERIVED_JK>& outJk) const;

    template <typename DERIVED_K, typename DERIVED_P>
    bool keypointToHomogeneous(const Eigen::MatrixBase<DERIVED_K>& keypoint,
                               const Eigen::MatrixBase<DERIVED_P>& outPoint) const;

    template <typename DERIVED_K, typename DERIVED_P, typename DERIVED_JK>
    bool keypointToHomogeneous(const Eigen::MatrixBase<DERIVED_K>& keypoint,
                               const Eigen::MatrixBase<DERIVED_P>& outPoint,
                               const Eigen::MatrixBase<DERIVED_JK>& outJk) const;

    template <typename DERIVED_P, typename DERIVED_JI>
    void euclideanToKeypointIntrinsicsJacobian(const Eigen::MatrixBase<DERIVED_P>& p,
                                               const Eigen::MatrixBase<DERIVED_JI>& outJi) const;

    template <typename DERIVED_P, typename DERIVED_JD>
    void euclideanToKeypointDistortionJacobian(const Eigen::MatrixBase<DERIVED_P>& p,
                                               const Eigen::MatrixBase<DERIVED_JD>& outJd) const;

    template <typename DERIVED_P, typename DERIVED_JI>
    void homogeneousToKeypointIntrinsicsJacobian(const Eigen::MatrixBase<DERIVED_P>& p,
                                                 const Eigen::MatrixBase<DERIVED_JI>& outJi) const;

    template <typename DERIVED_P, typename DERIVED_JD>
    void homogeneousToKeypointDistortionJacobian(const Eigen::MatrixBase<DERIVED_P>& p,
                                                 const Eigen::MatrixBase<DERIVED_JD>& outJd) const;

    /**
     * @brief Whether the point is in image
     *
     * @tparam DERIVED_K
     * @param keypoint
     * @return
     */
    template <typename DERIVED_K>
    bool isValid(const Eigen::MatrixBase<DERIVED_K>& keypoint) const;

    template <typename DERIVED_P>
    bool isEuclideanVisible(const Eigen::MatrixBase<DERIVED_P>& p) const;

    template <typename DERIVED_P>
    bool isHomogeneousVisible(const Eigen::MatrixBase<DERIVED_P>& ph) const;

    // aslam::backend compatibility
    void update(const double* v);
    int minimalDimensions() const;
    void getParameters(Eigen::MatrixXd& P) const;
    void setParameters(const Eigen::MatrixXd& P);
    Eigen::Vector2i parameterSize() const;

    enum { CLASS_SERIALIZATION_VERSION = 0 };
    BOOST_SERIALIZATION_SPLIT_MEMBER();

    template <class Archive>
    void load(Archive& ar, const unsigned int version);
    template <class Archive>
    void save(Archive& ar, const unsigned int version) const;

    // \brief creates a random valid keypoint.
    virtual Eigen::VectorXd createRandomKeypoint() const;

    // \brief creates a random visible point. Negative depth means random between 0 and 100 meters.
    virtual Eigen::Vector3d createRandomVisiblePoint(double depth = -1.0) const;

    bool isProjectionInvertible() const { return false; }

    void setDistortion(const distortion_t& distortion) { _distortion = distortion; }
    distortion_t& distortion() { return _distortion; };
    const distortion_t& distortion() const { return _distortion; };

    Eigen::Matrix3d getCameraMatrix() {
        Eigen::Matrix3d K;
        K << _fu, 0.0, _cu, 0.0, _fv, _cv, 0.0, 0.0, 1.0;
        return K;
    };

    double focalLengthCol() const { return _fu; }
    double focalLengthRow() const { return _fv; }
    double opticalCenterCol() const { return _cu; }
    double opticalCenterRow() const { return _cv; }

    /// \brief The horizontal focal length in pixels.
    double fu() const { return _fu; }
    /// \brief The vertical focal length in pixels.
    double fv() const { return _fv; }
    /// \brief The horizontal image center in pixels.
    double cu() const { return _cu; }
    /// \brief The vertical image center in pixels.
    double cv() const { return _cv; }
    /// \brief The horizontal resolution in pixels.
    int ru() const { return _ru; }
    /// \brief The vertical resolution in pixels.
    int rv() const { return _rv; }
    /// \brief The horizontal resolution in pixels.
    int width() const { return _ru; }
    /// \brief The vertical resolution in pixels.
    int height() const { return _rv; }

    int keypointDimension() const { return KeypointDimension; }

    bool isBinaryEqual(const PinholeProjection<distortion_t>& rhs) const;

    static PinholeProjection<distortion_t> getTestProjection();

    /// \brief resize the intrinsics based on a scaling of the image.
    void resizeIntrinsics(double scale);

    /// \brief Get a set of border rays
    void getBorderRays(Eigen::MatrixXd& rays);

    /// \brief initialize the intrinsics based on a list of views of a gridded calibration target
    /// \return true on success
    bool initializeIntrinsics(const std::vector<GridCalibrationTargetObservation>& observations);

    /// \brief compute the reprojection error based on a checkerboard observation.
    /// \return the number of corners successfully observed and projected
    size_t computeReprojectionError(const GridCalibrationTargetObservation& obs,
                                    const sm::kinematics::Transformation& T_target_camera, double& outErr) const;

    /// \brief estimate the transformation of the camera with respect to the calibration target
    ///        On success out_T_t_c is filled in with the transformation that takes points from
    ///        the camera frame to the target frame
    /// \return true on success
    bool estimateTransformation(const GridCalibrationTargetObservation& obs,
                                sm::kinematics::Transformation& out_T_t_c) const;

  private:
    void updateTemporaries();

    /// \brief The horizontal focal length in pixels.
    double _fu;
    /// \brief The vertical focal length in pixels.
    double _fv;
    /// \brief The horizontal image center in pixels.
    double _cu;
    /// \brief The vertical image center in pixels.
    double _cv;
    /// \brief The horizontal resolution in pixels.
    int _ru;
    /// \brief The vertical resolution in pixels.
    int _rv;

    /// \brief A computed value for speeding up computation.
    double _recip_fu;
    double _recip_fv;
    double _fu_over_fv;

    distortion_t _distortion;
};

}  // namespace cameras
}  // namespace aslam

#include "implementation/PinholeProjection.hpp"

SM_BOOST_CLASS_VERSION_T1(aslam::cameras::PinholeProjection);

#endif /* ASLAM_CAMERAS_PINHOLE_PROJECTION_HPP */


/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2010, Rice University
*  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 Rice University 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: Ioan Sucan */

#include "ompl/base/spaces/SO2StateSpace.h"
#include <algorithm>
#include <limits>
#include <cmath>
#include "ompl/tools/config/MagicConstants.h"
#include <boost/math/constants/constants.hpp>

// Define for boost version < 1.47
#ifndef BOOST_ASSERT_MSG
#define BOOST_ASSERT_MSG(expr, msg) assert(expr)
#endif

void ompl::base::SO2StateSampler::sampleUniform(State *state)
{
    state->as<SO2StateSpace::StateType>()->value =
        rng_.uniformReal(-boost::math::constants::pi<double>(), boost::math::constants::pi<double>());
}

void ompl::base::SO2StateSampler::sampleUniformNear(State *state, const State *near, const double distance)
{
    state->as<SO2StateSpace::StateType>()->value = rng_.uniformReal(near->as<SO2StateSpace::StateType>()->value - distance,
                                                                       near->as<SO2StateSpace::StateType>()->value + distance);
    space_->enforceBounds(state);
}

void ompl::base::SO2StateSampler::sampleGaussian(State *state, const State *mean, const double stdDev)
{
    state->as<SO2StateSpace::StateType>()->value = rng_.gaussian(mean->as<SO2StateSpace::StateType>()->value, stdDev);
    space_->enforceBounds(state);
}

unsigned int ompl::base::SO2StateSpace::getDimension() const
{
    return 1;
}

double ompl::base::SO2StateSpace::getMaximumExtent() const
{
    return boost::math::constants::pi<double>();
}

double ompl::base::SO2StateSpace::getMeasure() const
{
    return 2.0 * boost::math::constants::pi<double>();
}

void ompl::base::SO2StateSpace::enforceBounds(State *state) const
{
    double v = fmod(state->as<StateType>()->value, 2.0 * boost::math::constants::pi<double>());
    if (v < -boost::math::constants::pi<double>())
        v += 2.0 * boost::math::constants::pi<double>();
    else
        if (v >= boost::math::constants::pi<double>())
            v -= 2.0 * boost::math::constants::pi<double>();
    state->as<StateType>()->value = v;
}

bool ompl::base::SO2StateSpace::satisfiesBounds(const State *state) const
{
    return (state->as<StateType>()->value < boost::math::constants::pi<double>()) &&
           (state->as<StateType>()->value >= -boost::math::constants::pi<double>());
}

void ompl::base::SO2StateSpace::copyState(State *destination, const State *source) const
{
    destination->as<StateType>()->value = source->as<StateType>()->value;
}

unsigned int ompl::base::SO2StateSpace::getSerializationLength() const
{
    return sizeof(double);
}

void ompl::base::SO2StateSpace::serialize(void *serialization, const State *state) const
{
    memcpy(serialization, &state->as<StateType>()->value, sizeof(double));
}

void ompl::base::SO2StateSpace::deserialize(State *state, const void *serialization) const
{
    memcpy(&state->as<StateType>()->value, serialization, sizeof(double));
}

double ompl::base::SO2StateSpace::distance(const State *state1, const State *state2) const
{
    // assuming the states 1 & 2 are within bounds
    double d = fabs(state1->as<StateType>()->value - state2->as<StateType>()->value);
    BOOST_ASSERT_MSG(satisfiesBounds(state1) && satisfiesBounds(state2),
        "The states passed to SO2StateSpace::distance are not within bounds. Call "
        "SO2StateSpace::enforceBounds() in, e.g., ompl::control::ODESolver::PostPropagationEvent, "
        "ompl::control::StatePropagator, or ompl::base::StateValidityChecker");
    return (d > boost::math::constants::pi<double>()) ? 2.0 * boost::math::constants::pi<double>() - d : d;
}

double ompl::base::SO2StateSpace::getscore(const State *state1, const State *state2) const
{
    // assuming the states 1 & 2 are within bounds
    double d = fabs(state1->as<StateType>()->value - state2->as<StateType>()->value);
    BOOST_ASSERT_MSG(satisfiesBounds(state1) && satisfiesBounds(state2),
        "The states passed to SO2StateSpace::distance are not within bounds. Call "
        "SO2StateSpace::enforceBounds() in, e.g., ompl::control::ODESolver::PostPropagationEvent, "
        "ompl::control::StatePropagator, or ompl::base::StateValidityChecker");
    return (d > boost::math::constants::pi<double>()) ? 2.0 * boost::math::constants::pi<double>() - d : d;
}

bool ompl::base::SO2StateSpace::equalStates(const State *state1, const State *state2) const
{
    return fabs(state1->as<StateType>()->value - state2->as<StateType>()->value) < std::numeric_limits<double>::epsilon() * 2.0;
}

void ompl::base::SO2StateSpace::interpolate(const State *from, const State *to, const double t, State *state) const
{
    double diff = to->as<StateType>()->value - from->as<StateType>()->value;
    if (fabs(diff) <= boost::math::constants::pi<double>())
        state->as<StateType>()->value = from->as<StateType>()->value + diff * t;
    else
    {
        double &v = state->as<StateType>()->value;
        if (diff > 0.0)
            diff = 2.0 * boost::math::constants::pi<double>() - diff;
        else
            diff = -2.0 * boost::math::constants::pi<double>() - diff;
        v = from->as<StateType>()->value - diff * t;
        // input states are within bounds, so the following check is sufficient
        if (v > boost::math::constants::pi<double>())
            v -= 2.0 * boost::math::constants::pi<double>();
        else
            if (v < -boost::math::constants::pi<double>())
                v += 2.0 * boost::math::constants::pi<double>();
    }
}

ompl::base::StateSamplerPtr ompl::base::SO2StateSpace::allocDefaultStateSampler() const
{
    return StateSamplerPtr(new SO2StateSampler(this));
}

ompl::base::State* ompl::base::SO2StateSpace::allocState() const
{
    return new StateType();
}

void ompl::base::SO2StateSpace::freeState(State *state) const
{
    delete static_cast<StateType*>(state);
}

void ompl::base::SO2StateSpace::registerProjections()
{
    class SO2DefaultProjection : public ProjectionEvaluator
    {
    public:

        SO2DefaultProjection(const StateSpace *space) : ProjectionEvaluator(space)
        {
        }

        virtual unsigned int getDimension() const
        {
            return 1;
        }

        virtual void defaultCellSizes()
        {
            cellSizes_.resize(1);
            cellSizes_[0] = boost::math::constants::pi<double>() / magic::PROJECTION_DIMENSION_SPLITS;
            bounds_.resize(1);
            bounds_.low[0] = -boost::math::constants::pi<double>();
            bounds_.high[0] = boost::math::constants::pi<double>();
        }

        virtual void project(const State *state, EuclideanProjection &projection) const
        {
            projection(0) = state->as<SO2StateSpace::StateType>()->value;
        }
    };

    registerDefaultProjection(ProjectionEvaluatorPtr(dynamic_cast<ProjectionEvaluator*>(new SO2DefaultProjection(this))));
}

double* ompl::base::SO2StateSpace::getValueAddressAtIndex(State *state, const unsigned int index) const
{
    return index == 0 ? &(state->as<StateType>()->value) : NULL;
}

void ompl::base::SO2StateSpace::printState(const State *state, std::ostream &out) const
{
    out << "SO2State [";
    if (state)
        out << state->as<StateType>()->value;
    else
        out << "NULL";
    out << ']' << std::endl;
}

void ompl::base::SO2StateSpace::printSettings(std::ostream &out) const
{
    out << "SO2 state space '" << getName() << "'" << std::endl;
}


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

#include <iostream>
#include <fstream>
#include <string>
#include <boost/program_options.hpp>

#include <dpMM/dpSubclusterMM.hpp>
#include <dpMM/dpmmSampler.hpp>
#include <dpMM/dirMM.hpp>
#include <dpMM/dirMMcld.hpp>
#include <dpMM/clTGMMDataGpu.hpp>
#include <dpMM/niwSphere.hpp>
#include <dpMM/niwSphereFull.hpp>
//#include <dpMM/niwTangent.hpp>
#include <dpMM/niwBaseMeasure.hpp>
#include <dpMM/unifSphere.hpp>
//#include <dpMM/sphericalKMeans.hpp>
//#include <dpMM/kmeans.hpp>
#include <dpMM/crpMM.hpp>
#include <dpMM/clGMMData.hpp>
#include <dpMM/vmfBaseMeasure.hpp>
//#include <dpMM/dpvMFmeans.hpp>
#include <dpMM/timer.hpp>

using namespace Eigen;
using std::string;
using std::ofstream;
using std::ifstream;
namespace po = boost::program_options;

typedef double flt;

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

  // Declare the supported options.
  po::options_description desc("Allowed options");
  desc.add_options()
    ("help,h", "produce help message")
    ("seed", po::value<int>(), "seed for random number generator")
    ("N,N", po::value<int>(), "number of input datapoints")
    ("D,D", po::value<int>(), "number of dimensions of the data")
    ("T,T", po::value<int>(), "number of sampler iterations")
    ("alpha,a", po::value< vector<double> >()->multitoken(),
      "alpha parameter of the DP (if single value assumes all alpha_i are the "
      "same")
    ("K,K", po::value<int>(), "number of initial clusters ")
    ("nopropose,n", "flag to disable the propsal of splits and merges")
    ("silhouette,s", "flag to enable output of silhouett value of the last iteration")
    ("shuffle", "shuffle the data before processing")
    ("rowData", "switch to cols: dimensions and rows: data points")
    ("base", po::value<string>(),
      "which base measure to use (StickNiw, DpNiw (DP-GMM), "
      "DpNiwSphereFull (DP-TGMM), DpNiwSphere, NiwSphere, "
      "DirNiwSphereFull" " NiwSphereUnifNoise, CrpvMF, DirvMF right now)")
    ("params,p", po::value< vector<double> >()->multitoken(),
      "parameters of the base measure")
    ("brief", po::value< vector<double> >()->multitoken(),
      "brief parameters of the base measure (ie Delta = delta*I; "
      "theta=t*ones(D)")
    ("input,i", po::value<string>(),
      "path to input dataset .csv file (rows: dimensions; cols: different "
      "datapoints)")
    ("output,o", po::value<string>(),
      "path to output labels .csv file (rows: time; cols: different "
      "datapoints)")
    ("Nho", po::value<int>(), "number of heldout datapoints")
    ("heldout", po::value<string>(),
      "path to heldout dataset .csv file (rows: dimensions; cols: different "
      "datapoints)")
    ;

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

  if (vm.count("help")) {
    cout << desc << "\n";
    return 1;
  }

  uint64_t seed = time(0);
  if(vm.count("seed"))
    seed = static_cast<uint64_t>(vm["seed"].as<int>());
  boost::mt19937 rndGen(seed);
  std::srand(seed);
  uint32_t K=5;
  if (vm.count("K")) K = vm["K"].as<int>();
  // number of iterations
  uint32_t T=100;
  if (vm.count("T")) T = vm["T"].as<int>();
  uint32_t Nho = 0; // number of helout data points
  if (vm.count("Nho")) Nho = vm["Nho"].as<int>();
  uint32_t N=100;
  if (vm.count("N")) N = vm["N"].as<int>();
  uint32_t D=2;
  if (vm.count("D")) D = vm["D"].as<int>();
  cout << "T="<<T<<endl;
  bool proposeSplitMerge = true;
  if (vm.count("nopropose")) proposeSplitMerge = false;
  if(proposeSplitMerge)
    cout<<"Propose splits and merges"<<endl;
  else
    cout<<"DO not propose splits or merges"<<endl;

  // DP alpha parameter
  VectorXd alpha;
  if (vm.count("alpha"))
  {
    vector<double> params = vm["alpha"].as< vector<double> >();
    if(params.size()==1)
    {
      alpha.setOnes(K);
      alpha *= params[0];
    }else{
      cout<<"Warning: alpha.size = "<<params.size()<<" K0 = "<<K<<endl;
      alpha.setOnes(params.size());
      for (uint32_t k=0; k<params.size(); ++k)
        alpha(k) = params[k];
    }
  }
  cout << "alpha="<<alpha.transpose()<<endl;

  shared_ptr<MatrixXd> spx(new MatrixXd(D,N));
  MatrixXd& x(*spx);
  // ind -> mapping to allow shuffling and un-shuffling of data for the algos
  vector<uint32_t> ind(N);
  for (uint32_t i=0; i<N; ++i)  ind[i] = i;
  string pathIn ="";
  if(vm.count("input")) pathIn = vm["input"].as<string>();
  if (!pathIn.compare(""))
  {
    cout<<"please specify an input dataset"<<endl;
    exit(1);
  }else{
    cout<<"loading data from "<<pathIn<<endl;
    ifstream fin(pathIn.data(),ifstream::in);

    if(vm.count("shuffle"))
    {
      cout<<"shuffling input"<<endl;
      std::random_shuffle(ind.begin(),ind.end());
    }
    if(vm.count("rowData"))
    {
      for (uint32_t i=0; i<N; ++i)
        for (uint32_t j=0; j<D; ++j)
          fin>>x(j,ind[i]);
    }else{
      for (uint32_t j=0; j<D; ++j)
        for (uint32_t i=0; i<N; ++i)
          fin>>x(j,ind[i]);
    }
    fin.close();
    cout << x.col(0).transpose()<<endl;
    cout << x.col(1).transpose()<<endl;
    cout << x.col(2).transpose()<<endl;
    cout << x.col(3).transpose()<<endl;
    cout << x.col(4).transpose()<<endl;
    cout << x.col(5).transpose()<<endl;
  }

  shared_ptr<MatrixXd> spho; //(new MatrixXd(D,N));
  string pathHO ="";
  if(vm.count("heldout")) pathHO = vm["heldout"].as<string>();
  if (pathHO.compare("") != 0)
  {
    cout<<"loading heldout data set from "<<pathHO<<endl;
    cout<<"Nho = "<<Nho<<endl;
    ifstream fin(pathHO.data(),ifstream::in);
//    uint32_t Nho = 0;
//    uint32_t Dho = 0;
//    fin >> Dho >> Nho;
    spho = shared_ptr<MatrixXd>(new MatrixXd(D,Nho));
    if(vm.count("rowData"))
    {
      for (uint32_t i=0; i<Nho; ++i)
        for (uint32_t j=0; j<D; ++j)
          fin>>(*spho)(j,i);
    }else{
      for (uint32_t j=0; j<D; ++j)
        for (uint32_t i=0; i<Nho; ++i)
          fin>>(*spho)(j,i);
    }
    fin.close();
    cout << spho->col(0).transpose()<<endl;
  }

  // which base distribution
  string base = "DpNiw";
  if(vm.count("base")) base = vm["base"].as<string>();
  cout<<"will use base: "<<base<<endl;

  if(base.compare("DpNiwSphere") == 0 || base.compare("DirvMF") ==0
    || base.compare("CrpvMF")==0 ){
    cout<<"check unit length "<<endl;
    // normalize to unit length
    int err = 0;
#pragma omp parallel for
    for (uint32_t i=0; i<N; ++i)
      if(fabs(x.col(i).norm() - 1.0) > 1e-1)
      {
        err++;
        cout<<x.col(i).norm() <<endl;
      }else
        x.col(i) /= x.col(i).norm();
    if(err>0) return 0;
  }

  DpMM<double> *dpmm=NULL;
  DpMM<double> *dpmmf=NULL;
//  Clusterer<double> *spkm = NULL;
  if(!base.compare("StickNiw"))
  {
    MatrixXd Delta(D,D);
    VectorXd theta(D);
    double nu = 10.0;
    double kappa = 10.0;
//    Delta << nu,0.0,0.0,nu;
//    theta << 0.0,0.0;
    if(vm.count("params"))
    {
      vector<double> params = vm["params"].as< vector<double> >();
      cout<<"params length="<<params.size()<<" D="<<D<<endl;
      nu = params[0];
      kappa = params[1];
      for(uint32_t i=0; i<D; ++i)
        theta(i) = params[2+i];
      for(uint32_t i=0; i<D; ++i)
        for(uint32_t j=0; j<D; ++j)
          Delta(i,j) = params[2+D+i+D*j];
      cout <<"nu="<<nu<<endl;
      cout <<"kappa="<<kappa<<endl;
      cout <<"theta="<<theta<<endl;
      cout <<"Delta="<<Delta<<endl;
    }
    NIWd niw(Delta, theta, nu, kappa, &rndGen);
    dpmm = new DpStickMM<NIWd>(alpha(0), niw);

  }else if(!base.compare("DpNiw")){
    MatrixXd Delta(D,D);
    VectorXd theta(D);
    double nu = 10.0;
    double kappa = 10.0;
//    Delta << nu,0.0,0.0,nu;
//    theta << 0.0,0.0;
    if(vm.count("params"))
    {
      vector<double> params = vm["params"].as< vector<double> >();
      cout<<"params length="<<params.size()<<" D="<<D<<endl;
      nu = params[0];
      kappa = params[1];
      for(uint32_t i=0; i<D; ++i)
        theta(i) = params[2+i];
      for(uint32_t i=0; i<D; ++i)
        for(uint32_t j=0; j<D; ++j)
          Delta(i,j) = params[2+D+i+D*j];
      cout <<"nu="<<nu<<endl;
      cout <<"kappa="<<kappa<<endl;
      cout <<"theta="<<theta<<endl;
      cout <<"Delta="<<Delta<<endl;
    }
    NIWd niw(Delta, theta, nu, kappa, &rndGen);

    shared_ptr<NiwSampledd> niwSampl(new NiwSampledd(niw));
    shared_ptr<LrCluster<NiwSampledd,double> > lrTheta(new
        LrCluster<NiwSampledd,double>(niwSampl,1.0,&rndGen));
    dpmm = new DpSubclusterMM<NiwSampledd,double>(alpha(0), lrTheta, K, &rndGen);

  }else if(!base.compare("NiwSphere")){
    cout<<"D="<<D<<endl;
    MatrixXd Delta(D-1,D-1);
    double nu = 10.0;
    //    Delta << nu,0.0,0.0,nu;
    //    theta << 0.0,0.0;
    if(vm.count("params"))
    {
      vector<double> params = vm["params"].as< vector<double> >();
      cout<<"params length="<<params.size()<<" D="<<D<<endl;
      nu = params[0];
#pragma omp parallel for
      for(uint32_t i=0; i<D-1; ++i)
        for(uint32_t j=0; j<D-1; ++j)
          Delta(i,j) = params[1+i+(D-1)*j];
      if (D<30){
        cout <<"nu="<<nu<<endl;
        cout <<"Delta="<<Delta<<endl;
      }
    }else if(vm.count("brief"))
    {
      vector<double> params = vm["brief"].as< vector<double> >();
      nu = params[0];
      Delta = params[1]*MatrixXd::Identity(D-1,D-1);
      cout <<"nu="<<nu<<endl;
      cout <<"Delta="<<params[1]<<"I_"<<(D-1)<<endl;
      if (D<30){
        cout <<"nu="<<nu<<endl;
        cout <<"Delta="<<Delta<<endl;
      }
    }
    IWd iw(Delta, nu, &rndGen);
    shared_ptr<NiwSphered> niwSp (new NiwSphered(iw, &rndGen));
//    alphaV[K-1] *=0.1;
    DirCatd dir(alpha,&rndGen);
    dpmm = new DirMM<double>(dir, niwSp, K);

  }else if(!base.compare("DpNiwSphere")){
    cout<<"D="<<D<<endl;
    MatrixXd Delta(D-1,D-1);
    double nu = 10.0;
    //    Delta << nu,0.0,0.0,nu;
    //    theta << 0.0,0.0;
    if(vm.count("params"))
    {
      vector<double> params = vm["params"].as< vector<double> >();
      cout<<"params length="<<params.size()<<" D="<<D<<endl;
      nu = params[0];
#pragma omp parallel for
      for(uint32_t i=0; i<D-1; ++i)
        for(uint32_t j=0; j<D-1; ++j)
          Delta(i,j) = params[1+i+(D-1)*j];
      if (D<30){
        cout <<"nu="<<nu<<endl;
        cout <<"Delta="<<Delta<<endl;
      }
    }else if(vm.count("brief"))
    {
      vector<double> params = vm["brief"].as< vector<double> >();
      nu = params[0];
      Delta = params[1]*MatrixXd::Identity(D-1,D-1);
      cout <<"nu="<<nu<<endl;
      cout <<"Delta="<<params[1]<<"I_"<<(D-1)<<endl;
      if (D<30){
        cout <<"nu="<<nu<<endl;
        cout <<"Delta="<<Delta<<endl;
      }
    }
    IWd iw(Delta, nu, &rndGen);
    shared_ptr<NiwSphered> theta (new NiwSphered(iw, &rndGen));
    shared_ptr<LrCluster<NiwSphered,double> > lrTheta(new
      LrCluster<NiwSphered,double>(theta,1.0,&rndGen));
    dpmm = new DpSubclusterMM<NiwSphered,double>(alpha(0), lrTheta, K, &rndGen);

  }else if(!base.compare("DpNiwSphereFull")){
    cout<<"D="<<D<<endl;
    MatrixXd Delta(D-1,D-1);
    double nu = 10.0;
    //    Delta << nu,0.0,0.0,nu;
    //    theta << 0.0,0.0;
    if(vm.count("params"))
    {
      vector<double> params = vm["params"].as< vector<double> >();
      cout<<"params length="<<params.size()<<" D="<<D<<endl;
      nu = params[0];
#pragma omp parallel for
      for(uint32_t i=0; i<D-1; ++i)
        for(uint32_t j=0; j<D-1; ++j)
          Delta(i,j) = params[1+i+(D-1)*j];
      if (D<30){
        cout <<"nu="<<nu<<endl;
        cout <<"Delta="<<Delta<<endl;
      }
    }else if(vm.count("brief"))
    {
      vector<double> params = vm["brief"].as< vector<double> >();
      nu = params[0];
      Delta = params[1]*MatrixXd::Identity(D-1,D-1);
      cout <<"nu="<<nu<<endl;
      cout <<"Delta="<<params[1]<<"I_"<<(D-1)<<endl;
      if (D<30){
        cout <<"nu="<<nu<<endl;
        cout <<"Delta="<<Delta<<endl;
      }
    }
    IWd iw(Delta, nu, &rndGen);
    shared_ptr<NiwSphereFulld> theta (new NiwSphereFulld(iw,&rndGen));
    shared_ptr<LrCluster<NiwSphereFulld,double> > lrTheta(new
      LrCluster<NiwSphereFulld,double>(theta,1.0,&rndGen));
    dpmm = new DpSubclusterMM<NiwSphereFulld,double>(alpha(0), lrTheta,
        K, &rndGen);

  }else if(!base.compare("DirNiwSphereFull")){
    cout<<"D="<<D<<endl;
    MatrixXd Delta(D-1,D-1);
    double nu = 10.0;
    //    Delta << nu,0.0,0.0,nu;
    //    theta << 0.0,0.0;
    if(vm.count("params"))
    {
      vector<double> params = vm["params"].as< vector<double> >();
      cout<<"params length="<<params.size()<<" D="<<D<<endl;
      nu = params[0];
#pragma omp parallel for
      for(uint32_t i=0; i<D-1; ++i)
        for(uint32_t j=0; j<D-1; ++j)
          Delta(i,j) = params[1+i+(D-1)*j];
      if (D<30){
        cout <<"nu="<<nu<<endl;
        cout <<"Delta="<<Delta<<endl;
      }
    }else if(vm.count("brief"))
    {
      vector<double> params = vm["brief"].as< vector<double> >();
      nu = params[0];
      Delta = params[1]*MatrixXd::Identity(D-1,D-1);
      cout <<"nu="<<nu<<endl;
      cout <<"Delta="<<params[1]<<"I_"<<(D-1)<<endl;
      if (D<30){
        cout <<"nu="<<nu<<endl;
        cout <<"Delta="<<Delta<<endl;
      }
    }
    DirCatd dir(alpha,&rndGen);
    IWd iw(Delta, nu, &rndGen);
    shared_ptr<NiwSphereFulld> theta (new NiwSphereFulld(iw,&rndGen));
    dpmm = new DirMM<double>(dir, theta, K);

//  }else if(!base.compare("DpNiwTangent")){
//    cout<<"D="<<D<<endl;
//    MatrixXd Delta(D-1,D-1);
//    double nu = 10.0;
//    //    Delta << nu,0.0,0.0,nu;
//    //    theta << 0.0,0.0;
//    if(vm.count("params"))
//    {
//      vector<double> params = vm["params"].as< vector<double> >();
//      cout<<"params length="<<params.size()<<" D="<<D<<endl;
//      nu = params[0];
//#pragma omp parallel for
//      for(uint32_t i=0; i<D-1; ++i)
//        for(uint32_t j=0; j<D-1; ++j)
//          Delta(i,j) = params[1+i+(D-1)*j];
//      if (D<30){
//        cout <<"nu="<<nu<<endl;
//        cout <<"Delta="<<Delta<<endl;
//      }
//    }else if(vm.count("brief"))
//    {
//      vector<double> params = vm["brief"].as< vector<double> >();
//      nu = params[0];
//      Delta = params[1]*MatrixXd::Identity(D-1,D-1);
//      cout <<"nu="<<nu<<endl;
//      cout <<"Delta="<<params[1]<<"I_"<<(D-1)<<endl;
//      if (D<30){
//        cout <<"nu="<<nu<<endl;
//        cout <<"Delta="<<Delta<<endl;
//      }
//    }
//    VectorXd mu(D-1); mu.setZero();
//    double kappa = .01;
//    cout<<"theta = "<<mu.transpose()<<endl;
//    cout<<"kappa = "<<kappa<<endl;
//    NIWd niw(Delta, mu, nu, kappa, &rndGen);
//    shared_ptr<NiwTangentd> theta (new NiwTangentd(niw,&rndGen));
//    shared_ptr<LrCluster<NiwTangentd,double> > lrTheta(new
//      LrCluster<NiwTangentd,double>(theta,1.0,&rndGen));
//    dpmm = new DpSubclusterMM<NiwTangentd,double>(alpha(0), lrTheta,
//        K, &rndGen);
//
  }else if(!base.compare("NiwSphereUnifNoise")){

    MatrixXd Delta(D-1,D-1);
    double nu = 10.0;
    //    Delta << nu,0.0,0.0,nu;
    //    theta << 0.0,0.0;
    if(vm.count("params"))
    {
      vector<double> params = vm["params"].as< vector<double> >();
      cout<<"params length="<<params.size()<<" D="<<D<<endl;
      nu = params[0];
#pragma omp parallel for
      for(uint32_t i=0; i<D-1; ++i)
        for(uint32_t j=0; j<D-1; ++j)
          Delta(i,j) = params[1+i+(D-1)*j];
      if (D<30){
        cout <<"nu="<<nu<<endl;
        cout <<"Delta="<<Delta<<endl;
      }
    }else if(vm.count("brief"))
    {
      vector<double> params = vm["brief"].as< vector<double> >();
      nu = params[0];
      Delta = params[1]*MatrixXd::Identity(D-1,D-1);
      cout <<"nu="<<nu<<endl;
      cout <<"Delta="<<params[1]<<" I_"<<(D-1)<<endl;
      if (D<30){
        cout <<"nu="<<nu<<endl;
        cout <<"Delta="<<Delta<<endl;
      }
    }

    IWd iw(Delta, nu, &rndGen);
    vector<shared_ptr<BaseMeasure<double> > > thetas;
    for (uint32_t k=0; k<K-1; ++k)
      thetas.push_back(shared_ptr<BaseMeasure<double> >(
            new NiwSphered(iw, &rndGen)));
    thetas.push_back(boost::shared_ptr<BaseMeasure<double> >(new UnifSphered(D)));

    DirCatd dir(alpha,&rndGen);
    dpmm = new DirMM<double>(dir, thetas);

  }else if(!base.compare("NiwSphereCuda")){
    cout<<"D="<<D<<endl;
    MatrixXd Delta(D-1,D-1);
    double nu = 10.0;
    //    Delta << nu,0.0,0.0,nu;
    //    theta << 0.0,0.0;
    if(vm.count("params"))
    {
      vector<double> params = vm["params"].as< vector<double> >();
      cout<<"params length="<<params.size()<<" D="<<D<<endl;
      nu = params[0];
#pragma omp parallel for
      for(uint32_t i=0; i<D-1; ++i)
        for(uint32_t j=0; j<D-1; ++j)
          Delta(i,j) = params[1+i+(D-1)*j];
      if (D<30){
        cout <<"nu="<<nu<<endl;
        cout <<"Delta="<<Delta<<endl;
      }
    }else if(vm.count("brief"))
    {
      vector<double> params = vm["brief"].as< vector<double> >();
      nu = params[0];
      Delta = params[1]*MatrixXd::Identity(D-1,D-1);
      cout <<"nu="<<nu<<endl;
      cout <<"Delta="<<params[1]<<"I_"<<(D-1)<<endl;
      if (D<30){
        cout <<"nu="<<nu<<endl;
        cout <<"Delta="<<Delta<<endl;
      }
    }
    IWd iw(Delta, nu, &rndGen);
    shared_ptr<NiwSphered> niwSp (new NiwSphered(iw, &rndGen));
//    alphaV[K-1] *=0.1;
    DirCatd dir(alpha,&rndGen);
    dpmmf = new DirMMcld<NiwSphered,double>(dir, niwSp);
//  }else if(!base.compare("DPvMFmeans")){
//    double lambda = cos(5.0*M_PI/180.0);
//    if(vm.count("params"))
//    {
//      vector<double> params = vm["params"].as< vector<double> >();
//      cout<<"params length="<<params.size()<<endl;
//      lambda = params[0];
//    }
//    spkm = new DPvMFMeans<double>(spx, K, lambda, &rndGen);
  }else if(!base.compare("CrpvMF")){
    cout<<"D="<<D<<endl;

    double a0 = 100.0;
    double b0 = 80.0;
    double t0 = 0.01;
    VectorXd m0(D);

    if(vm.count("params"))
    {
      vector<double> params = vm["params"].as< vector<double> >();
      cout<<"params length="<<params.size()<<" D="<<D<<endl;
      for(uint32_t i=0; i<D; ++i)
        m0(i) = params[i];
      t0 = params[D];
      a0 = params[D+1];
      b0 = params[D+2];
    }
    vMFpriorFull<double> vMFprior(m0,t0,a0,b0,&rndGen);
    shared_ptr<vMFbase<double> > vMFsampled( new vMFbase<double>(vMFprior));
    dpmm = new CrpMM<double>(alpha(0),vMFsampled,K,&rndGen);
  }else if(!base.compare("DirvMF")){
    cout<<"D="<<D<<endl;

    double a0 = 2.0;
    double b0 = 1.7;
    double t0 = 0.01;
    VectorXd m0(D);

    if(vm.count("params"))
    {
      vector<double> params = vm["params"].as< vector<double> >();
      cout<<"params length="<<params.size()<<" D="<<D<<endl;
      for(uint32_t i=0; i<D; ++i)
        m0(i) = params[i];
      t0 = params[D];
      a0 = params[D+1];
      b0 = params[D+2];
    }
    vMFpriorFull<double> vMFprior(m0,t0,a0,b0,&rndGen);
    shared_ptr<vMFbase<double> > vMFsampled( new vMFbase<double>(vMFprior));
    Dir<Cat<double>,double> dir(alpha,&rndGen);
    dpmm = new DirMM<double>(dir,vMFsampled,K);

//  }else if(!base.compare("spkm")){
//    spkm = new SphericalKMeans<double>(spx, K, &rndGen);
//  }else if(!base.compare("spkmKarcher")){
//    spkm = new SphericalKMeansKarcher<double>(spx, K, &rndGen);
//  }else if(!base.compare("kmeans")){
//    spkm = new KMeans<double>(spx, K, &rndGen);
  }else{
    cout<<"base "<<base<<" not supported"<<endl;
    return 1;
  }

  string pathOut ="./labels.csv";
  if(vm.count("output"))
    pathOut = vm["output"].as<string>();
  cout<<"output to "<<pathOut<<endl;

  VectorXu z_;
  uint32_t K_;

  // TODO stupid duplication
  if(!base.compare("NiwSphereCuda"))
  {
    assert(dpmmf!=NULL);
//    MatrixXf x_f = x.cast<float>();
    shared_ptr<ClTGMMDataGpud> cld(new ClTGMMDataGpud(spx,
          spVectorXu(new VectorXu(N)),K));
    dpmmf->initialize(cld);

    ofstream fout(pathOut.data(),ofstream::out);
    ofstream foutJointLike((pathOut+"_jointLikelihood.csv").data(),ofstream::out);
    for (uint32_t t=0; t<T; ++t)
    {
      cout<<"------------ t="<<t<<" -------------"<<endl;
      dpmmf->sampleLabels();

      const VectorXu& z = dpmmf->getLabels().transpose();
      cout<<"   K="<<dpmm->getK();
      cout<<"  counts=   "<<counts<double,uint32_t>(z,dpmmf->getK()).transpose()<<endl;
//      cout<<"  logJoint= "<<dpmmf->logJoint()<<endl;
      for (uint32_t i=0; i<z.size()-1; ++i)
        fout<<z(ind[i])<<" ";
      fout<<z(ind[z.size()-1])<<endl;
      foutJointLike<<dpmmf->logJoint()<<endl;

      dpmmf->sampleParameters();
    }
    fout.close();
    foutJointLike.close();

    MatrixXd logLikes;
    MatrixXu inds = dpmmf->mostLikelyInds(10,logLikes);
    cout<<"most likely indices"<<endl;
    cout<<inds<<endl;
    cout<<"----------------------------------------"<<endl;

    fout.open((pathOut+"mlInds.csv").data(),ofstream::out);
    fout<<inds<<endl;
    fout.close();
    fout.open((pathOut+"mlLogLikes.csv").data(),ofstream::out);
    fout<<logLikes<<endl;
    fout.close();

    z_ = dpmmf->getLabels().transpose();
    K_ = dpmmf->getK();

//  }else if(!base.compare("spkm") || !base.compare("spkmKarcher")
//      || !base.compare("kmeans"))
//  {
//    ofstream fout(pathOut.data(),ofstream::out);
//    ofstream foutJointLike((pathOut+"_jointLikelihood.csv").data(),ofstream::out);
//    Timer watch;
//    for (uint32_t t=0; t<T; ++t)
//    {
//      cout<<"------------ t="<<t<<" -------------"<<endl;
//      watch.tic();
//      spkm->updateCenters();
//      watch.toctic("-- updateCenters");
//
//      const VectorXu& z = spkm->z();
//      for (uint32_t i=0; i<z.size()-1; ++i)
//        fout<<z(ind[i])<<" ";
//      fout<<z(ind[z.size()-1])<<endl;
//      double deviation = spkm->avgIntraClusterDeviation();
//
//      cout<<"   K="<<spkm->getK();
//      cout<<"  counts=   "<<counts<double,uint32_t>(z,spkm->getK()).transpose();
//      cout<<" avg deviation "<<deviation<<endl;
//      foutJointLike<<deviation<<endl;
//
//      watch.tic();
//      spkm->updateLabels();
//      watch.toctic("-- updateLabels");
//    }
//    fout.close();
//
//    MatrixXd deviates;
//    MatrixXu inds = spkm->mostLikelyInds(10,deviates);
//    cout<<"most likely indices"<<endl;
//    cout<<inds<<endl;
//    cout<<"----------------------------------------"<<endl;
//
//    fout.open((pathOut+"mlInds.csv").data(),ofstream::out);
//    fout<<inds<<endl;
//    fout.close();
//    fout.open((pathOut+"mlLogLikes.csv").data(),ofstream::out);
//    fout<<deviates<<endl;
//    fout.close();
//
//    ofstream foutMeans((pathOut+"_means.csv").data(),ofstream::out);
//    foutMeans << spkm->centroids()<<endl;
//    foutMeans.close();
//
//    z_ = spkm->z().transpose();
//    K_ = spkm->getK();
//
  }else{
    assert(dpmm!=NULL);
    dpmm->initialize(x);

    ofstream fout(pathOut.data(),ofstream::out);
    ofstream foutJointLike((pathOut+"_jointLikelihood.csv").data(),ofstream::out);
    ofstream foutHoLogLike((pathOut+"_hoLogLike.csv").data(),ofstream::out);
    ofstream foutMeans((pathOut+"_means.csv").data(),ofstream::out);
    ofstream foutCovs((pathOut+"_covs.csv").data(),ofstream::out);

    if(Nho == 0)
    { // not output to save time
      const VectorXu& z = dpmm->getLabels().transpose();
      for (uint32_t i=0; i<z.size()-1; ++i)
        fout<<z(ind[i])<<" ";
      fout<<z(ind[z.size()-1])<<endl;
      foutJointLike<<dpmm->logJoint()<<endl;
      dpmm->dump(foutMeans,foutCovs);
    }

    for (uint32_t t=0; t<T; ++t)
    {
      cout<<"------------ t="<<t<<" -------------"<<endl;
      Timer t0;
      // sample clusters from the base measure to fill up to K clusters
      // only if we are not proposing splits and merges to make sure that the
      // number of clusters stays the originally intended one
//      if(!proposeSplitMerge)  dpmm->resampleFromBase(K);

//      VectorXd Ns = counts(dpmm->getLabels(),dpmm->getK()*2).transpose();
//      cout<<"-- counts= "<<Ns.transpose()<<" sum="<<Ns.sum()<<endl;
      dpmm->sampleParameters();
      if(Nho == 0)
      { // not output to save time
        const VectorXu& z = dpmm->getLabels().transpose();
        for (uint32_t i=0; i<z.size()-1; ++i)
          fout<<z(ind[i])<<" ";
        fout<<z(ind[z.size()-1])<<endl;
        foutJointLike<<dpmm->logJoint()<<endl;
        dpmm->dump(foutMeans,foutCovs);
      }
      dpmm->sampleLabels();

      if(Nho == 0)
      { // not output to save time
        VectorXd Ns = dpmm->getCounts();
        cout<<"--  counts= "<<Ns.transpose()<<" sum="<<Ns.sum()<<endl;
        cout<<"    K="<<dpmm->getK();
        cout<<"    logJoint= "<<dpmm->logJoint()<<endl;
      }

      if(proposeSplitMerge)
      {
        dpmm->proposeMerges();
        //      Ns = counts(dpmm->getLabels(),dpmm->getK()*2).transpose();
        //      cout<<"-- counts= "<<Ns.transpose()<<" sum="<<Ns.sum()<<endl;

        dpmm->proposeSplits();
        //      Ns = counts(dpmm->getLabels(),dpmm->getK()*2).transpose();
        //      cout<<"-- counts= "<<Ns.transpose()<<" sum="<<Ns.sum()<<endl;
      }

      if(Nho > 0)
      {
        double dt = t0.toc() ;
        double hoLogLike = 0;
        // evaluate the heldout word log likelihood
        for(uint32_t i=0; i<spho->cols(); ++i)
        {
//          cout<<spho->col(i).transpose()<<" = "<<dpmm->evalLogLikelihood(spho->col(i))<<endl;
          hoLogLike += dpmm->evalLogLikelihood(spho->col(i));
        }
        hoLogLike /= spho->cols();
        foutHoLogLike << hoLogLike << " " << dt << endl;

        cout<<"--- "<<hoLogLike << " " << dt << endl;
      }

    }
    fout.close();
    foutJointLike.close();

    MatrixXd logLikes;
    MatrixXu inds = dpmm->mostLikelyInds(10,logLikes);
    cout<<"most likely indices"<<endl;
    cout<<inds<<endl;
    cout<<"----------------------------------------"<<endl;

    fout.open((pathOut+"mlInds.csv").data(),ofstream::out);
    fout<<inds<<endl;
    fout.close();
    fout.open((pathOut+"mlLogLikes.csv").data(),ofstream::out);
    fout<<logLikes<<endl;
    fout.close();

    z_ = dpmm->getLabels().transpose();
    K_ = dpmm->getK();

  }
  if(vm.count("silhouette"))
  {
    spVectorXu spz(new VectorXu(z_));
    ClGMMData<double> cld(spx,spz,K_);
    cld.update(K_); // compute SS
    double sil = 0.;
    if(!base.compare("spkm") || !base.compare("spkmKarcher")
        || !base.compare("DpNiwSphereFull")
        || !base.compare("DpNiwSphere")
        || !base.compare("DirNiwSphereFull")
        || !base.compare("CrpvMF")
        || !base.compare("DirvMF"))
    {
      cout<<"spherical silhouette"<<endl;
      sil = silhouette<double,Spherical<double> >(cld);
    }else{
      cout<<"euclidean silhouette"<<endl;
      sil = silhouette<double,Euclidean<double> >(cld);
    }
    cout<<"silhouette = "<<sil<<" saved to "<<(pathOut+"_measures.csv")<<endl;
    ofstream fout((pathOut+"_measures.csv").data(),ofstream::out);
    fout<<sil<<endl;
    fout.close();
  }
};


#pragma once

#include <polyfem/Problem.hpp>
#include <polyfem/ExpressionValue.hpp>

#include <Eigen/Dense>

#include <array>
#include <vector>

namespace polyfem
{
	class Interpolation
	{
	public:
		virtual ~Interpolation() {}
		virtual double eval(const double t) const = 0;
		virtual void init(const json &params) {}

		static std::shared_ptr<Interpolation> build(const json &params);
	};

	class NoInterpolation : public Interpolation
	{
	public:
		double eval(const double t) const override { return 1; };
	};

	class LinearInterpolation : public Interpolation
	{
	public:
		double eval(const double t) const override { return t; }
	};

	class LinearRamp : public Interpolation
	{
	public:
		double eval(const double t) const override;
		void init(const json &params) override;

	private:
		double to_;
	};

	class GenericTensorProblem : public Problem
	{
	public:
		GenericTensorProblem(const std::string &name);

		void rhs(const AssemblerUtils &assembler, const std::string &formulation, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;
		bool is_rhs_zero() const override
		{
			for (int i = 0; i < 3; ++i)
			{
				if (!rhs_[i].is_zero())
					return false;
			}
			return true;
		}

		void bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;
		void neumann_bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const Eigen::MatrixXd &normals, const double t, Eigen::MatrixXd &val) const override;

		bool has_exact_sol() const override { return has_exact_; }
		bool is_scalar() const override { return false; }
		bool is_time_dependent() const override { return is_time_dept_; }
		void set_time_dependent(const bool val) { is_time_dept_ = val; }
		bool is_constant_in_time() const override { return !is_time_dept_; }

		void velocity_bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;
		void acceleration_bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;

		void initial_solution(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &pts, Eigen::MatrixXd &val) const override;
		void initial_velocity(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &pts, Eigen::MatrixXd &val) const override;
		void initial_acceleration(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &pts, Eigen::MatrixXd &val) const override;

		void set_parameters(const json &params) override;

		bool is_dimension_dirichet(const int tag, const int dim) const override;
		bool all_dimensions_dirichlet() const override { return all_dimensions_dirichlet_; }

		void exact(const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;
		void exact_grad(const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;

		// bool is_mixed() const override { return is_mixed_; }
		int n_incremental_load_steps(const double diag) const override;

		void add_dirichlet_boundary(const int id, const Eigen::RowVector3d &val, const bool isx, const bool isy, const bool isz);
		void add_neumann_boundary(const int id, const Eigen::RowVector3d &val);
		void add_pressure_boundary(const int id, const double val);

		void add_dirichlet_boundary(const int id, const std::function<Eigen::MatrixXd(double x, double y, double z, double t)> &func, const bool isx, const bool isy, const bool isz);
		void add_neumann_boundary(const int id, const std::function<Eigen::MatrixXd(double x, double y, double z, double t)> &func);
		void add_pressure_boundary(const int id, const std::function<double(double x, double y, double z, double t)> &func);

		void set_rhs(double x, double y, double z);

		void clear() override;

	private:
		bool all_dimensions_dirichlet_ = true;
		bool has_exact_ = false;
		bool has_exact_grad_ = false;
		bool is_time_dept_ = false;
		// bool is_mixed_ = false;

		std::vector<std::array<ExpressionValue, 3>> forces_;
		std::vector<std::shared_ptr<Interpolation>> forces_interpolation_;
		std::vector<std::array<ExpressionValue, 3>> displacements_;
		std::vector<std::shared_ptr<Interpolation>> displacements_interpolation_;
		std::vector<ExpressionValue> pressures_;
		std::vector<std::shared_ptr<Interpolation>> pressure_interpolation_;

		std::vector<std::pair<int, std::array<ExpressionValue, 3>>> initial_position_;
		std::vector<std::pair<int, std::array<ExpressionValue, 3>>> initial_velocity_;
		std::vector<std::pair<int, std::array<ExpressionValue, 3>>> initial_acceleration_;

		std::vector<Eigen::Matrix<bool, 1, 3>> dirichlet_dimensions_;

		std::array<ExpressionValue, 3> rhs_;
		std::array<ExpressionValue, 3> exact_;
		std::array<ExpressionValue, 9> exact_grad_;
		bool is_all_;
	};

	class GenericScalarProblem : public Problem
	{
	public:
		GenericScalarProblem(const std::string &name);

		void rhs(const AssemblerUtils &assembler, const std::string &formulation, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;
		bool is_rhs_zero() const override { return rhs_.is_zero(); }

		void bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;
		void neumann_bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const Eigen::MatrixXd &normals, const double t, Eigen::MatrixXd &val) const override;

		bool has_exact_sol() const override { return has_exact_; }
		bool is_scalar() const override { return true; }
		bool is_time_dependent() const override { return is_time_dept_; }
		void set_time_dependent(const bool val) { is_time_dept_ = val; }
		bool is_constant_in_time() const override { return !is_time_dept_; }

		void set_parameters(const json &params) override;

		void exact(const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;
		void exact_grad(const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;

		void add_dirichlet_boundary(const int id, const double val);
		void add_neumann_boundary(const int id, const double val);

		void add_dirichlet_boundary(const int id, const std::function<double(double x, double y, double z, double t)> &func);
		void add_neumann_boundary(const int id, const std::function<double(double x, double y, double z, double t)> &func);

		void clear() override;

	private:
		std::vector<ExpressionValue> neumann_;
		std::vector<ExpressionValue> dirichlet_;

		std::vector<std::shared_ptr<Interpolation>> neumann_interpolation_;
		std::vector<std::shared_ptr<Interpolation>> dirichlet_interpolation_;

		ExpressionValue rhs_;
		ExpressionValue exact_;
		std::array<ExpressionValue, 3> exact_grad_;
		bool is_all_;
		bool has_exact_ = false;
		bool has_exact_grad_ = false;
		bool is_time_dept_ = false;
	};
} // namespace polyfem