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

#ifndef _MSC_VER
extern "C" {
#include <csim/memory_ops.h>
#include <csim/stat_ops.h>
}
#else
#include <csim/memory_ops.h>
#include <csim/stat_ops.h>
#endif

// post-selection probability check
TEST(StatOperationTest, ProbTest) {
    const UINT n = 6;
    const ITYPE dim = 1ULL << n;
    const UINT max_repeat = 10;
    const double eps = 1e-14;

    CTYPE* state = allocate_quantum_state(dim);
    Eigen::MatrixXcd P0(2, 2), P1(2, 2);
    P0 << 1, 0, 0, 0;
    P1 << 0, 0, 0, 1;

    for (UINT rep = 0; rep < max_repeat; ++rep) {
        initialize_Haar_random_state(state, dim);
        ASSERT_NEAR(state_norm(state, dim), 1, eps);
        Eigen::VectorXcd test_state(dim);
        for (ITYPE i = 0; i < dim; ++i) test_state[i] = state[i];

        for (UINT target = 0; target < n; ++target) {
            double p0 = M0_prob(target, state, dim);
            double p1 = M1_prob(target, state, dim);
            ASSERT_NEAR((get_expanded_eigen_matrix_with_identity(target, P0, n)*test_state).squaredNorm(), p0, eps);
            ASSERT_NEAR((get_expanded_eigen_matrix_with_identity(target, P1, n)*test_state).squaredNorm(), p1, eps);
            ASSERT_NEAR(p0 + p1, 1, eps);
        }
    }
    release_quantum_state(state);
}

// marginal probability check
TEST(StatOperationTest, MarginalProbTest) {
    const UINT n = 6;
    const ITYPE dim = 1ULL << n;
    const UINT max_repeat = 10;
    const double eps = 1e-14;

    CTYPE* state = allocate_quantum_state(dim);
    Eigen::MatrixXcd P0(2, 2), P1(2, 2), Identity(2,2);
    P0 << 1, 0, 0, 0;
    P1 << 0, 0, 0, 1;
    Identity << 1, 0, 0, 1;

    for (UINT rep = 0; rep < max_repeat; ++rep) {
        initialize_Haar_random_state(state, dim);
        ASSERT_NEAR(state_norm(state, dim), 1, eps);
        Eigen::VectorXcd test_state(dim);
        for (ITYPE i = 0; i < dim; ++i) test_state[i] = state[i];

        for (UINT target = 0; target < n; ++target) {
            // merginal probability check
            Eigen::MatrixXcd mat = Eigen::MatrixXcd::Identity(1, 1);
            std::vector<UINT> index_list, measured_value_list;

            index_list.clear();
            measured_value_list.clear();
            for (UINT i = 0; i < n; ++i) {
                UINT measured_value = rand_int(3);
                if (measured_value != 2) {
                    measured_value_list.push_back(measured_value);
                    index_list.push_back(i);
                }
                if (measured_value == 0) {
                    mat = kronecker_product(P0, mat);
                }
                else if (measured_value == 1) {
                    mat = kronecker_product(P1, mat);
                }
                else {
                    mat = kronecker_product(Identity, mat);
                }
            }
            double test_marginal_prob = (mat*test_state).squaredNorm();
            double res = marginal_prob(index_list.data(), measured_value_list.data(), (UINT)index_list.size(), state, dim);
            ASSERT_NEAR(test_marginal_prob, res, eps);
        }
    }
    release_quantum_state(state);
}

// entropy
TEST(StatOperationTest, EntropyTest) {
    const UINT n = 6;
    const ITYPE dim = 1ULL << n;
    const UINT max_repeat = 10;
    const double eps = 1e-14;

    CTYPE* state = allocate_quantum_state(dim);
    for (UINT rep = 0; rep < max_repeat; ++rep) {
        initialize_Haar_random_state(state, dim);
        ASSERT_NEAR(state_norm(state, dim), 1, eps);
        Eigen::VectorXcd test_state(dim);
        for (ITYPE i = 0; i < dim; ++i) test_state[i] = state[i];

        for (UINT target = 0; target < n; ++target) {
            double ent = 0;
            for (ITYPE ind = 0; ind < dim; ++ind) {
                double prob = norm(test_state[ind]);
                if (prob > eps)
                    ent += -prob * log(prob);
            }
            ASSERT_NEAR(ent, measurement_distribution_entropy(state, dim), eps);
        }
    }
    release_quantum_state(state);
}

// inner product
TEST(StatOperationTest, InnerProductTest) {
    const UINT n = 6;
    const ITYPE dim = 1ULL << n;
    const UINT max_repeat = 10;
    const double eps = 1e-14;

    CTYPE* state = allocate_quantum_state(dim);
    CTYPE* buffer = allocate_quantum_state(dim);
    for (UINT rep = 0; rep < max_repeat; ++rep) {

        initialize_Haar_random_state(state, dim);
        ASSERT_NEAR(state_norm(state, dim), 1, eps);
        Eigen::VectorXcd test_state(dim);
        for (ITYPE i = 0; i < dim; ++i) test_state[i] = state[i];

        for (UINT target = 0; target < n; ++target) {
            initialize_Haar_random_state(buffer, dim);
            CTYPE inp = state_inner_product(buffer, state, dim);
            Eigen::VectorXcd test_buffer(dim);
            for (ITYPE i = 0; i < dim; ++i) test_buffer[i] = buffer[i];
            std::complex<double> test_inp = (test_buffer.adjoint() * test_state);
            ASSERT_NEAR(creal(inp), test_inp.real(), eps);
            ASSERT_NEAR(cimag(inp), test_inp.imag(), eps);
        }
    }
    release_quantum_state(state);
    release_quantum_state(buffer);
}

// single qubit expectation value
TEST(StatOperationTest, SingleQubitExpectationValueTest) {
    const UINT n = 6;
    const ITYPE dim = 1ULL << n;
    const UINT max_repeat = 10;
    const double eps = 1e-14;

    CTYPE* state = allocate_quantum_state(dim);
    Eigen::MatrixXcd Identity(2, 2), X(2, 2), Y(2, 2), Z(2, 2);
    Eigen::MatrixXcd pauli_op;
    Identity << 1, 0, 0, 1;
    X << 0, 1, 1, 0;
    Z << 1, 0, 0, -1;
    Y << 0, -1.i, 1.i, 0;

    for (UINT rep = 0; rep < max_repeat; ++rep) {
        initialize_Haar_random_state(state, dim);
        ASSERT_NEAR(state_norm(state, dim), 1, eps);
        Eigen::VectorXcd test_state(dim);
        for (ITYPE i = 0; i < dim; ++i) test_state[i] = state[i];

        for (UINT target = 0; target < n; ++target) {
            // single qubit expectation value check
            target = rand_int(n);
            UINT pauli = rand_int(3) + 1;
            if (pauli == 0) pauli_op = Identity;
            else if (pauli == 1) pauli_op = X;
            else if (pauli == 2) pauli_op = Y;
            else if (pauli == 3) pauli_op = Z;
            std::complex<double> value = (test_state.adjoint()*get_expanded_eigen_matrix_with_identity(target, pauli_op, n)*test_state);
            ASSERT_NEAR(value.imag(), 0, eps);
            double test_expectation = value.real();
            double expectation = expectation_value_single_qubit_Pauli_operator(target, pauli, state, dim);
            ASSERT_NEAR(expectation, test_expectation, eps);
        }
    }
    release_quantum_state(state);
}

// multi qubit expectation value whole
TEST(StatOperationTest, MultiQubitExpectationValueWholeTest) {
    const UINT n = 6;
    const ITYPE dim = 1ULL << n;
    const UINT max_repeat = 10;
    const double eps = 1e-14;

    CTYPE* state = allocate_quantum_state(dim);
    Eigen::MatrixXcd Identity(2, 2), X(2, 2), Y(2, 2), Z(2, 2);
    Eigen::MatrixXcd pauli_op;
    Identity << 1, 0, 0, 1;
    X << 0, 1, 1, 0;
    Z << 1, 0, 0, -1;
    Y << 0, -1.i, 1.i, 0;

    for (UINT rep = 0; rep < max_repeat; ++rep) {
        initialize_Haar_random_state(state, dim);
        ASSERT_NEAR(state_norm(state, dim), 1, eps);
        Eigen::VectorXcd test_state(dim);
        for (ITYPE i = 0; i < dim; ++i) test_state[i] = state[i];

        for (UINT target = 0; target < n; ++target) {
            // multi qubit expectation whole list value check
            Eigen::MatrixXcd mat = Eigen::MatrixXcd::Identity(1, 1);
            std::vector<UINT> pauli_whole;
            for (UINT i = 0; i < n; ++i) {
                UINT pauli = rand_int(4);
                if (pauli == 0) pauli_op = Identity;
                else if (pauli == 1) pauli_op = X;
                else if (pauli == 2) pauli_op = Y;
                else if (pauli == 3) pauli_op = Z;
                mat = kronecker_product(pauli_op, mat);
                pauli_whole.push_back(pauli);
            }
            std::complex<double> value = (test_state.adjoint()*mat*test_state);
            ASSERT_NEAR(value.imag(), 0, eps);
            double test_expectation = value.real();
            double expectation = expectation_value_multi_qubit_Pauli_operator_whole_list(pauli_whole.data(), n, state, dim);
            ASSERT_NEAR(expectation, test_expectation, eps);
        }
    }
    release_quantum_state(state);
}

// multi qubit expectation value whole
TEST(StatOperationTest, MultiQubitExpectationValueZopWholeTest) {
	const UINT n = 6;
	const ITYPE dim = 1ULL << n;
	const UINT max_repeat = 10;
	const double eps = 1e-14;

	CTYPE* state = allocate_quantum_state(dim);
	Eigen::MatrixXcd Identity(2, 2), X(2, 2), Y(2, 2), Z(2, 2);
	Eigen::MatrixXcd pauli_op;
	Identity << 1, 0, 0, 1;
	X << 0, 1, 1, 0;
	Z << 1, 0, 0, -1;
	Y << 0, -1.i, 1.i, 0;

	for (UINT rep = 0; rep < max_repeat; ++rep) {
		initialize_Haar_random_state(state, dim);
		ASSERT_NEAR(state_norm(state, dim), 1, eps);
		Eigen::VectorXcd test_state(dim);
		for (ITYPE i = 0; i < dim; ++i) test_state[i] = state[i];

		for (UINT target = 0; target < n; ++target) {
			// multi qubit expectation whole list value check
			Eigen::MatrixXcd mat = Eigen::MatrixXcd::Identity(1, 1);
			std::vector<UINT> pauli_whole;
			for (UINT i = 0; i < n; ++i) {
				UINT pauli = rand_int(2);
				if (pauli == 1) pauli = 3;
				if (pauli == 0) pauli_op = Identity;
				else pauli_op = Z;
				mat = kronecker_product(pauli_op, mat);
				pauli_whole.push_back(pauli);
			}
			std::complex<double> value = (test_state.adjoint()*mat*test_state);
			ASSERT_NEAR(value.imag(), 0, eps);
			double test_expectation = value.real();
			double expectation = expectation_value_multi_qubit_Pauli_operator_whole_list(pauli_whole.data(), n, state, dim);
			ASSERT_NEAR(expectation, test_expectation, eps);
		}
	}
	release_quantum_state(state);
}

// multi qubit expectation value whole
TEST(StatOperationTest, MultiQubitExpectationValuePartialTest) {
    const UINT n = 6;
    const ITYPE dim = 1ULL << n;
    const double eps = 1e-14;
    const UINT max_repeat = 10;

    CTYPE* state = allocate_quantum_state(dim);
    for (UINT rep = 0; rep < max_repeat; ++rep) {
        initialize_Haar_random_state(state, dim);
        ASSERT_NEAR(state_norm(state, dim), 1, eps);
        Eigen::VectorXcd test_state(dim);
        for (ITYPE i = 0; i < dim; ++i) test_state[i] = state[i];

        Eigen::MatrixXcd Identity(2,2), X(2,2), Y(2,2), Z(2,2);
        Identity << 1, 0, 0, 1;
        X << 0, 1, 1, 0;
        Z << 1, 0, 0, -1;
        Y << 0, -1.i, 1.i, 0;

        for (UINT target = 0; target < n; ++target) {
            // multi qubit expectation partial list value check
            Eigen::MatrixXcd mat = Eigen::MatrixXcd::Identity(1, 1);
            Eigen::MatrixXcd pauli_op;

            std::vector<UINT> pauli_partial, pauli_index;
            std::vector<std::pair<UINT, UINT>> pauli_partial_pair;
            for (UINT i = 0; i < n; ++i) {
                UINT pauli = rand_int(4);
                if (pauli == 0) pauli_op = Identity;
                else if (pauli == 1) pauli_op = X;
                else if (pauli == 2) pauli_op = Y;
                else if (pauli == 3) pauli_op = Z;
                mat = kronecker_product(pauli_op, mat);
                if (pauli != 0) {
                    pauli_partial_pair.push_back(std::make_pair(i, pauli));
                }
            }
            std::random_shuffle(pauli_partial_pair.begin(), pauli_partial_pair.end());
            for (auto val : pauli_partial_pair) {
                pauli_index.push_back(val.first);
                pauli_partial.push_back(val.second);
            }
            std::complex<double> value = (test_state.adjoint()*mat*test_state);
            ASSERT_NEAR(value.imag(), 0, eps);
            double test_expectation = value.real();
            double expectation = expectation_value_multi_qubit_Pauli_operator_partial_list(pauli_index.data(), pauli_partial.data(), (UINT)pauli_index.size(), state,dim);
            ASSERT_NEAR(expectation, test_expectation, eps);
        }
    }
    release_quantum_state(state);
}

// multi qubit expectation value whole
TEST(StatOperationTest, MultiQubitExpectationValueZopPartialTest) {
	const UINT n = 6;
	const ITYPE dim = 1ULL << n;
	const double eps = 1e-14;
	const UINT max_repeat = 10;

	CTYPE* state = allocate_quantum_state(dim);
	for (UINT rep = 0; rep < max_repeat; ++rep) {
		initialize_Haar_random_state(state, dim);
		ASSERT_NEAR(state_norm(state, dim), 1, eps);
		Eigen::VectorXcd test_state(dim);
		for (ITYPE i = 0; i < dim; ++i) test_state[i] = state[i];

		Eigen::MatrixXcd Identity(2, 2), X(2, 2), Y(2, 2), Z(2, 2);
		Identity << 1, 0, 0, 1;
		X << 0, 1, 1, 0;
		Z << 1, 0, 0, -1;
		Y << 0, -1.i, 1.i, 0;

		for (UINT target = 0; target < n; ++target) {
			// multi qubit expectation partial list value check
			Eigen::MatrixXcd mat = Eigen::MatrixXcd::Identity(1, 1);
			Eigen::MatrixXcd pauli_op;

			std::vector<UINT> pauli_partial, pauli_index;
			std::vector<std::pair<UINT, UINT>> pauli_partial_pair;
			for (UINT i = 0; i < n; ++i) {
				UINT pauli = rand_int(2);
				if (pauli == 1) pauli = 3;
				if (pauli == 0) pauli_op = Identity;
				else pauli_op = Z;
				mat = kronecker_product(pauli_op, mat);
				if (pauli != 0) {
					pauli_partial_pair.push_back(std::make_pair(i, pauli));
				}
			}
			std::random_shuffle(pauli_partial_pair.begin(), pauli_partial_pair.end());
			for (auto val : pauli_partial_pair) {
				pauli_index.push_back(val.first);
				pauli_partial.push_back(val.second);
			}
			std::complex<double> value = (test_state.adjoint()*mat*test_state);
			ASSERT_NEAR(value.imag(), 0, eps);
			double test_expectation = value.real();
			double expectation = expectation_value_multi_qubit_Pauli_operator_partial_list(pauli_index.data(), pauli_partial.data(), (UINT)pauli_index.size(), state, dim);
			ASSERT_NEAR(expectation, test_expectation, eps);
		}
	}
	release_quantum_state(state);
}

// multi qubit expectation value whole
TEST(StatOperationTest, MultiQubitTransitionAmplitudeWholeTest) {
	const UINT n = 6;
	const ITYPE dim = 1ULL << n;
	const UINT max_repeat = 10;
	const double eps = 1e-14;

	CTYPE* state_ket = allocate_quantum_state(dim);
	CTYPE* state_bra = allocate_quantum_state(dim);
	Eigen::MatrixXcd Identity(2, 2), X(2, 2), Y(2, 2), Z(2, 2);
	Eigen::MatrixXcd pauli_op;
	Identity << 1, 0, 0, 1;
	X << 0, 1, 1, 0;
	Z << 1, 0, 0, -1;
	Y << 0, -1.i, 1.i, 0;

	for (UINT rep = 0; rep < max_repeat; ++rep) {
		initialize_Haar_random_state(state_ket, dim);
		initialize_Haar_random_state(state_bra, dim);
		ASSERT_NEAR(state_norm(state_ket, dim), 1, eps);
		ASSERT_NEAR(state_norm(state_bra, dim), 1, eps);

		Eigen::VectorXcd test_state_ket(dim);
		Eigen::VectorXcd test_state_bra(dim);
		for (ITYPE i = 0; i < dim; ++i) test_state_ket[i] = state_ket[i];
		for (ITYPE i = 0; i < dim; ++i) test_state_bra[i] = state_bra[i];

		for (UINT target = 0; target < n; ++target) {
			// multi qubit expectation whole list value check
			Eigen::MatrixXcd mat = Eigen::MatrixXcd::Identity(1, 1);
			std::vector<UINT> pauli_whole;
			for (UINT i = 0; i < n; ++i) {
				UINT pauli = rand_int(4);
				if (pauli == 0) pauli_op = Identity;
				else if (pauli == 1) pauli_op = X;
				else if (pauli == 2) pauli_op = Y;
				else if (pauli == 3) pauli_op = Z;
				mat = kronecker_product(pauli_op, mat);
				pauli_whole.push_back(pauli);
			}
			std::complex<double> test_transition_amplitude = (test_state_bra.adjoint()*mat*test_state_ket);
			CTYPE transition_amplitude = transition_amplitude_multi_qubit_Pauli_operator_whole_list(pauli_whole.data(), n, state_bra, state_ket, dim);
			ASSERT_NEAR(creal(transition_amplitude), test_transition_amplitude.real(), eps);
			ASSERT_NEAR(cimag(transition_amplitude), test_transition_amplitude.imag(), eps);
		}
	}
	release_quantum_state(state_ket);
	release_quantum_state(state_bra);
}

// multi qubit expectation value whole
TEST(StatOperationTest, MultiQubitTransitionAmplitudeZopWholeTest) {
	const UINT n = 6;
	const ITYPE dim = 1ULL << n;
	const UINT max_repeat = 10;
	const double eps = 1e-14;

	CTYPE* state_ket = allocate_quantum_state(dim);
	CTYPE* state_bra = allocate_quantum_state(dim);
	Eigen::MatrixXcd Identity(2, 2), X(2, 2), Y(2, 2), Z(2, 2);
	Eigen::MatrixXcd pauli_op;
	Identity << 1, 0, 0, 1;
	X << 0, 1, 1, 0;
	Z << 1, 0, 0, -1;
	Y << 0, -1.i, 1.i, 0;

	for (UINT rep = 0; rep < max_repeat; ++rep) {
		initialize_Haar_random_state(state_ket, dim);
		initialize_Haar_random_state(state_bra, dim);
		ASSERT_NEAR(state_norm(state_ket, dim), 1, eps);
		ASSERT_NEAR(state_norm(state_bra, dim), 1, eps);

		Eigen::VectorXcd test_state_ket(dim);
		Eigen::VectorXcd test_state_bra(dim);
		for (ITYPE i = 0; i < dim; ++i) test_state_ket[i] = state_ket[i];
		for (ITYPE i = 0; i < dim; ++i) test_state_bra[i] = state_bra[i];

		for (UINT target = 0; target < n; ++target) {
			// multi qubit expectation whole list value check
			Eigen::MatrixXcd mat = Eigen::MatrixXcd::Identity(1, 1);
			std::vector<UINT> pauli_whole;
			for (UINT i = 0; i < n; ++i) {
				UINT pauli = rand_int(2);
				if (pauli == 1) pauli = 3;
				if (pauli == 0) pauli_op = Identity;
				else pauli_op = Z;
				mat = kronecker_product(pauli_op, mat);
				pauli_whole.push_back(pauli);
			}
			std::complex<double> test_transition_amplitude = (test_state_bra.adjoint()*mat*test_state_ket);
			CTYPE transition_amplitude = transition_amplitude_multi_qubit_Pauli_operator_whole_list(pauli_whole.data(), n, state_bra, state_ket, dim);
			ASSERT_NEAR(creal(transition_amplitude), test_transition_amplitude.real(), eps);
			ASSERT_NEAR(cimag(transition_amplitude), test_transition_amplitude.imag(), eps);
		}
	}
	release_quantum_state(state_ket);
	release_quantum_state(state_bra);
}

// multi qubit expectation value whole
TEST(StatOperationTest, MultiQubitTransitionAmplitudePartialTest) {
	const UINT n = 6;
	const ITYPE dim = 1ULL << n;
	const UINT max_repeat = 10;
	const double eps = 1e-14;

	CTYPE* state_ket = allocate_quantum_state(dim);
	CTYPE* state_bra = allocate_quantum_state(dim);
	Eigen::MatrixXcd Identity(2, 2), X(2, 2), Y(2, 2), Z(2, 2);
	Eigen::MatrixXcd pauli_op;
	Identity << 1, 0, 0, 1;
	X << 0, 1, 1, 0;
	Z << 1, 0, 0, -1;
	Y << 0, -1.i, 1.i, 0;

	for (UINT rep = 0; rep < max_repeat; ++rep) {
		initialize_Haar_random_state(state_ket, dim);
		initialize_Haar_random_state(state_bra, dim);
		ASSERT_NEAR(state_norm(state_ket, dim), 1, eps);
		ASSERT_NEAR(state_norm(state_bra, dim), 1, eps);

		Eigen::VectorXcd test_state_ket(dim);
		Eigen::VectorXcd test_state_bra(dim);
		for (ITYPE i = 0; i < dim; ++i) test_state_ket[i] = state_ket[i];
		for (ITYPE i = 0; i < dim; ++i) test_state_bra[i] = state_bra[i];

		for (UINT target = 0; target < n; ++target) {
			// multi qubit expectation partial list value check
			Eigen::MatrixXcd mat = Eigen::MatrixXcd::Identity(1, 1);
			Eigen::MatrixXcd pauli_op;

			std::vector<UINT> pauli_partial, pauli_index;
			std::vector<std::pair<UINT, UINT>> pauli_partial_pair;
			for (UINT i = 0; i < n; ++i) {
				UINT pauli = rand_int(4);
				if (pauli == 0) pauli_op = Identity;
				else if (pauli == 1) pauli_op = X;
				else if (pauli == 2) pauli_op = Y;
				else if (pauli == 3) pauli_op = Z;
				mat = kronecker_product(pauli_op, mat);
				if (pauli != 0) {
					pauli_partial_pair.push_back(std::make_pair(i, pauli));
				}
			}
			std::random_shuffle(pauli_partial_pair.begin(), pauli_partial_pair.end());
			for (auto val : pauli_partial_pair) {
				pauli_index.push_back(val.first);
				pauli_partial.push_back(val.second);
			}
			std::complex<double> test_transition_amplitude = (test_state_bra.adjoint()*mat*test_state_ket);
			CTYPE transition_amplitude = transition_amplitude_multi_qubit_Pauli_operator_partial_list(pauli_index.data(), pauli_partial.data(), (UINT)pauli_index.size(), state_bra, state_ket, dim);
			ASSERT_NEAR(creal(transition_amplitude), test_transition_amplitude.real(), eps);
			ASSERT_NEAR(cimag(transition_amplitude), test_transition_amplitude.imag(), eps);
		}
	}
	release_quantum_state(state_ket);
	release_quantum_state(state_bra);
}

// multi qubit expectation value whole
TEST(StatOperationTest, MultiQubitTransitionAmplitudeZopPartialTest) {
	const UINT n = 6;
	const ITYPE dim = 1ULL << n;
	const UINT max_repeat = 10;
	const double eps = 1e-14;

	CTYPE* state_ket = allocate_quantum_state(dim);
	CTYPE* state_bra = allocate_quantum_state(dim);
	Eigen::MatrixXcd Identity(2, 2), X(2, 2), Y(2, 2), Z(2, 2);
	Eigen::MatrixXcd pauli_op;
	Identity << 1, 0, 0, 1;
	X << 0, 1, 1, 0;
	Z << 1, 0, 0, -1;
	Y << 0, -1.i, 1.i, 0;

	for (UINT rep = 0; rep < max_repeat; ++rep) {
		initialize_Haar_random_state(state_ket, dim);
		initialize_Haar_random_state(state_bra, dim);
		ASSERT_NEAR(state_norm(state_ket, dim), 1, eps);
		ASSERT_NEAR(state_norm(state_bra, dim), 1, eps);

		Eigen::VectorXcd test_state_ket(dim);
		Eigen::VectorXcd test_state_bra(dim);
		for (ITYPE i = 0; i < dim; ++i) test_state_ket[i] = state_ket[i];
		for (ITYPE i = 0; i < dim; ++i) test_state_bra[i] = state_bra[i];

		for (UINT target = 0; target < n; ++target) {
			// multi qubit expectation partial list value check
			Eigen::MatrixXcd mat = Eigen::MatrixXcd::Identity(1, 1);
			Eigen::MatrixXcd pauli_op;

			std::vector<UINT> pauli_partial, pauli_index;
			std::vector<std::pair<UINT, UINT>> pauli_partial_pair;
			for (UINT i = 0; i < n; ++i) {
				UINT pauli = rand_int(2);
				if (pauli == 1) pauli = 3;
				if (pauli == 0) pauli_op = Identity;
				else pauli_op = Z;
				mat = kronecker_product(pauli_op, mat);
				if (pauli != 0) {
					pauli_partial_pair.push_back(std::make_pair(i, pauli));
				}
			}
			std::random_shuffle(pauli_partial_pair.begin(), pauli_partial_pair.end());
			for (auto val : pauli_partial_pair) {
				pauli_index.push_back(val.first);
				pauli_partial.push_back(val.second);
			}
			std::complex<double> test_transition_amplitude = (test_state_bra.adjoint()*mat*test_state_ket);
			CTYPE transition_amplitude = transition_amplitude_multi_qubit_Pauli_operator_partial_list(pauli_index.data(), pauli_partial.data(), (UINT)pauli_index.size(), state_bra, state_ket, dim);
			ASSERT_NEAR(creal(transition_amplitude), test_transition_amplitude.real(), eps);
			ASSERT_NEAR(cimag(transition_amplitude), test_transition_amplitude.imag(), eps);
		}
	}
	release_quantum_state(state_ket);
	release_quantum_state(state_bra);
}


//==================================================
// cubicBezier
//==================================================
#define BOOST_PYTHON_STATIC_LIB
#define BOOST_NUMPY_STATIC_LIB
#include <Python.h>
#include <valarray>
#include <iostream>
#include <boost/python.hpp>
#include <boost/shared_ptr.hpp>

/*
 ◆開発環境
 [OS]: macOS 10.14~
 [c++] gcc version 9.2.0
 [boost] 1.71.0
 [Python] v2.7.16
 */

typedef std::valarray<double> darray;
typedef std::valarray<darray> d2array;
namespace py = boost::python;
namespace cubicBezier
{
    darray cubicBezier(darray p0, darray p1, darray p2, darray p3, double u, bool restraint=true)
    {
        darray _p0 (2);
        darray _p1 (2);
        darray _p2 (2);
        darray _p3 (2);
        _p0 = p0; //# start-point
        _p1 = p1; //# start-point weighted
        _p2 = p2; //# end-point weighted
        _p3 = p3; //# end-point

        if(restraint)
        { // p0, p1, p2, p3のx値が互い違いになっている場合の為の処理。
            // p0[0]==3, p1[0]==1の様にp0がp1より先のx地点にある場合。
            double scale;
            double btKey;
            double vx_A;
            double vx_B;
            btKey = _p3[0] - _p0[0];
            vx_A = _p1[0] - _p0[0];
            vx_B = _p3[0] - _p2[0];
            scale = btKey/(vx_A + vx_B);
            if(scale<1)
            {
                darray vec_A (2);
                darray vec_B (2);
                vec_A = _p1 - p0;
                vec_B = _p2 - p3;
                //vecB*scale;

                _p1 = vec_A*scale; //vec_A apply ([=](double v) (return v*sclae;))
                _p2 = vec_B*scale; //vec_B apply ([=](double v) (return v;))

                _p1 = _p1+_p0;
                _p2 = _p2+_p3;
            }
        }
        darray _p4 (2);
        darray _p5 (2);
        darray _p6 (2);
        darray _p7 (2);
        darray _p8 (2);
        darray _p9 (2);

        // 計算
        _p4 = (_p1 - _p0)*u +_p0; //
        _p5 = (_p2 - _p1)*u +_p1; //
        _p6 = (_p3 - _p2)*u +_p2; //
        _p7 = (_p5 - _p4)*u +_p4; //
        _p8 = (_p6 - _p5)*u +_p5; //
        _p9 = (_p8 - _p7)*u +_p7; //
        return _p9;
    }

    d2array sampling_cubicBezier_t(d2array points, int count=100)
    {
    /*
     cubicBezierのポイント４点を用いて４点間のcubicBezierの離散化したカーブ(point群)を生成する。
     */
    double u;
    darray p;
    d2array rList(count);
    for(int t=0; t <count; t++)
        {
            u = double(t) / double(count-1);
            p = cubicBezier::cubicBezier(points[0], points[1], points[2], points[3], u);
            rList[t] = p;
        }
    return rList;
    }

}

//==================================================
// convert
//==================================================
d2array convert_list_to_d2array(py::list array)
{
    // array is expected as a d2array (referenced-code)
    int count = py::len(array);
    d2array rArray(count);
    py::list val;

    for(int i=0; i<count; i++)
        {
            val = py::extract<py::list>(array[i]);
            int mCount = py::len(val);
            rArray[i] = darray(mCount);
            for(int m=0; m<mCount; m++)
                {
                rArray[i][m] = py::extract<double>(val[m]);
                }

        }
    return rArray;
}

py::list convert_array_to_list(d2array array)
{
    // array is expected as a d2array //(referencerd-code)
    int count = array.size();
    py::list rArray;
    for(int i=0; i<count; i++)
        {
            py::list temp;
            int mCount = array[i].size();
            for(int m=0; m<mCount; m++)
                {
                temp.append(array[i][m]);
                }
            rArray.append(temp);
        }
    return  rArray;
}

//==================================================
// python command
//==================================================
py::list py_sampling_cubicBezier(py::list points, int count=100)
{
           d2array _points(count);
           _points = convert_list_to_d2array(points);
           d2array tmpArray;

           tmpArray = cubicBezier::sampling_cubicBezier_t(_points, count);
           return convert_array_to_list(tmpArray);
}

py::list py_cubicBezier(py::list p0, py::list p1, py::list p2, py::list p3, double u, bool restraint=true )
{
    darray _p0(2);
    darray _p1(2);
    darray _p2(2);
    darray _p3(2);
    _p0[0] = py::extract<double>(p0[0]);
    _p0[1] = py::extract<double>(p0[1]); //# start-point
    _p1[0] = py::extract<double>(p1[0]);
    _p1[1] = py::extract<double>(p1[1]); //# start-point(weighted)
    _p2[0] = py::extract<double>(p2[0]);
    _p2[1] = py::extract<double>(p2[1]); //# end-point(weigted)
    _p3[0] = py::extract<double>(p3[0]);
    _p3[1] = py::extract<double>(p3[1]); //# end-point

    darray points = cubicBezier::cubicBezier(_p0, _p1, _p2, _p3, u, restraint);
    py::list rValue;
    for(auto& v:points)
       {
        rValue.append(v);
       }
    return rValue;
}

BOOST_PYTHON_MODULE(lib)
{
           def("cubicBezier", &py_cubicBezier);
           def("sampling_cubicBezier", &py_sampling_cubicBezier);
}


///////////////////////////////////////////////////////////////////////////////
// importance_sampling::apply_exp_offset                                     //
//                                                                           //
//  Copyright 2009 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_IMPORTANCE_SAMPLING_WEIGHTS_APPLY_EXP_OFFSET_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_IMPORTANCE_SAMPLING_WEIGHTS_APPLY_EXP_OFFSET_HPP_ER_2009
#include <cmath>
#include <algorithm>
#include <boost/lambda/lambda.hpp>
#include <boost/lambda/bind.hpp>
#include <boost/math/tools/precision.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace importance_sampling{

    // Returns          The smallest value, c, s/t *i + c <= t, i in [b_w,e_w)
    // Side effect:     *i <- exp(*i+c)
    //
    // The greater c, the higher the precision but also risk of isinf.
    // Textbook often show t = 0 so that exp(w+c) <= 1.
    template<typename It>
    typename iterator_value<It>::type
    apply_exp_offset(
        It b,
        It e,
        typename iterator_value<It>::type t
    ){
        typedef typename iterator_value<It>::type val_;
        val_ max = *std::max_element(b,e);
        val_ offset = (t - max);
        typedef val_(*fp_)(val_);
        fp_ fp = std::exp;
        std::transform(
        	b,
            e,
            b,
            lambda::bind<val_>(fp,lambda::_1 + offset)
        );
        return offset;
    }

    // Same as above, but t set such that exp(t+epsilon) = inf, exp(t)<inf
    template<typename It>
    typename iterator_value<It>::type
    apply_exp_offset(
        It b,
        It e
    ){
        typedef typename iterator_value<It>::type val_;
        const val_ log_max = boost::math::tools::log_max_value<val_>();

        return apply_exp_offset(
            b,
            e,
            log_max
        );
    }

}// importance_weights
}// detail
}// statistics
}// boost

#endif


/**
 * \file
 * \author Thomas Fischer
 * \date   2011-03-17
 * \brief  Implementation of the AngleSkewMetric class.
 *
 * \copyright
 * Copyright (c) 2012-2022, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/project/license
 *
 */

#include "AngleSkewMetric.h"

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

#include "MathLib/MathTools.h"
#include "MeshLib/Node.h"

using namespace boost::math::double_constants;

namespace MeshLib
{
namespace
{
template <unsigned long N>
std::tuple<double, double> getMinMaxAngle(
    std::array<MeshLib::Node, N> const& nodes)
{
    double min_angle(two_pi);
    double max_angle(0.0);

    for (decltype(N) i = 0; i < N; ++i)
    {
        const double angle(MathLib::getAngle(nodes[i], nodes[(i + 1) % N],
                                             nodes[(i + 2) % N]));
        min_angle = std::min(angle, min_angle);
        max_angle = std::max(angle, max_angle);
    }
    return {min_angle, max_angle};
}

double checkTriangle(Element const& elem)
{
    std::array const nodes = {*elem.getNode(0), *elem.getNode(1),
                              *elem.getNode(2)};
    auto const& [min_angle, max_angle] = getMinMaxAngle(nodes);
    return std::max((max_angle - third_pi) / two_thirds_pi,
                    (third_pi - min_angle) / third_pi);
}

double checkQuad(Element const& elem)
{
    std::array const nodes = {*elem.getNode(0), *elem.getNode(1),
                              *elem.getNode(2), *elem.getNode(3)};
    auto const& [min_angle, max_angle] = getMinMaxAngle(nodes);

    return std::max((max_angle - half_pi) / half_pi,
                    (half_pi - min_angle) / half_pi);
}

double checkTetrahedron(Element const& elem)
{
    std::array<double, 4> min;
    std::array<double, 4> max;
    for (auto face_number = 0; face_number < 4; ++face_number)
    {
        std::unique_ptr<Element const> face{elem.getFace(face_number)};
        std::array const nodes = {*face->getNode(0), *face->getNode(1),
                                  *face->getNode(2)};
        std::tie(min[face_number], max[face_number]) = getMinMaxAngle(nodes);
    }

    double const min_angle = *std::min_element(min.begin(), min.end());
    double const max_angle = *std::max_element(max.begin(), max.end());

    return std::max((max_angle - third_pi) / two_thirds_pi,
                    (third_pi - min_angle) / third_pi);
}

double checkHexahedron(Element const& elem)
{
    std::array<double, 6> min;
    std::array<double, 6> max;
    for (auto face_number = 0; face_number < 6; ++face_number)
    {
        std::unique_ptr<Element const> face{elem.getFace(face_number)};
        std::array const nodes = {*face->getNode(0), *face->getNode(1),
                                  *face->getNode(2), *face->getNode(3)};
        std::tie(min[face_number], max[face_number]) = getMinMaxAngle(nodes);
    }

    double const min_angle = *std::min_element(min.begin(), min.end());
    double const max_angle = *std::max_element(max.begin(), max.end());

    return std::max((max_angle - half_pi) / half_pi,
                    (half_pi - min_angle) / half_pi);
}

double checkPrism(Element const& elem)
{
    // face 0: triangle (0,1,2)
    std::unique_ptr<Element const> f0{elem.getFace(0)};
    std::array const nodes_f0 = {*f0->getNode(0), *f0->getNode(1),
                                 *f0->getNode(2)};
    auto const& [min_angle_tri0, max_angle_tri0] = getMinMaxAngle(nodes_f0);

    // face 4: triangle (3,4,5)
    std::unique_ptr<Element const> f4{elem.getFace(4)};
    std::array const nodes_f4 = {*f4->getNode(0), *f4->getNode(1),
                                 *f4->getNode(2)};
    auto const& [min_angle_tri1, max_angle_tri1] = getMinMaxAngle(nodes_f4);

    auto const min_angle_tri = std::min(min_angle_tri0, min_angle_tri1);
    auto const max_angle_tri = std::max(max_angle_tri0, max_angle_tri1);

    double const tri_criterion(
        std::max((max_angle_tri - third_pi) / two_thirds_pi,
                 (third_pi - min_angle_tri) / third_pi));

    std::array<double, 3> min;
    std::array<double, 3> max;
    for (int i = 1; i < 4; ++i)
    {
        std::unique_ptr<Element const> f{elem.getFace(i)};
        std::array const nodes = {*f->getNode(0), *f->getNode(1),
                                  *f->getNode(2), *f->getNode(3)};
        std::tie(min[i - 1], max[i - 1]) = getMinMaxAngle(nodes);
    }

    double const min_angle_quad = *std::min_element(min.begin(), min.end());
    double const max_angle_quad = *std::max_element(max.begin(), max.end());

    double const quad_criterion(std::max((max_angle_quad - half_pi) / half_pi,
                                         (half_pi - min_angle_quad) / half_pi));

    return std::min(tri_criterion, quad_criterion);
}

}  // end unnamed namespace

void AngleSkewMetric::calculateQuality()
{
    for (auto const e : _mesh.getElements())
    {
        switch (e->getGeomType())
        {
            case MeshElemType::LINE:
                _element_quality_metric[e->getID()] = -1.0;
                break;
            case MeshElemType::TRIANGLE:
                _element_quality_metric[e->getID()] = checkTriangle(*e);
                break;
            case MeshElemType::QUAD:
                _element_quality_metric[e->getID()] = checkQuad(*e);
                break;
            case MeshElemType::TETRAHEDRON:
                _element_quality_metric[e->getID()] = checkTetrahedron(*e);
                break;
            case MeshElemType::HEXAHEDRON:
                _element_quality_metric[e->getID()] = checkHexahedron(*e);
                break;
            case MeshElemType::PRISM:
                _element_quality_metric[e->getID()] = checkPrism(*e);
                break;
            default:
                break;
        }
    }
}

}  // end namespace MeshLib


#include "model.h"
#include <math.h>
#include <time.h>
#include <boost/random.hpp>

Model::Model()
{
}

void Model::init(double dt, int m, int n, int nh, double sigma, double sv0)
{
    this->dt = pow(10, dt);
    this->nh = nh;
    this->n = n;
    this->m = m;
    if (nh % 2 == 0)
        this->N = nh*nh - nh/2;
    else
        this->N = nh*nh - (nh-1)/2;
    this->sigma = sigma;
    this->sv0 = sv0;

    update();

    p.resize(2, N);
    q.resize(2, N);
    f1.resize(2, N);
    f2.resize(2, N);
    f3.resize(2, N);
    f4.resize(2, N);
    v1.resize(2, N);
    v2.resize(2, N);
    v3.resize(2, N);
    v4.resize(2, N);

    typedef boost::mt19937                     ENG;    // Mersenne Twister
    typedef boost::normal_distribution<double> DIST;   // Normal Distribution
    typedef boost::variate_generator<ENG,DIST> GEN;    // Variate generator

    ENG  eng;
    DIST dist(0, sv0);
    GEN  gen(eng, dist);

    gen.engine().seed(time(NULL) + ::getpid());
    gen.distribution().reset();

    Eigen::Vector2d
            qi(sigma - 0.5, sigma - 0.5),
            n1(sigma, 0),
            n2(sigma/2, sigma*sqrt(3)/2);

    for (int i=0; i<N; ++i) {
        p(0,i) = gen();
        p(1,i) = gen();
        q.col(i) = qi;
        qi += n1;
        if (qi[0] > nh*sigma - 0.25)
            qi += n2 - nh*n1;
    }
}

void Model::update()
{
    this->mass = 1.0;
    this->boxW = sigma * (nh + 1) - 1;
    this->boxH = sigma * ((nh - 1) * sqrt(3)/2 + 2) - 1;
    this->packingFractionHCP = (N * M_PI / 4) / (boxW * boxH) / (M_PI / (2*sqrt(3)));
    if (m == 0)
        this->cutoff = pow(1e-10, -1./(n+1.));
    else
        this->cutoff = pow((n/(double)m-1.)*1e-10, -1./(m+1.));
}

void Model::forces(const Eigen::Matrix2Xd &p, const Eigen::Matrix2Xd &q,
                   Eigen::Matrix2Xd &f, Eigen::Matrix2Xd &v)
{
    Eigen::Vector2d rij, fij;
    double r;

    for (int i=0; i<N; ++i)
        f.col(i) << 0, 0;

    for (int i=0; i<N; ++i) {
        fij << n*pow(q(0,i)+0.5, -n-1) - m*pow(q(0,i)+0.5, -m-1) - \
               n*pow(boxW-q(0,i)+0.5, -n-1) + m*pow(boxW-q(0,i)+0.5, -m-1),
               n*pow(q(1,i)+0.5, -n-1) - m*pow(q(1,i)+0.5, -m-1) - \
               n*pow(boxH-q(1,i)+0.5, -n-1) + m*pow(boxH-q(1,i)+0.5, -m-1);
        f.col(i) = f.col(i) + fij;

        for (int j=i+1; j<N; ++j) {
            rij = q.col(j) - q.col(i);
            r = rij.norm();
            if (r > cutoff)
                continue;
            fij = (n*pow(r, -n-1) - m*pow(r, -m-1)) * rij.normalized();
            f.col(j) = f.col(j) + fij;
            f.col(i) = f.col(i) - fij;
        }
    }

    v = p;
}

void Model::rk4step()
{
    forces(p,             q,             f1, v1);
    forces(p + 0.5*dt*f1, q + 0.5*dt*v1, f2, v2);
    forces(p + 0.5*dt*f2, q + 0.5*dt*v2, f3, v3);
    forces(p + dt*f3,     q + dt*v3,     f4, v4);

    p += dt * (f1 + 2*(f2 + f3) + f4) / 6.0;
    q += dt * (v1 + 2*(v2 + v3) + v4) / 6.0;

    t += dt;
}


#include <iostream>
#include <fstream>
#include <vector>
#include <boost/numeric/ublas/matrix.hpp>

const int columns = 5;
const int rows = 5;

std::vector<int> getBingoNumbers(std::ifstream *file);
std::vector<boost::numeric::ublas::bounded_matrix<int, rows, columns>> getBingoCards(std::ifstream *file);
bool checkRow(int row, boost::numeric::ublas::bounded_matrix<bool, rows, columns> *bingoCardHistory);
bool checkColumn(int column, boost::numeric::ublas::bounded_matrix<bool, rows, columns> *bingoCardHistory);
int calcWinningScore(int winningNumber, boost::numeric::ublas::bounded_matrix<int, rows, columns> *bingoCard, boost::numeric::ublas::bounded_matrix<bool, rows, columns> *bingoCardHistory);

int main() {
	std::cout << "Hello world" << std::endl;
  std::ifstream bingoNumbersFile("hello-world.txt");
  if (!bingoNumbersFile.is_open())
  {
    std::cout << "No file 'hello-world.txt' found! Did you name it correctly?" << std::endl;
    exit(1);
  }

  std::vector<int> bingoNumbers = getBingoNumbers(&bingoNumbersFile);
  std::vector<boost::numeric::ublas::bounded_matrix<int, rows, columns>> bingoCards = getBingoCards(&bingoNumbersFile);

  std::vector<boost::numeric::ublas::bounded_matrix<bool, rows, columns>> bingoCardsHistory(bingoCards.size());

  int winner;
  //loop over all numbers called
  for (size_t i = 0; i < bingoNumbers.size(); i++)
  {
    int bingoNumber = bingoNumbers[i];
    //loop over all bingocards
    for (size_t j = 0; j < bingoCards.size(); j++)
    {
      boost::numeric::ublas::bounded_matrix<int, rows, columns> bingoCard = bingoCards[j];
      //loop over rows of bingocard
      for (size_t r = 0; r < rows; r++)
      {
        //loop over columns of the row
        for (size_t c = 0; c < columns; c++)
        {
          if (bingoCard(r, c) == bingoNumber)
          {
            bingoCardsHistory[j](r,c) = 1;
            if (checkRow(r, &bingoCardsHistory[j]) || checkColumn(c, &bingoCardsHistory[j]))
            {
              winner = calcWinningScore(bingoNumber, &bingoCard, &bingoCardsHistory[j]);
              std::cout << "ow sheesh :s" << std::endl;
              return 0;
            }
          }
        }
      }
    }
  }
}

int calcWinningScore(int winningNumber, boost::numeric::ublas::bounded_matrix<int, rows, columns> *bingoCard, boost::numeric::ublas::bounded_matrix<bool, rows, columns> *bingoCardHistory) {
  int sum = 0;
  for (size_t i = 0; i < rows; i++)
  {
    for (size_t j = 0; j < columns; j++)
    {
      if ((*bingoCardHistory)(i,j) != 1)
      {
        sum += (*bingoCard)(i,j);
      }
    }
  }
  std::cout << "winning number: " << sum * winningNumber << std::endl;
  return sum * winningNumber;
}

bool checkRow(int row, boost::numeric::ublas::bounded_matrix<bool, rows, columns> *bingoCardHistory) {
  for (size_t i = 0; i < columns; i++)
  {
    if ((*bingoCardHistory)(row, i) != 1)
    {
      return 0;
    }
  }
  return 1;
}

bool checkColumn(int column, boost::numeric::ublas::bounded_matrix<bool, rows, columns> *bingoCardHistory) {
  for (size_t i = 0; i < rows; i++)
  {
    if ((*bingoCardHistory)(i, column) != 1)
    {
      return 0;
    }
  }
  return 1;
}

std::vector<boost::numeric::ublas::bounded_matrix<int, rows, columns>> getBingoCards(std::ifstream *file) {
  std::vector<boost::numeric::ublas::bounded_matrix<int, rows, columns>> bingoCards;

  int currentColumn = 0;
  int currentRow = 0;
  boost::numeric::ublas::bounded_matrix<int, rows, columns> bingoCard;
  int x;
  while (*file >> x)
  {
    bingoCard(currentRow, currentColumn) = x;

    currentColumn ++;
    if (currentColumn == columns)
    {
      currentColumn = 0;
      currentRow ++;
    }
    if (currentRow == rows)
    {
      currentRow = 0;
      bingoCards.push_back(bingoCard);
    }
  }
  return bingoCards;
}

std::vector<int> getBingoNumbers(std::ifstream *file) {
  std::string bingoNumbersString;
  std::getline(*file, bingoNumbersString);

  std::string delimiter = ",";
  size_t pos = 0;

  std::vector<int> bingoNumbers;
  std::string token;
  while ((pos = bingoNumbersString.find(delimiter)) != std::string::npos) {
      bingoNumbers.push_back(std::stoi(bingoNumbersString.substr(0, pos)));
      bingoNumbersString.erase(0, pos + delimiter.length());
  }

  return bingoNumbers;
}

day 04/Marnix - C++/hello-world-1/hello-world.cpp


#include "aikido/planner/vectorfield/VectorFieldUtil.hpp"

#include <Eigen/Geometry>
#include <dart/optimizer/Solver.hpp>
#include <dart/optimizer/nlopt/NloptSolver.hpp>

#include "aikido/common/algorithm.hpp"
#include "aikido/trajectory/Spline.hpp"

namespace aikido {
namespace planner {
namespace vectorfield {

namespace {

/// A function class that defines an objective. The objective measures
/// the difference between a desired twist and Jacobian * joint velocities.
///
class DesiredTwistFunction : public dart::optimizer::Function
{
public:
  using Twist = Eigen::Vector6d;
  using Jacobian = dart::math::Jacobian;

  /// Constructor.
  ///
  /// \param[in] twist A desired twist.
  /// \param[in] jacobian System Jacobian.
  DesiredTwistFunction(const Twist& twist, const Jacobian& jacobian)
    : dart::optimizer::Function("DesiredTwistFunction")
    , mTwist(twist)
    , mJacobian(jacobian)
  {
    // Do nothing
  }

  /// Implementation inherited.
  /// Evaluating an objective by a state value.
  ///
  /// \param[in] qd Joint velocities.
  /// \return Objective value.
  double eval(const Eigen::VectorXd& qd) override
  {
    return 0.5 * (mJacobian * qd - mTwist).squaredNorm();
  }

  /// Implementation inherited.
  /// Evaluating gradient of an objective by a state value.
  /// \param[in] qd Joint velocities.
  /// \param[out] grad Gradient of a defined objective.
  void evalGradient(
      const Eigen::VectorXd& qd, Eigen::Map<Eigen::VectorXd> grad) override
  {
    grad = mJacobian.transpose() * (mJacobian * qd - mTwist);
  }

protected:
  /// Twist.
  Twist mTwist;

  /// Jacobian of Meta Skeleton.
  Jacobian mJacobian;

public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace

//==============================================================================
bool computeJointVelocityFromTwist(
    Eigen::VectorXd& jointVelocity,
    const Eigen::Vector6d& desiredTwist,
    const dart::dynamics::MetaSkeletonPtr metaSkeleton,
    const dart::dynamics::ConstBodyNodePtr bodyNode,
    double jointLimitPadding,
    const Eigen::VectorXd& jointVelocityLowerLimits,
    const Eigen::VectorXd& jointVelocityUpperLimits,
    bool enforceJointVelocityLimits,
    double stepSize)
{
  using dart::math::Jacobian;
  using dart::optimizer::Problem;
  using dart::optimizer::Solver;
  using Eigen::VectorXd;

  // Use LBFGS to find joint angles that won't violate the joint limits.
  const Jacobian jacobian = metaSkeleton->getWorldJacobian(bodyNode);

  const std::size_t numDofs = metaSkeleton->getNumDofs();

  jointVelocity = Eigen::VectorXd::Zero(numDofs);
  VectorXd positions = metaSkeleton->getPositions();
  VectorXd initialGuess = metaSkeleton->getVelocities();
  VectorXd positionLowerLimits = metaSkeleton->getPositionLowerLimits();
  VectorXd positionUpperLimits = metaSkeleton->getPositionUpperLimits();
  VectorXd velocityLowerLimits = jointVelocityLowerLimits;
  VectorXd velocityUpperLimits = jointVelocityUpperLimits;

  const auto problem = std::make_shared<Problem>(numDofs);
  if (enforceJointVelocityLimits)
  {
    for (std::size_t i = 0; i < numDofs; ++i)
    {
      const double position = positions[i];
      const double positionLowerLimit = positionLowerLimits[i];
      const double positionUpperLimit = positionUpperLimits[i];
      const double velocityLowerLimit = velocityLowerLimits[i];
      const double velocityUpperLimit = velocityUpperLimits[i];

      if (position + stepSize * velocityLowerLimit
          <= positionLowerLimit + jointLimitPadding)
      {
        velocityLowerLimits[i] = 0.0;
      }

      if (position + stepSize * velocityUpperLimit
          >= positionUpperLimit - jointLimitPadding)
      {
        velocityUpperLimits[i] = 0.0;
      }

      initialGuess[i] = common::clamp(
          initialGuess[i], velocityLowerLimits[i], velocityUpperLimits[i]);
    }
    problem->setLowerBounds(velocityLowerLimits);
    problem->setUpperBounds(velocityUpperLimits);
  }

  problem->setInitialGuess(initialGuess);
  problem->setObjective(dart::common::make_aligned_shared<DesiredTwistFunction>(
      desiredTwist, jacobian));

#if DART_VERSION_AT_LEAST(6, 9, 0)
  dart::optimizer::NloptSolver solver(
      problem, dart::optimizer::NloptSolver::LD_LBFGS);
#else
  dart::optimizer::NloptSolver solver(problem, nlopt::LD_LBFGS);
#endif
  if (!solver.solve())
  {
    return false;
  }
  jointVelocity = problem->getOptimalSolution();
  return true;
}

//==============================================================================
Eigen::Vector6d computeGeodesicTwist(
    const Eigen::Isometry3d& fromTrans, const Eigen::Isometry3d& toTrans)
{
  using dart::math::logMap;
  Eigen::Isometry3d relativeTrans = fromTrans.inverse() * toTrans;
  Eigen::Vector3d relativeTranslation
      = fromTrans.linear() * relativeTrans.translation();
  Eigen::Vector3d axisAngles = logMap(relativeTrans.linear());
  Eigen::Vector3d relativeAngles = fromTrans.linear() * axisAngles;
  Eigen::Vector6d geodesicTwist;
  geodesicTwist << relativeAngles, relativeTranslation;
  return geodesicTwist;
}

//==============================================================================
Eigen::Vector4d computeGeodesicError(
    const Eigen::Isometry3d& fromTrans, const Eigen::Isometry3d& toTrans)
{
  using dart::math::logMap;
  Eigen::Isometry3d relativeTrans = fromTrans.inverse() * toTrans;
  Eigen::Vector3d relativeTranslation
      = fromTrans.linear() * relativeTrans.translation();
  Eigen::Vector3d axisAngles = logMap(relativeTrans.linear());
  Eigen::Vector4d geodesicError;
  geodesicError << axisAngles.norm(), relativeTranslation;
  return geodesicError;
}

//==============================================================================
double computeGeodesicDistance(
    const Eigen::Isometry3d& fromTrans,
    const Eigen::Isometry3d& toTrans,
    double r)
{
  Eigen::Vector4d error = computeGeodesicError(fromTrans, toTrans);
  error[0] = r * error[0];
  return error.norm();
}

} // namespace vectorfield
} // namespace planner
} // namespace aikido


/*
Metaheuristic Optimization Using Population-Based Simulated Annealing.

Copyright (c) 2021 Gabriele Gilardi

    func            Function to minimize
    LB              Lower boundaries of the search space
    UB              Upper boundaries of the search space
    nPop            Number of agents (population)
    epochs          Number of iterations
    nMove           Number of neighbours of a state evaluated at each epoch
    T0              Initial temperature
    alphaT          Temperature reduction rate
    sigma0          Initial standard deviation used to search the neighboroud
                    of a state (given as a fraction of the search space)
    alphaS          Standard deviation reduction rate
    prob            Probability the dimension of a state is changed
    IntVar          List of indexes specifying which variable should be treated
                    as integer
    normalize       Specifies if the search space should be normalized
    args            Tuple containing any parameter that needs to be passed to
                    the function

    Dimensions:
    (nVar)          LB, UB, LB_orig, UB_orig, sigma, best_pos
    (nPop, nVar)    agent_pos, neigh_pos, agent_pos_orig, neigh_pos_orig
    (nPop)          agent_cost, neigh_cost
    (epochs)        F
    (0-nVar)        IntVar
*/

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

using namespace std;
using namespace Eigen;

/* Structure used to pass the parameters */
struct Parameters {
    int nPop;
    int epochs;
    int nMove;
    double T0;
    double alphaT;
    double sigma0;
    double alphaS;
    double prob;
    bool normalize;
    ArrayXi IntVar;
    ArrayXXd args;
    int seed;
};

/* Structure used to return the results */
struct Results {
    double best_cost;
    ArrayXXd best_pos;
    ArrayXd F;
    double T;
    ArrayXXd sigma;
};

/* Minimize a function using simulated annealing */
Results sa(ArrayXd (*func)(ArrayXXd, ArrayXXd), ArrayXXd LB, ArrayXXd UB,
           Parameters p)
{
    int nVar, nIntVar, idx;
    double T, prob_swap, delta, best_cost;
    Results res;

    /*Eigen declarations*/
    Index r_min, c_min;
    ArrayXd agent_cost, neigh_cost, F;
    ArrayXXd LBe, UBe, LBe_orig, UBe_orig, sigma, agent_pos, neigh_pos,
             agent_pos_orig, neigh_pos_orig, best_pos, rn, flips;

    /* Random generator and probability distributions */
    mt19937 generator(p.seed);
    uniform_real_distribution<double> unif(0.0, 1.0);
    normal_distribution<double> norm(0.0, 1.0);

    nVar = LB.size();
    nIntVar = p.IntVar.size();

    /* Create boundaries for all agents*/
    LBe = LB.replicate(p.nPop, 1);
    UBe = UB.replicate(p.nPop, 1);

    /* Normalize search space */
    if (p.normalize) {
        LBe_orig = LBe;
        UBe_orig = UBe;
        LBe.setZero(p.nPop, nVar);
        UBe.setOnes(p.nPop, nVar);
    }

    T = p.T0;                           // Temperature
    sigma = p.sigma0 * (UBe - LBe);     // Standard deviation of each dimension

    /* Initial position of each agent */
    rn.setZero(p.nPop, nVar);
    for (int i=0; i<p.nPop; i++) {
        for (int j=0; j<nVar; j++) {
            rn(i, j) = unif(generator);
        }
    }
    agent_pos = LBe + rn * (UBe - LBe);

    /* Correct for any integer variable */
    for (int j=0; j<nIntVar; j++) {
        idx = p.IntVar(j);
        agent_pos.col(idx) = round(agent_pos.col(idx));
    }

    /* Initial cost of each agent */
    if (p.normalize) {
        agent_pos_orig = LBe_orig + agent_pos * (UBe_orig - LBe_orig);
        agent_cost = func(agent_pos_orig, p.args);
    }
    else {
        agent_cost = func(agent_pos, p.args);
    }

    /* Initial (overall) best position/cost */
    best_cost = agent_cost.minCoeff(&r_min, &c_min);
    best_pos = agent_pos.row(r_min);

    /* Main loop (T = const) */
    neigh_pos.setZero(p.nPop, nVar);
    F.setZero(p.epochs);
    for (int epoch=0; epoch<p.epochs; epoch++) {

        /* Sub-loop (search the neighboroud of a state) */
        for (int move=0; move<p.nMove; move++) {

            /* Randomly decide in which dimension to search */
            for (int i=0; i<p.nPop; i++) {
                for (int j=0; j<nVar; j++) {
                    rn(i, j) = unif(generator);
                }
            }
            flips = (rn <= p.prob).cast<double>();

            /* Create each agent's neighbours */
            for (int i=0; i<p.nPop; i++) {
                for (int j=0; j<nVar; j++) {
                    rn(i, j) = norm(generator);
                }
            }
            neigh_pos = agent_pos + flips * rn * sigma;

            /* Correct for any integer variable */
            for (int j=0; j<nIntVar; j++) {
                idx = p.IntVar(j);
                neigh_pos.col(idx) = round(neigh_pos.col(idx));
            }

            /* Impose position boundaries */
            neigh_pos = neigh_pos.max(LBe);
            neigh_pos = neigh_pos.min(UBe);

            for (int j=0; j<nIntVar; j++) {
                idx = p.IntVar(j);
                neigh_pos.col(idx) = neigh_pos.col(idx).max(ceil(LBe.col(idx)));
                neigh_pos.col(idx) = neigh_pos.col(idx).min(floor(UBe.col(idx)));
            }

            /* Evaluate the cost of each agent's neighbour */
            if (p.normalize) {
                neigh_pos_orig = LBe_orig + neigh_pos * (UBe_orig - LBe_orig);
                neigh_cost = func(neigh_pos_orig, p.args);
            }
            else {
                neigh_cost = func(neigh_pos, p.args);
            }

            /* Decide if each agent will change its state */
            for (int i=0; i<p.nPop; i++) {

                /* Swap states if the neighbour state is better ... */
                if (neigh_cost(i) <= agent_cost(i)) {
                    agent_cost(i) = neigh_cost(i);
                    agent_pos.row(i) = neigh_pos.row(i);
                }

                /* ... or decide probabilistically */
                else {

                    /* Acceptance probability */
                    delta = (neigh_cost(i) - agent_cost(i)) / agent_cost(i);
                    prob_swap = exp(-delta / T);

                    /* Randomly swap states */
                    if (unif(generator) <= prob_swap) {
                        agent_cost(i) = neigh_cost(i);
                        agent_pos.row(i) = neigh_pos.row(i);
                    }
                }

                /* Update the (overall) best position/cost */
                best_cost = agent_cost.minCoeff(&r_min, &c_min);
                best_pos = agent_pos.row(r_min);
            }
        }

        /* Save the best cost for this epoch */
        F(epoch) = best_cost;

        /* Cooling scheduling */
        T = p.alphaT * T;

        /* Random neighboroud search schedule */
        sigma = p.alphaS * sigma;
    }

    /* De-normalize */
    if (p.normalize) {
        best_pos = LB + best_pos * (UB - LB);
        sigma = sigma * (UBe_orig - LBe_orig);
    }

    /* Copy solution */
    res.best_cost = best_cost;
    res.T = T;
    res.best_pos = best_pos;
    res.sigma = sigma.row(0);
    res.F = F;

    return res;
}


#include <stdio.h>
#include <iostream>
#include <math.h>
#include <string>
#include <Eigen/Dense>
#include "ros/ros.h"

#include <yaml-cpp/yaml.h>
#include <ros/package.h>

#include <std_msgs/Float32MultiArray.h>
#include <sensor_msgs/JointState.h>
#include <robot_localization_msgs/HeadTransform.h>

using namespace ros;
using namespace std;
using namespace Eigen;

float angle_torso_pitch=0.0;
float angle_torso_yaw=0.0;
float angle_head_yaw=0.0;
float angle_head_pitch=0.0;

void jointCallback(const sensor_msgs::JointState::ConstPtr& msg);

bool RobotYamlRead(string path, float* length_pelvis2torsopitch, float* length_torsopitch2torsoyaw, float* length_torsoyaw2headyaw, float* length_headyaw2headpitch, float* length_headpitch2camera);
bool ParticleYamlRead(string path, float& unit);

Matrix4f makeYawMatrix(float& theta);
Matrix4f makePitchMatrix(float& theta);
Matrix4f makeTransMatrix(Vector3f& position);
Matrix4f makeTransMatrix(Vector3f& position, float& unit);

void printMatrixValue(Matrix4f& matrix);

void insertMatrix2Message(Matrix4f& matrix, std_msgs::Float32MultiArray& msg);
void insertMatrix2Message(Matrix4f& matrix, robot_localization_msgs::HeadTransform& msg, int flag); // flag=0 -> original, flag=1 -> unit

int main(int argc, char** argv)
{
    //read setting
    string yaml_robot_path = package::getPath("robot_localization_data") + "/data/kin_dyn_3.yaml";
    string yaml_particle_path = package::getPath("robot_localization_data") + "/data/particle_setting.yaml";

    float length_pelvis2torsopitch[3];
    float length_torsopitch2torsoyaw[3];

    float length_torsoyaw2headyaw[3];
    float length_headyaw2headpitch[3];
    float length_headpitch2camera[3];

    float unit;

    if( !RobotYamlRead(yaml_robot_path, length_pelvis2torsopitch, length_torsopitch2torsoyaw, length_torsoyaw2headyaw, length_headyaw2headpitch, length_headpitch2camera) ) exit(0);
    if( !ParticleYamlRead(yaml_particle_path, unit) ) exit(0);

    Vector3f V_length_pelvis2torsopitch(length_pelvis2torsopitch[0], length_pelvis2torsopitch[1], length_pelvis2torsopitch[2]);
    Vector3f V_length_torsopitch2torsoyaw(length_torsopitch2torsoyaw[0], length_torsopitch2torsoyaw[1], length_torsopitch2torsoyaw[2]);
    Vector3f V_length_torsoyaw2headyaw(length_torsoyaw2headyaw[0],length_torsoyaw2headyaw[1],length_torsoyaw2headyaw[2]);
    Vector3f V_length_headyaw2headpitch(length_headyaw2headpitch[0],length_headyaw2headpitch[1],length_headyaw2headpitch[2]);
    Vector3f V_length_headpitch2camera(length_headpitch2camera[0], length_headpitch2camera[1], length_headpitch2camera[2]);

    //start ros
    ros::init(argc, argv, "robot_localization_head_receive3");
    ros::NodeHandle n;
    ros::Rate loop_rate(100);

    ros::Publisher headtransform_pub = n.advertise<std_msgs::Float32MultiArray>("/alice/camera_transform",10);
    ros::Publisher custom_headtransform_pub = n.advertise<robot_localization_msgs::HeadTransform>("/robot_localization/head_transform",10);
    ros::Subscriber joint_sub = n.subscribe("/robotis/present_joint_states",10,jointCallback);

    while(ros::ok())
    {
        std_msgs::Float32MultiArray headtransform_msg;
        robot_localization_msgs::HeadTransform custom_headtransform_msg;

        //real world
        Matrix4f t_pelvis2torsopitch = makeTransMatrix(V_length_pelvis2torsopitch);
        Matrix4f r_pelvis2torsopitch = Matrix4f::Identity();

        Matrix4f t_torsopitch2torsoyaw = makeTransMatrix(V_length_torsopitch2torsoyaw);
        Matrix4f r_torsopitch2torsoyaw = makePitchMatrix(angle_torso_pitch);

        Matrix4f t_torsoyaw2headyaw = makeTransMatrix(V_length_torsoyaw2headyaw);
        Matrix4f r_torsoyaw2headyaw = makeYawMatrix(angle_torso_yaw);

        Matrix4f t_headyaw2headpitch = makeTransMatrix(V_length_headyaw2headpitch);
        Matrix4f r_headyaw2headpitch = makeYawMatrix(angle_head_yaw);

        Matrix4f t_headpitch2camera = makeTransMatrix(V_length_headpitch2camera);
        Matrix4f r_headpitch2camera = makePitchMatrix(angle_head_pitch);

        Matrix4f TR01 = r_pelvis2torsopitch * t_pelvis2torsopitch;
        Matrix4f TR12 = r_torsopitch2torsoyaw * t_torsopitch2torsoyaw;
        Matrix4f TR23 = r_torsoyaw2headyaw * t_torsoyaw2headyaw;
        Matrix4f TR34 = r_headyaw2headpitch * t_headyaw2headpitch;
        Matrix4f TR45 = r_headpitch2camera * t_headpitch2camera;

        Matrix4f Transform_pelvis2head = TR01 * TR12 * TR23 * TR34 * TR45;  // Transform matrix real world = TR

        //virtual world
        Matrix4f t_pelvis2torsopitch_V = makeTransMatrix(V_length_pelvis2torsopitch, unit);
        Matrix4f t_torsopitch2torsoyaw_V = makeTransMatrix(V_length_torsopitch2torsoyaw, unit);
        Matrix4f t_torsoyaw2headyaw_V = makeTransMatrix(V_length_torsoyaw2headyaw, unit);
        Matrix4f t_headyaw2headpitch_V = makeTransMatrix(V_length_headyaw2headpitch, unit);
        Matrix4f t_headpitch2camera_V = makeTransMatrix(V_length_headpitch2camera, unit);

        Matrix4f TV01 = r_pelvis2torsopitch * t_pelvis2torsopitch_V;
        Matrix4f TV12 = r_torsopitch2torsoyaw * t_torsopitch2torsoyaw_V;
        Matrix4f TV23 = r_torsoyaw2headyaw * t_torsoyaw2headyaw_V;
        Matrix4f TV34 = r_headyaw2headpitch * t_headyaw2headpitch_V;
        Matrix4f TV45 = r_headpitch2camera * t_headpitch2camera_V;

        Matrix4f unit_Transform_pelvis2head = TV01 * TV12 * TV23 * TV34 * TV45;   // Transform matrix virtual world = TV

        insertMatrix2Message(Transform_pelvis2head, headtransform_msg);

        insertMatrix2Message(Transform_pelvis2head, custom_headtransform_msg, 0);
        insertMatrix2Message(unit_Transform_pelvis2head, custom_headtransform_msg, 1);

        headtransform_pub.publish(headtransform_msg);
        custom_headtransform_pub.publish(custom_headtransform_msg);

        cout << "-----------------------------------------------" << endl;
        cout << "waist_pitch : " << angle_torso_pitch * 180 / M_PI << endl;
        cout << "waist_yaw : " << angle_torso_yaw * 180 / M_PI << endl;
        cout << "head_yaw : " << angle_head_yaw * 180 / M_PI << endl;
        cout << "head_pitch : " << angle_head_pitch * 180 / M_PI << endl;
        cout << "-----------------------------------------------" << endl;
        printMatrixValue(Transform_pelvis2head);
        cout << "-----------------------------------------------" << endl << endl;

        ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;
}

void jointCallback(const sensor_msgs::JointState::ConstPtr& msg)
{
    int torso_pitch_index, torso_yaw_index, head_yaw_index, head_pitch_index;
    for(int i=0; i<(msg->name).size(); i++)
    {
        if(msg->name[i] == "waist_pitch") torso_pitch_index = i;
        if(msg->name[i] == "waist_yaw"  ) torso_yaw_index = i;
        if(msg->name[i] == "head_yaw"   ) head_yaw_index = i;
        if(msg->name[i] == "head_pitch" ) head_pitch_index = i;
    }

    angle_torso_pitch = msg->position[torso_pitch_index];
    angle_torso_yaw = msg->position[torso_yaw_index];
    angle_head_yaw = msg->position[head_yaw_index];
    angle_head_pitch = msg->position[head_pitch_index];
}

bool RobotYamlRead(string path, float* length_pelvis2torsopitch, float* length_torsopitch2torsoyaw, float* length_torsoyaw2headyaw, float* length_headyaw2headpitch, float* length_headpitch2camera)
{
    YAML::Node yaml_node;
    try
    {
        yaml_node = YAML::LoadFile(path.c_str());

        length_pelvis2torsopitch[0]   = yaml_node["torso_p"]["relative_position"][0].as<float>();
        length_pelvis2torsopitch[1]   = yaml_node["torso_p"]["relative_position"][1].as<float>();
        length_pelvis2torsopitch[2]   = yaml_node["torso_p"]["relative_position"][2].as<float>();

        length_torsopitch2torsoyaw[0] = yaml_node["torso_y"]["relative_position"][0].as<float>();
        length_torsopitch2torsoyaw[1] = yaml_node["torso_y"]["relative_position"][1].as<float>();
        length_torsopitch2torsoyaw[2] = yaml_node["torso_y"]["relative_position"][2].as<float>();

        length_torsoyaw2headyaw[0]    = yaml_node["head_y"]["relative_position"][0].as<float>();
        length_torsoyaw2headyaw[1]    = yaml_node["head_y"]["relative_position"][1].as<float>();
        length_torsoyaw2headyaw[2]    = yaml_node["head_y"]["relative_position"][2].as<float>();

        length_headyaw2headpitch[0]  = yaml_node["head_p"]["relative_position"][0].as<float>();
        length_headyaw2headpitch[1]  = yaml_node["head_p"]["relative_position"][1].as<float>();
        length_headyaw2headpitch[2]  = yaml_node["head_p"]["relative_position"][2].as<float>();

        length_headpitch2camera[0]   = yaml_node["cam"]["relative_position"][0].as<float>();
        length_headpitch2camera[1]   = yaml_node["cam"]["relative_position"][1].as<float>();
        length_headpitch2camera[2]   = yaml_node["cam"]["relative_position"][2].as<float>();
    }
    catch(const exception& e)
    {
        ROS_ERROR("fail to read robot yaml file");
        return false;
    }
    return true;
}

bool ParticleYamlRead(string path, float& unit)
{
    YAML::Node yaml_node;
    try
    {
        yaml_node = YAML::LoadFile(path.c_str());
        unit = yaml_node["ETCSetting"]["unit"].as<float>();
    }
    catch(const exception& e)
    {
        ROS_ERROR("fail to read particle yaml file");
        return false;
    }
    return true;
}

void insertMatrix2Message(Matrix4f& matrix, std_msgs::Float32MultiArray& msg)
{
    for(int i=0; i<4; i++)
        for(int j=0; j<4; j++)
            msg.data.push_back(matrix(i,j));
}

void printMatrixValue(Matrix4f& matrix){
    vector<float> data;
    for(int i=0; i<4; i++){
        for(int j=0; j<4; j++){
            data.push_back(matrix(i,j));
            printf("%3.3f  ", data.back());
        }
        cout << endl;
    }
}

void insertMatrix2Message(Matrix4f& matrix, robot_localization_msgs::HeadTransform& msg, int flag)
{
    switch(flag)
    {
        case 0:
            for(int i=0; i<4; i++)
                for(int j=0; j<4; j++)
                    msg.Tdata.push_back(matrix(i,j));
            break;
        case 1:
            for(int i=0; i<4; i++)
                for(int j=0; j<4; j++)
                    msg.Udata.push_back(matrix(i,j));
            break;
        default:
            break;
    }
}

Matrix4f makeYawMatrix(float& theta)
{
    Matrix4f matrix;
    matrix(0,0)=cos(theta); matrix(0,1)=-sin(theta); matrix(0,2)=0; matrix(0,3)=0;
    matrix(1,0)=sin(theta); matrix(1,1)= cos(theta); matrix(1,2)=0; matrix(1,3)=0;
    matrix(2,0)=0;          matrix(2,1)=0;           matrix(2,2)=1; matrix(2,3)=0;
    matrix(3,0)=0;          matrix(3,1)=0;           matrix(3,2)=0; matrix(3,3)=1;

    return matrix;
}

Matrix4f makePitchMatrix(float& theta)
{
    Matrix4f matrix;
    matrix(0,0)= cos(theta); matrix(0,1)=0; matrix(0,2)=sin(theta); matrix(0,3)=0;
    matrix(1,0)=0;           matrix(1,1)=1; matrix(1,2)=0;          matrix(1,3)=0;
    matrix(2,0)=-sin(theta); matrix(2,1)=0; matrix(2,2)=cos(theta); matrix(2,3)=0;
    matrix(3,0)=0;           matrix(3,1)=0; matrix(3,2)=0;          matrix(3,3)=1;

    return matrix;
}

Matrix4f makeTransMatrix(Vector3f& position)
{
    Matrix4f matrix;
    matrix(0,0)=1; matrix(0,1)=0; matrix(0,2)=0; matrix(0,3)=position(0);
    matrix(1,0)=0; matrix(1,1)=1; matrix(1,2)=0; matrix(1,3)=position(1);
    matrix(2,0)=0; matrix(2,1)=0; matrix(2,2)=1; matrix(2,3)=position(2);
    matrix(3,0)=0; matrix(3,1)=0; matrix(3,2)=0; matrix(3,3)=1;

    return matrix;
}

Matrix4f makeTransMatrix(Vector3f& position, float& unit)
{
    Matrix4f matrix;
    matrix(0,0)=1; matrix(0,1)=0; matrix(0,2)=0; matrix(0,3)=position(0)*unit;
    matrix(1,0)=0; matrix(1,1)=1; matrix(1,2)=0; matrix(1,3)=position(1)*unit;
    matrix(2,0)=0; matrix(2,1)=0; matrix(2,2)=1; matrix(2,3)=position(2)*unit;
    matrix(3,0)=0; matrix(3,1)=0; matrix(3,2)=0; matrix(3,3)=1;

    return matrix;
}


// Copyright (c) 2018 by University Paris-Est Marne-la-Vallee
// InnerExplicit.hpp
// This file is part of the Garamon for e3ga.
// Authors: Stephane Breuils and Vincent Nozick
// Contact: vincent.nozick@u-pem.fr
//
// Licence MIT
// A a copy of the MIT License is given along with this program

/// \file InnerExplicit.hpp
/// \author Stephane Breuils, Vincent Nozick
/// \brief Explicit precomputed per grades inner products of e3ga.

#ifndef E3GA_INNER_PRODUCT_EXPLICIT_HPP__
#define E3GA_INNER_PRODUCT_EXPLICIT_HPP__
#pragma once

#include <Eigen/Core>

#include "e3ga/Mvec.hpp"
#include "e3ga/Inner.hpp"
#include "e3ga/Constants.hpp"

/*!
 * @namespace e3ga
 */
namespace e3ga {
    template<typename T> class Mvec;

    /// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 0) and mv2 (grade 0).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 0 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 0 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 0
	template<typename T>
	void inner_0_0(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3.coeffRef(0) +=  mv1.coeff(0)*mv2.coeff(0);
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 0) and mv2 (grade 1).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 0 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 1 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 1
	template<typename T>
	void inner_0_1(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3 += mv1.coeff(0)*mv2;
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 0) and mv2 (grade 2).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 0 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 2 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 2
	template<typename T>
	void inner_0_2(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3 += mv1.coeff(0)*mv2;
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 0) and mv2 (grade 3).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 0 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 3 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 3
	template<typename T>
	void inner_0_3(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3 += mv1.coeff(0)*mv2;
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 1) and mv2 (grade 0).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 1 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 0 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 1
	template<typename T>
	void inner_1_0(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3 += mv1*mv2.coeff(0);
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 1) and mv2 (grade 1).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 1 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 1 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 0
	template<typename T>
	void inner_1_1(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3.coeffRef(0) +=  mv1.coeff(0)*mv2.coeff(0) + mv1.coeff(1)*mv2.coeff(1) + mv1.coeff(2)*mv2.coeff(2);
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 1) and mv2 (grade 2).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 1 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 2 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 1
	template<typename T>
	void inner_1_2(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3.coeffRef(0) += -mv1.coeff(1)*mv2.coeff(0) - mv1.coeff(2)*mv2.coeff(1);
		mv3.coeffRef(1) +=  mv1.coeff(0)*mv2.coeff(0) - mv1.coeff(2)*mv2.coeff(2);
		mv3.coeffRef(2) +=  mv1.coeff(0)*mv2.coeff(1) + mv1.coeff(1)*mv2.coeff(2);
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 1) and mv2 (grade 3).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 1 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 3 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 2
	template<typename T>
	void inner_1_3(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3.coeffRef(0) +=  mv1.coeff(2)*mv2.coeff(0);
		mv3.coeffRef(1) += -mv1.coeff(1)*mv2.coeff(0);
		mv3.coeffRef(2) +=  mv1.coeff(0)*mv2.coeff(0);
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 2) and mv2 (grade 0).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 2 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 0 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 2
	template<typename T>
	void inner_2_0(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3 += mv1*mv2.coeff(0);
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 2) and mv2 (grade 1).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 2 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 1 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 1
	template<typename T>
	void inner_2_1(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3.coeffRef(0) +=  mv1.coeff(0)*mv2.coeff(1) + mv1.coeff(1)*mv2.coeff(2);
		mv3.coeffRef(1) += -mv1.coeff(0)*mv2.coeff(0) + mv1.coeff(2)*mv2.coeff(2);
		mv3.coeffRef(2) += -mv1.coeff(1)*mv2.coeff(0) - mv1.coeff(2)*mv2.coeff(1);
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 2) and mv2 (grade 2).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 2 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 2 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 0
	template<typename T>
	void inner_2_2(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3.coeffRef(0) += -mv1.coeff(0)*mv2.coeff(0) - mv1.coeff(1)*mv2.coeff(1) - mv1.coeff(2)*mv2.coeff(2);
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 2) and mv2 (grade 3).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 2 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 3 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 1
	template<typename T>
	void inner_2_3(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3.coeffRef(0) += -mv1.coeff(2)*mv2.coeff(0);
		mv3.coeffRef(1) +=  mv1.coeff(1)*mv2.coeff(0);
		mv3.coeffRef(2) += -mv1.coeff(0)*mv2.coeff(0);
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 3) and mv2 (grade 0).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 3 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 0 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 3
	template<typename T>
	void inner_3_0(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3 += mv1*mv2.coeff(0);
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 3) and mv2 (grade 1).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 3 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 1 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 2
	template<typename T>
	void inner_3_1(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3.coeffRef(0) +=  mv1.coeff(0)*mv2.coeff(2);
		mv3.coeffRef(1) += -mv1.coeff(0)*mv2.coeff(1);
		mv3.coeffRef(2) +=  mv1.coeff(0)*mv2.coeff(0);
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 3) and mv2 (grade 2).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 3 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 2 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 1
	template<typename T>
	void inner_3_2(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3.coeffRef(0) += -mv1.coeff(0)*mv2.coeff(2);
		mv3.coeffRef(1) +=  mv1.coeff(0)*mv2.coeff(1);
		mv3.coeffRef(2) += -mv1.coeff(0)*mv2.coeff(0);
	}

	/// \brief Compute the inner product between two homogeneous multivectors mv1 (grade 3) and mv2 (grade 3).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 3 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 3 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1.mv2, which is also a homogeneous multivector of grade 0
	template<typename T>
	void inner_3_3(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3.coeffRef(0) += -mv1.coeff(0)*mv2.coeff(0);
	}

    template<typename T>
	std::array<std::array<std::function<void(const Eigen::Matrix<T, Eigen::Dynamic, 1> & , const Eigen::Matrix<T, Eigen::Dynamic, 1> & , Eigen::Matrix<T, Eigen::Dynamic, 1>&)>, 4>, 4> innerFunctionsContainer = {{
		{{inner_0_0<T>,inner_0_1<T>,inner_0_2<T>,inner_0_3<T>}},
		{{inner_1_0<T>,inner_1_1<T>,inner_1_2<T>,inner_1_3<T>}},
		{{inner_2_0<T>,inner_2_1<T>,inner_2_2<T>,inner_2_3<T>}},
		{{inner_3_0<T>,inner_3_1<T>,inner_3_2<T>,inner_3_3<T>}}
	}};

}/// End of Namespace

#endif // E3GA_INNER_PRODUCT_EXPLICIT_HPP__


#pragma once

#include <string>
#include <Eigen/Geometry>

#include <common_robotics_utilities/math.hpp>

namespace common_robotics_utilities
{
namespace simple_robot_model_interface
{
/// This is basically the absolute minimal robot model interface needed to do
/// motion planning and forward kinematics. It does not include link geometry.
template<typename Configuration,
         typename ConfigAlloc=std::allocator<Configuration>>
class SimpleRobotModelInterface
{
public:
  virtual ~SimpleRobotModelInterface() {}

  /// Clone the current robot model.
  virtual SimpleRobotModelInterface<Configuration,
                                    ConfigAlloc>* Clone() const = 0;

  /// Return the current position (i.e. joint values).
  virtual const Configuration& GetPosition() const = 0;

  /// Set a new position (i.e. joint values) and return the new value.
  virtual const Configuration& SetPosition(const Configuration& config) = 0;

  /// Get names of links in the robot.
  virtual std::vector<std::string> GetLinkNames() const = 0;

  /// Get the transform of the link with index @param link_index relative to
  /// world.
  virtual Eigen::Isometry3d GetLinkTransform(
      const int64_t link_index) const = 0;

  /// Get the transform of the link with name @param link_name relative to
  /// world.
  virtual Eigen::Isometry3d GetLinkTransform(
      const std::string& link_name) const = 0;

  /// Get the transforms of all links in the robot relative to world in the same
  /// order as returned by GetLinkNames().
  virtual math::VectorIsometry3d GetLinkTransforms() const = 0;

  /// Return a map of <name, transform> for all links in the robot relative to
  /// the world.
  virtual math::MapStringIsometry3d GetLinkTransformsMap() const = 0;

  /// Return C-space distance between @param config1 and @param config2.
  virtual double ComputeConfigurationDistance(
      const Configuration& config1, const Configuration& config2) const = 0;

  /// Return C-space absolute-value distance between @param config1 and @param
  /// config2 for each dimension of the C-space separately.
  Eigen::VectorXd ComputePerDimensionConfigurationDistance(
      const Configuration& config1, const Configuration& config2) const
  {
    return ComputePerDimensionConfigurationSignedDistance(
        config1, config2).cwiseAbs();
  }

  /// Return C-space signed distance between @param config1 and @param config2
  /// for each dimension of the C-space separately.
  virtual Eigen::VectorXd ComputePerDimensionConfigurationSignedDistance(
      const Configuration& config1, const Configuration& config2) const = 0;

  /// Return C-space distance between the current configuration and @param
  /// config.
  double ComputeConfigurationDistanceTo(const Configuration& config) const
  {
    return ComputeConfigurationDistance(GetPosition(), config);
  }

  /// Return C-space abolsute-value distance between the current configuration
  /// and @param config for each dimension of the C-space separately.
  Eigen::VectorXd ComputePerDimensionConfigurationDistanceTo(
      const Configuration& config) const
  {
    return ComputePerDimensionConfigurationDistance(GetPosition(), config);
  }

  /// Return C-space signed distance between the current configuration and
  /// @param config for each dimension of the C-space separately.
  Eigen::VectorXd ComputePerDimensionConfigurationSignedDistanceTo(
      const Configuration& config) const
  {
    return ComputePerDimensionConfigurationSignedDistance(
        GetPosition(), config);
  }

  /// Interpolate a configuration of the robot between @param start and @param
  /// end for the provided ratio @param ratio.
  virtual Configuration InterpolateBetweenConfigurations(
      const Configuration& start, const Configuration& end,
      const double ratio) const = 0;

  /// Average the provided set of configurations @param configurations.
  virtual Configuration AverageConfigurations(
      const std::vector<Configuration, ConfigAlloc>& configurations) const = 0;

  /// Compute the translation-only part of the Jacobian at the provided point
  /// @param link_relative_point on link @param link_name in the robot.
  virtual Eigen::Matrix<double, 3, Eigen::Dynamic>
  ComputeLinkPointTranslationJacobian(
      const std::string& link_name,
      const Eigen::Vector4d& link_relative_point) const = 0;

  /// Compute the Jacobian at the provided point @param link_relative_point on
  /// link @param link_name in the robot.
  virtual Eigen::Matrix<double, 6, Eigen::Dynamic>
  ComputeLinkPointJacobian(
      const std::string& link_name,
      const Eigen::Vector4d& link_relative_point) const = 0;
};
}  // namespace simple_robot_model_interface
}  // namespace common_robotics_utilities


#ifndef CAMERA_HPP
#define CAMERA_HPP

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

namespace threedimutil
{
    class Camera
    {
    public:
        Camera();

        enum class Mode
        {
            Rotate,
            Pan,
            Zoom,
            None
        };

        double& fov() { return m_fov; }
        double fov() const { return m_fov; }

        Eigen::Vector3d& position() { return m_position; }
        const Eigen::Vector3d& position() const { return m_position; }
        Eigen::Vector3d& target() { return m_target; }
        const Eigen::Vector3d& target() const { return m_target; }
        Eigen::Vector3d& up() { return m_up; }
        const Eigen::Vector3d& up() const { return m_up; }

        // Method for obtaining matrices
        Eigen::Matrix4d GetLookAtMatrix() const;

        // Method for animated visualization
        void RotateAroundTarget(double theta_in_radian);

        // Methods for cursor interaction
        void BeginTrackball(int x, int y, Mode mode);
        void MoveTrackball(int x, int y);
        void EndTrackball();

    private:
        double m_fov = M_PI * 45.0 / 180.0;

        Eigen::Vector3d m_position;
        Eigen::Vector3d m_target;
        Eigen::Vector3d m_up;

        Mode m_mode;
        Eigen::Vector2i m_prev_position;
    };
}

#endif // CAMERA_HPP


#pragma once
#include <Eigen/Dense>

namespace filter_bay
{
/*!
The movement state consists of a pose and a velocity.
The state is stored as a Eigen::Matrix so it can be used directly int the
systems model.
The pose is stored in the first half of the state, the velocity is stored in
the second half.
*/
template <size_t DOF>
struct MovementState
{
  using Pose = Eigen::Matrix<double, DOF, 1>;
  using Velocity = Eigen::Matrix<double, DOF, 1>;
  using State = Eigen::Matrix<double, 2 * DOF, 1>;

  State state;

  Pose get_pose() const
  {
    return state.block<DOF, 1>(0, 0);
  }

  Velocity get_velocity() const
  {
    return state.block<DOF, 1>(DOF, 0);
  }

  void set_pose(Pose pose)
  {
    state.block<DOF, 1>(0, 0) = std::move(pose);
  }

  void set_velocity(Velocity velocity)
  {
    state.block<DOF, 1>(DOF, 0) = std::move(velocity);
  }
};
} // namespace filter_bay


#pragma once

#include <Eigen/Dense>

namespace e = Eigen;

typedef e::Matrix<double, 6, 1> Vec6;
typedef e::Matrix<double, 6, 6> Mat6;
typedef e::Matrix<double, 3, 1> Vec3;
typedef e::Matrix<double, 3, 3> Mat3;
typedef e::Matrix<double, 2, 1> Vec2;
typedef e::Matrix<double, 2, 2> Mat2;


#include <boost/math/complex/acosh.hpp>


#ifndef nomad__src__matrix__functions__dot_hpp
#define nomad__src__matrix__functions__dot_hpp

#include <Eigen/Core>

#include <src/var/var.hpp>
#include <src/var/derived/dot_var_node.hpp>

namespace nomad {

  template<typename DerivedA, typename DerivedB>
  inline typename
  std::enable_if<
    is_var<typename Eigen::MatrixBase<DerivedA>::Scalar>::value &&
    is_var<typename Eigen::MatrixBase<DerivedB>::Scalar>::value &&
    std::is_same<typename Eigen::MatrixBase<DerivedA>::Scalar,
                 typename Eigen::MatrixBase<DerivedB>::Scalar>::value,
    typename Eigen::MatrixBase<DerivedA>::Scalar >::type
  dot(const Eigen::MatrixBase<DerivedA>& v1,
      const Eigen::MatrixBase<DerivedB>& v2) {

    const short autodiff_order = Eigen::MatrixBase<DerivedA>::Scalar::order();
    const bool strict_smoothness = Eigen::MatrixBase<DerivedA>::Scalar::strict();
    const bool validate_io = Eigen::MatrixBase<DerivedA>::Scalar::validate();

    eigen_idx_t N = v1.size();
    const nomad_idx_t n_inputs = static_cast<nomad_idx_t>(2 * N);

    create_node<dot_var_node<autodiff_order>>(n_inputs);

    double sum = 0;

    for (eigen_idx_t n = 0; n < N; ++n)
      sum += v1(n).first_val() * v2(n).first_val();

    push_dual_numbers<autodiff_order>(sum);

    for (eigen_idx_t n = 0; n < N; ++n)
      push_inputs(v1(n).dual_numbers());

    for (eigen_idx_t n = 0; n < N; ++n)
      push_inputs(v2(n).dual_numbers());

    return var<autodiff_order, strict_smoothness, validate_io>(next_node_idx_ - 1);

  }

}

#endif


// Std includes
#include <cmath>
#include <iostream>
#include <vector>
// Thirdparties includes
#include <Eigen/Dense>
// Lib includes
#include "fl0w/jhtdb/channel_flow.h"

const unsigned int DIM = 3;

using TypeScalar = double;
using TypeVector = Eigen::Matrix<TypeScalar, DIM, 1>;
using TypeMatrix = Eigen::Matrix<TypeScalar, DIM, DIM>;
template<typename... Args>
using TypeRef = Eigen::Ref<Args...>;
using TypeFlow = fl0w::jhtdb::ChannelFlow<TypeVector, TypeMatrix, TypeRef>;

void print(const TypeFlow& flow, const TypeVector& x, const TypeScalar& t) {
    std::cout << std::endl;
    std::cout << "flow.getVelocity(" << x.transpose() << ", " << t << ") -> " << flow.getVelocity(x, t).transpose() << std::endl;
    std::cout << "flow.getJacobian(" << x.transpose() << ", " << t << "):\n -> " << flow.getJacobian(x, t) << std::endl;
    std::cout << std::endl;
}

int main () {
    TypeFlow flow;
    TypeVector x;
    double t;
    // Init
    x << 1.0, 0.5, 1.0;
    t = 1.0;
    print(flow, x, t);
}


e#include <iostream>
#include <vector>
#include <tuple>
#include <map>
#include <algorithm>
#include <cmath>
#include <boost/multiprecision/cpp_int.hpp>

namespace mp = boost::multiprecision;
using namespace std;
// 使えるマッチ棒の数字 -> 作るのに必要な本数
map<int, int> costs;

// n: 残り本数 -> ret: (最大の本数, 使った数字履歴)
map<int, int> calc_order_memo{ {0, 1} }; // n->桁数: 0本で使い切ったときは終了条件なので1本としておく
map<int, mp::cpp_int> calc_num_memo{ {0, 0} }; // n->使った数字をメモしておきたい

tuple<int, mp::cpp_int> calc_order(int n) {
    if(calc_order_memo.count(n)) {
        return { calc_order_memo[n], calc_num_memo[n] };
    } else {
        // 本数を使い切らずに一番コストの低い数字を選択する
        int max_result = -1; // 帰ってきた桁数で最大の藻を
        int select_n = -1; // max_result時にこのCall上で選んだ数字
        mp::cpp_int max_num = -1; // 帰ってきた関数の具体的な数値

        for(const auto& c: costs) {
            // cout << "[DEBUG] n=" << n << ", c=(" << c.first << ", " << c.second << ")" << endl;
            // つくれないので失敗
            if (c.second > n) {
                continue;
            }
            // 作れるので再帰する
            auto t = calc_order(n - c.second);
            auto result = get<0>(t);
            auto num = get<1>(t);

            if(result > max_result) {
                max_result = result;
                select_n = c.first;
                max_num = num;
            }
        }
        if (max_result == -1) {
            return { -1, 0 }; // つくれない
        } else {
            calc_order_memo[n] = max_result + 1; // 桁数
            calc_num_memo[n] = (max_num * 10) + select_n; // 使った数字+これまでの桁を足しておく
            // cout << "[DEBUG] ret {" << calc_order_memo[n] << ", " << calc_num_memo[n] << "}" << endl;

            return { calc_order_memo[n], calc_num_memo[n] }; // 桁数を一個足して返す
        }
    }
}
// 各桁の数字を並び替えて大きくする
void sort_order(int order, mp::cpp_int src, std::string& str){
    vector<int> arr(order);
    str = src.str();
    sort(str.begin(), str.end(), greater<char>());
}

int main(void) {
    map<int, int> costs_src;
    costs_src[1] = 2;
    costs_src[2] = 5;
    costs_src[3] = 5;
    costs_src[4] = 4;
    costs_src[5] = 5;
    costs_src[6] = 6;
    costs_src[7] = 3;
    costs_src[8] = 7;
    costs_src[9] = 6;

    int n, m;
    cin >> n; // マッチ本数
    cin >> m; // 使える数字の種類
    for (int i = 0; i < m; ++i) {
        int an;
        cin >> an;
        costs[an] = costs_src[an];
    }
    // 先に同じコストの桁数であれば大きい方に間引いてしまう
    for(int i = 1 ; i < 8 ; ++i) {
        int x = -1;
        for(int j = costs.size() - 1 ; j >= 0 ; --j) {
            if(!costs.count(j)) continue;
            else if(costs[j] == i) {
                if (x != -1) {
                    // cout << "[DEBUG] remove costs:" << j << endl;
                    costs.erase(j);
                } else {
                    x = j;
                }
            }
        }
    }
    // for (const auto &c : costs) {
    //     cout << c.first << ", " << c.second << endl;
    // }
    // 作れる最大の桁数を求める
    auto r = calc_order(n);
    auto order = get<0>(r);
    auto num = get<1>(r);
    // cout << "[DEBUG] order: " << order << ", num:" << num << endl;

    string dst;
    sort_order(order, num, dst);
    cout << dst << endl;
    return 0;
}


// Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>

#ifndef KDL_CHAIN_IKSOLVERVEL_PINV_GIVENS_HPP
#define KDL_CHAIN_IKSOLVERVEL_PINV_GIVENS_HPP

#include "chainiksolver.hpp"
#include "chainjnttojacsolver.hpp"

#include <Eigen/Core>

using namespace Eigen;

namespace KDL
{
    /**
     * Implementation of a inverse velocity kinematics algorithm based
     * on the generalize pseudo inverse to calculate the velocity
     * transformation from Cartesian to joint space of a general
     * KDL::Chain. It uses a svd-calculation based on householders
     * rotations.
     *
     * @ingroup KinematicFamily
     */
    class ChainIkSolverVel_pinv_givens : public ChainIkSolverVel
    {
    public:

        /**
         * Constructor of the solver
         *
         * @param chain the chain to calculate the inverse velocity
         * kinematics for
         *
         */
        explicit ChainIkSolverVel_pinv_givens(const Chain& chain);
        ~ChainIkSolverVel_pinv_givens();

        virtual int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out);
        /**
         * not (yet) implemented.
         *
         */
        virtual int CartToJnt(const JntArray& /*q_init*/, const FrameVel& /*v_in*/, JntArrayVel& /*q_out*/){return (error = E_NOT_IMPLEMENTED);};

        /// @copydoc KDL::SolverI::updateInternalDataStructures
        virtual void updateInternalDataStructures();

    private:
        const Chain& chain;
        unsigned int nj;
        ChainJntToJacSolver jnt2jac;
        Jacobian jac;
        bool transpose,toggle;
        unsigned int m,n;
        MatrixXd jac_eigen,U,V,B;
        VectorXd S,tempi,UY,SUY,qdot_eigen,v_in_eigen;
    };
}
#endif


///////////////////////////////////////////////////////////////////////////////
// partition_by_position_parity.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_MPL_DETAIL_PARTITION_BY_POSITION_PARITY_HPP_ER_2010
#define BOOST_MPL_DETAIL_PARTITION_BY_POSITION_PARITY_HPP_ER_2010

#include <boost/mpl/at.hpp>
#include <boost/mpl/bind.hpp>
#include <boost/mpl/inserter.hpp>
#include <boost/mpl/math/is_even.hpp>
#include <boost/mpl/transform.hpp>
#include <boost/mpl/range_c.hpp>
#include <boost/mpl/size.hpp>
#include <boost/mpl/vector.hpp>
#include <boost/mpl/zip_view.hpp>
#include <boost/mpl/partition.hpp>
#include <boost/mpl/pair.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/mpl/push_back.hpp>
#include <boost/mpl/unpack_args.hpp>

namespace boost {
namespace mpl{
namespace detail{

    // Usage:
    //     partition_by_position_parity< vector<a,x,b,y> >::type
    // equivalent to
    //     pair< vector<a,b>, vector<x,y> >
    template<typename T>
    struct partition_by_position_parity{

        typedef typename boost::mpl::size<T>::type size_;
        typedef boost::mpl::range_c<int,0,size_::value> range_;
        typedef boost::mpl::zip_view< boost::mpl::vector<T, range_> > zip_view_;

        template<typename Seq>
        struct fetch : boost::mpl::apply<
            boost::mpl::unpack_args<boost::mpl::_1>,
            Seq
        >{};

        struct inserter : boost::mpl::inserter<
           boost::mpl::vector0<>,
           boost::mpl::push_back<
                boost::mpl::_1,
                fetch<boost::mpl::_2>
            >
        >{};

        typedef typename boost::mpl::partition<
            zip_view_
            , boost::mpl::unpack_args< boost::mpl::is_even<boost::mpl::_2> >,
            inserter,
            inserter
        >::type  type;

    };

}// detail
}// mpl
}// boost

#endif