/*
@copyright Louis Dionne 2015
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/either.hpp>

#include <boost/hana/assert.hpp>
#include <boost/hana/tuple.hpp>

#include <laws/applicative.hpp>
#include <laws/base.hpp>
#include <laws/functor.hpp>
#include <laws/monad.hpp>
#include <test/identity.hpp>
using namespace boost::hana;

int main() {
    using test::ct_eq;
    struct undefined { };
    test::_injection<0> f{};

    auto eqs = make<Tuple>(
        right(ct_eq<0>{}), left(ct_eq<0>{}),
        right(ct_eq<1>{}), left(ct_eq<1>{}),
        right(ct_eq<2>{}), left(ct_eq<2>{}),
        right(ct_eq<3>{}), left(ct_eq<3>{})
    );

    auto eq_keys = make<Tuple>(ct_eq<0>{}, ct_eq<2>{});

    auto nested_eqs = make<Tuple>(
        right(right(ct_eq<2>{})), left(right(ct_eq<2>{})),
        right(right(ct_eq<3>{})), left(right(ct_eq<3>{})),

        right(left(ct_eq<0>{})), left(left(ct_eq<0>{})),
        right(left(ct_eq<1>{})), left(left(ct_eq<1>{}))
    );

    //////////////////////////////////////////////////////////////////////////
    // Functor
    //////////////////////////////////////////////////////////////////////////
    {
        // transform
        {
            test::_injection<0> f{};
            BOOST_HANA_CONSTANT_CHECK(equal(
                transform(left(ct_eq<0>{}), undefined{}),
                left(ct_eq<0>{})
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                transform(right(ct_eq<0>{}), f),
                right(f(ct_eq<0>{}))
            ));
        }

        // laws
        test::TestFunctor<Either>{eqs, eq_keys};
    }

    //////////////////////////////////////////////////////////////////////////
    // Applicative
    //////////////////////////////////////////////////////////////////////////
    {
        // ap
        {
            BOOST_HANA_CONSTANT_CHECK(equal(
                ap(left(ct_eq<0>{}), left(undefined{})),
                left(ct_eq<0>{})
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                ap(left(ct_eq<0>{}), right(undefined{})),
                left(ct_eq<0>{})
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                ap(right(undefined{}), left(ct_eq<0>{})),
                left(ct_eq<0>{})
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                ap(right(f), right(ct_eq<0>{})),
                right(f(ct_eq<0>{}))
            ));
        }

        // lift
        {
            BOOST_HANA_CONSTANT_CHECK(equal(
                lift<Either>(ct_eq<0>{}),
                right(ct_eq<0>{})
            ));
        }

        // laws
        test::TestApplicative<Either>{};
    }

    //////////////////////////////////////////////////////////////////////////
    // Monad
    //////////////////////////////////////////////////////////////////////////
    {
        // flatten
        {
            BOOST_HANA_CONSTANT_CHECK(equal(
                flatten(left(left(ct_eq<0>{}))),
                left(left(ct_eq<0>{}))
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                flatten(left(right(ct_eq<0>{}))),
                left(right(ct_eq<0>{}))
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                flatten(right(left(ct_eq<0>{}))),
                left(ct_eq<0>{})
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                flatten(right(right(ct_eq<0>{}))),
                right(ct_eq<0>{})
            ));
        }

        // laws
        test::TestMonad<Either>{eqs, nested_eqs};
    }
}


///////////////////////////////////////////////////////////////////////////////
// sequence_to_pairs.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_FUSION_CONTAINER_MAP_DETAIL_SEQUENCE_TO_PAIRS_HPP_ER_2010
#define BOOST_FUSION_CONTAINER_MAP_DETAIL_SEQUENCE_TO_PAIRS_HPP_ER_2010
#include <boost/fusion/include/pair.hpp>

#include <boost/mpl/at.hpp>
#include <boost/mpl/apply.hpp>
#include <boost/mpl/bind.hpp>
#include <boost/mpl/transform.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/mpl/unpack_args.hpp>
#include <boost/mpl/vector/vector10.hpp>
#include <boost/mpl/zip_view.hpp>
#include <boost/mpl/transform_view.hpp>

#include <boost/mpl/detail/partition_by_position_parity.hpp>

namespace boost{
namespace fusion{
namespace detail{

    // sequence_to_pairs<mpl::vector<K1,D1,...,Kn,Dn> >::type
    template<typename T>
    struct sequence_to_pairs{

        typedef typename boost::mpl::detail::partition_by_position_parity<
             T>::type partition_;
        typedef typename partition_::first keys_;
        typedef typename partition_::second datas_;

        typedef boost::mpl::transform_view<
           boost::mpl::zip_view< boost::mpl::vector2<keys_,datas_> >
           , boost::mpl::unpack_args<
               boost::fusion::pair<boost::mpl::_1,boost::mpl::_2>
            >
        > type;

    };

}// detail
}// fusion
}// boost

#endif


/*!
  @author Shin'ichiro Nakaoka
*/

#include "PyUtil.h"
#include "../EigenTypes.h"
#include "../EigenUtil.h"
#include <boost/format.hpp>

namespace python = boost::python;
using namespace boost::python;
using namespace cnoid;

namespace {

template<typename VectorType>
struct Vector_to_pylist_converter {
    static PyObject* convert(const VectorType& v){
        python::list retval;
        for(int i=0; i < v.size(); ++i){
            retval.append(v[i]);
        }
        return python::incref(retval.ptr());
    }
};

template<typename VectorType, int dim>
struct pylist_to_Vector_converter {
    pylist_to_Vector_converter(){
        converter::registry::push_back(
            &pylist_to_Vector_converter::convertible,
            &pylist_to_Vector_converter::construct,
            python::type_id<VectorType>());
    }
    static void* convertible(PyObject* pyo){
        if(PySequence_Check(pyo) && PySequence_Size(pyo) == dim){
            return pyo;
        }
        return 0;
    }
    static void construct(PyObject* pyo, python::converter::rvalue_from_python_stage1_data* data){
        VectorType* pv = new(reinterpret_cast<python::converter::rvalue_from_python_storage<VectorType>*>(data)->storage.bytes) VectorType();
        for(python::ssize_t i = 0; i < dim; ++i) {
            (*pv)[i] = python::extract<typename VectorType::Scalar>(PySequence_GetItem(pyo, i));
        }
        data->convertible = pv;
    }
};

template<typename MatrixType>
struct Matrix_to_pylist_converter {
    static PyObject* convert(const MatrixType& R){
        python::list Rout;
        for(int i=0; i < R.rows(); ++i){
            python::list row;
            for(int j=0; j < R.cols(); ++j){
                row.append(R(i, j));
            }
            Rout.append(row);
        }
        return python::incref(Rout.ptr());
    }
};

template<typename MatrixType, int rows, int cols>
struct pylist_to_Matrix_converter {
    pylist_to_Matrix_converter(){
        converter::registry::push_back(
            &pylist_to_Matrix_converter::convertible,
            &pylist_to_Matrix_converter::construct,
            python::type_id<MatrixType>());
    }
    static void* convertible(PyObject* pyo){
        if(PySequence_Check(pyo)){
            return pyo;
        }
        return 0;
    }
    static void construct(PyObject* pyo, python::converter::rvalue_from_python_stage1_data* data){
        MatrixType* M =
            new(reinterpret_cast<python::converter::rvalue_from_python_storage<MatrixType>*>(data)->storage.bytes)
            MatrixType();
        for(python::ssize_t i = 0; i < rows; ++i) {
            python::list row = python::extract<python::list>(PySequence_GetItem(pyo, i));
            for(python::ssize_t j = 0; j < rows; ++j) {
                (*M)(i, j) = python::extract<typename MatrixType::Scalar>(row[j]);
            }
        }
        data->convertible = M;
    }
};

template<typename TransformType>
struct Transform_to_pylist_converter {
    static PyObject* convert(const TransformType& T){
        python::list Tout;
        typename TransformType::ConstLinearPart R = T.linear();
        typename TransformType::ConstTranslationPart p = T.translation();
        python::list bottom;
        for(int i=0; i < 3; ++i){
            python::list row;
            row.append(R(i, 0));
            row.append(R(i, 1));
            row.append(R(i, 2));
            row.append(p[i]);
            Tout.append(row);
            bottom.append(static_cast<typename TransformType::Scalar>(0.0));
        }
        bottom.append(static_cast<typename TransformType::Scalar>(1.0));
        Tout.append(bottom);
        return python::incref(Tout.ptr());
    }
};

template<typename TransformType>
struct pylist_to_Transform_converter {
    pylist_to_Transform_converter(){
        converter::registry::push_back(
            &pylist_to_Transform_converter::convertible,
            &pylist_to_Transform_converter::construct,
            python::type_id<TransformType>());
    }
    static void* convertible(PyObject* pyo){
        if(PySequence_Check(pyo)){
            int numRows = PySequence_Size(pyo);
            if(numRows == 3 || numRows == 4){
                return pyo;
            }
        }
        return 0;
    }
    static void construct(PyObject* pyo, python::converter::rvalue_from_python_stage1_data* data){
        TransformType* T =
            new(reinterpret_cast<python::converter::rvalue_from_python_storage<TransformType>*>(data)->storage.bytes)
            TransformType();
        for(python::ssize_t i = 0; i < 3; ++i) {
            typename TransformType::LinearPart R = T->linear();
            typename TransformType::TranslationPart p = T->translation();
            python::list row = python::extract<python::list>(PySequence_GetItem(pyo, i));
            R(i, 0) = python::extract<typename TransformType::Scalar>(row[0]);
            R(i, 1) = python::extract<typename TransformType::Scalar>(row[1]);
            R(i, 2) = python::extract<typename TransformType::Scalar>(row[2]);
            p[i] = python::extract<typename TransformType::Scalar>(row[3]);
        }
        data->convertible = T;
    }
};

//! \todo replace this function with another python functions
Affine3 getAffine3FromAngleAxis(double angle, const Vector3& vec){
    /*
    Affine3 m;
    m = AngleAxis(angle, vec);
    return m;
    */
    return Affine3(AngleAxis(angle, vec));
}

//! \todo replace this function with another python functions
Vector3 getNormalized(const Vector3& vec){
    return vec.normalized();
}

}

namespace cnoid {

void exportPyEigenTypes()
{
    to_python_converter<Vector2, Vector_to_pylist_converter<Vector2> >();
    pylist_to_Vector_converter<Vector2, 2>();

    to_python_converter<Vector3, Vector_to_pylist_converter<Vector3> >();
    pylist_to_Vector_converter<Vector3, 3>();

    to_python_converter<Vector4, Vector_to_pylist_converter<Vector4> >();
    pylist_to_Vector_converter<Vector4, 4>();

    to_python_converter<Vector6, Vector_to_pylist_converter<Vector6> >();
    pylist_to_Vector_converter<Vector6, 6>();

    to_python_converter<Matrix3, Matrix_to_pylist_converter<Matrix3> >();
    pylist_to_Matrix_converter<Matrix3, 3, 3>();

    to_python_converter<Matrix4, Matrix_to_pylist_converter<Matrix4> >();
    pylist_to_Matrix_converter<Matrix4, 4, 4>();

    to_python_converter<Affine3, Transform_to_pylist_converter<Affine3> >();
    pylist_to_Transform_converter<Affine3>();

    to_python_converter<Position, Transform_to_pylist_converter<Position> >();
    pylist_to_Transform_converter<Position>();

    void (SE3::*SE3_set1)(const Vector3& translation, const Quat& rotation) = &SE3::set;
    void (SE3::*SE3_set2)(const Vector3& translation, const Matrix3& R) = &SE3::set;
    void (SE3::*SE3_set3)(const Position& T) = &SE3::set;
    const Vector3& (SE3::*SE3_translation_const)() const = &SE3::translation;
    const Quat& (SE3::*SE3_rotation_const)() const = &SE3::rotation;

    class_<SE3, boost::noncopyable>("SE3", init<>())
        .def("set", SE3_set1)
        .def("set", SE3_set2)
        .def("set", SE3_set3)
        .def("translation", SE3_translation_const, return_value_policy<copy_const_reference>())
        .def("rotation", SE3_rotation_const, return_value_policy<copy_const_reference>());

    python::def("rpyFromRot", cnoid::rpyFromRot);
    Matrix3 (*cnoid_rotFromRpy1)(const Vector3& rpy) = cnoid::rotFromRpy;
    python::def("rotFromRpy", cnoid_rotFromRpy1);
    python::def("omegaFromRot", cnoid::omegaFromRot);

    //! \todo replace the following functions with another python functions
    python::def("angleAxis", getAffine3FromAngleAxis);
    python::def("normalized", getNormalized);
}

}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_FABS_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_FABS_HPP_INCLUDED

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

 /*!
  @ingroup group-arithmetic
    Function object implementing fabs capabilities

    Computes the absolute value of its parameter.

    This is a convenient alias of @ref abs
  **/
  const boost::dispatch::functor<tag::fabs_> fabs = {};
} }
#endif

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

#endif


#include "UCCSD.hpp"
#include "GateFunction.hpp"
#include "FermionToSpinTransformation.hpp"
#include "CommutingSetGenerator.hpp"
// #include <boost/math/constants/constants.hpp>
#include "xacc_service.hpp"

using namespace xacc::quantum;

namespace xacc {
namespace vqe {

std::shared_ptr<Function> UCCSD::generate(
			std::map<std::string, InstructionParameter> parameters) {

    if (!parameters.count("n-electrons") && !parameters.count("n_electrons")) {
        xacc::error("Invalid mapping of parameters for UCCSD generator, missing n-electrons key.");
    }

    if (!parameters.count("n-qubits") && !parameters.count("n_qubits")) {
        xacc::error("Invalid mapping of parameters for UCCSD generator, missing n-qubits key.");
    }

    bool hasUnderscore = false;

    if (parameters.count("n_electrons")) {
        hasUnderscore = true;
        if (!parameters.count("n_qubits")) xacc::error("UCCSD Generator missing n_qubits key.");
    }

    if (parameters.count("n_qubits")) {
        hasUnderscore = true;
        if (!parameters.count("n_electrons")) xacc::error("UCCSD Generator missing n_electrons key.");
    }

    std::vector<InstructionParameter> params;
    if (hasUnderscore) {
        xacc::setOption("n-electrons",parameters["n_electrons"].toString());
        params.push_back(parameters["n_electrons"]);
        params.push_back(parameters["n_qubits"]);
    } else {
        params.push_back(parameters["n-electrons"]);
        params.push_back(parameters["n-qubits"]);
    }

    if (parameters.size() > 2) {
        for (auto& kv : parameters) {
            if(kv.first.find("electrons") == std::string::npos && kv.first.find("qubits") == std::string::npos) {
            // if (!boost::contains(kv.first, "electrons") && !boost::contains(kv.first, "qubits")) {
                params.push_back(kv.second);
            }
        }
    }

    return generate(nullptr, params);
}

std::shared_ptr<Function> UCCSD::generate(
		std::shared_ptr<AcceleratorBuffer> buffer,
		std::vector<InstructionParameter> parameters) {

    xacc::info("Running UCCSD Generator.");
    std::vector<xacc::InstructionParameter> variables;

    int nQubits = 0;
    int nElectrons = 0;
    if (parameters.empty()) {
    	if (!xacc::optionExists("n-electrons")) {
	    	xacc::error("To use this UCCSD State Prep IRGenerator, you "
		    		"must specify the number of electrons.");
	    }

	    if (!xacc::optionExists("n-qubits")) {
		    xacc::error("To use this UCCSD State Prep IRGenerator, you "
			    	"must specify the number of qubits.");
	    }

	    nQubits = std::stoi(xacc::getOption("n-qubits"));
	    nElectrons = std::stoi(xacc::getOption("n-electrons"));

    } else {
        if (parameters.size() < 2) xacc::error("Invalid input parameters for UCCSD generator.");

        nElectrons = parameters[0].as<int>();
        nQubits = parameters[1].as<int>();

        if (parameters.size() > 2) {
            for (int i = 2; i < parameters.size(); i++) {
                variables.push_back(parameters[i]);
            }
        }
    }

    xacc::info("UCCSD Generator (nqubits,nelectrons) = " + std::to_string(nQubits)+", " + std::to_string(nElectrons) +".");

	// Compute the number of parameters
	auto _nOccupied = (int) std::ceil(nElectrons / 2.0);
	auto _nVirtual = nQubits / 2 - _nOccupied;
	auto nSingle = _nOccupied * _nVirtual;
	auto nDouble = nSingle * (nSingle+1) / 2;
	auto _nParameters = nSingle + nDouble;

	std::vector<std::string> params;

    if (variables.empty()) {
	    for (int i = 0; i < _nParameters; i++) {
            params.push_back("theta" + std::to_string(i));
		    variables.push_back(InstructionParameter("theta" + std::to_string(i)));
        }
	} else {
        for (int i = 0; i < _nParameters; i++) {
		    params.push_back(variables[i].as<std::string>());
	    }
    }

    auto slice = [](const std::vector<std::string>& v, int start=0, int end=-1) {
        int oldlen = v.size();
        int newlen;
        if (end == -1 or end >= oldlen){
            newlen = oldlen-start;
        } else {
            newlen = end-start;
        }
        std::vector<std::string> nv(newlen);
        for (int i=0; i<newlen; i++) {
            nv[i] = v[start+i];
        }
        return nv;
    };
    auto singleParams = slice(params, 0, nSingle);
    auto doubleParams1 = slice(params, nSingle, 2*nSingle);
    auto doubleParams2 = slice(params, 2*nSingle);
    std::vector<std::function<int(int)>> fs{[](int i) {return 2*i;}, [](int i) {return 2*i+1;}};

    using OpType = std::vector<std::pair<int,int>>;
	auto kernel = std::make_shared<FermionKernel>("fermiUCCSD");
    int count = 0;
    for (int i = 0; i < _nVirtual; i++) {
        for (int j = 0; j < _nOccupied; j++) {
            auto vs = _nOccupied+i;
            auto os = j;
            for (int s = 0; s < 2; s++) {
                auto ti = fs[s];
                auto oi = fs[1-s];
                auto vt = ti(vs);
                auto vo = oi(vs);
                auto ot = ti(os);
                auto oo = oi(os);

                OpType op1{{vt,1},{ot,0}}, op2{{ot,1},{vt,0}};
                auto i1 = std::make_shared<FermionInstruction>(op1, singleParams[count]);
                auto i2 = std::make_shared<FermionInstruction>(op2, singleParams[count], std::complex<double>(-1.,0.));
                kernel->addInstruction(i1);
                kernel->addInstruction(i2);

                OpType op3{{vt,1},{ot,0}, {vo,1},{oo,0}}, op4{{oo,1}, {vo,0}, {ot,1},{vt,0}};
                auto i3 = std::make_shared<FermionInstruction>(op3, doubleParams1[count], std::complex<double>(-1.,0.));
                auto i4 = std::make_shared<FermionInstruction>(op4, doubleParams1[count]);

                kernel->addInstruction(i3);
                kernel->addInstruction(i4);

            }
            count++;
        }
    }

    count = 0;
    // routine for converting amplitudes for use by UCCSD
	std::vector< std::tuple<int, int> > tupleVec;
	for (int i = 0; i < _nVirtual; i++){
		for (int j = 0; j < _nOccupied; j++){
			tupleVec.push_back(std::make_tuple(i, j));
		}
	}
	// Combination lambda used to determine indices
	auto Combination = [=](std::vector< std::tuple<int, int> > t){
	std::vector< std::tuple<int, int, int, int> > comboVec;
		for (int i = 0; i < t.size(); i++){
			for (int j = i+1; j < t.size(); j++){
				const std::tuple<int, int, int, int> newTuple = std::tuple_cat(t[i], t[j]);
				comboVec.push_back(newTuple);
			}
		}
		return comboVec;
	};

	auto combineVec = Combination(tupleVec);
    for (auto i : combineVec) {
        auto p = std::get<0>(i);
        auto q = std::get<1>(i);
        auto r = std::get<2>(i);
        auto s = std::get<3>(i);

        auto vs1 = _nOccupied + p;
        auto os1 = q;
        auto vs2 = _nOccupied + r;
        auto os2 = s;

        for (int sa = 0; sa < 2; sa++) {
            for (int sb = 0; sb < 2; sb++) {
                auto ia = fs[sa];
                auto ib = fs[sb];

                auto v1a = ia(vs1);
                auto o1a = ia(os1);
                auto v2b = ib(vs2);
                auto o2b = ib(os2);

                OpType op5{{v1a,1},{o1a,0},{v2b,1},{o2b,0}}, op6{{o2b,1},{o1a,0},{v2b,1},{o2b,0}};
                auto i5 = std::make_shared<FermionInstruction>(op5, doubleParams2[count], std::complex<double>(-1.,0.));
                auto i6 = std::make_shared<FermionInstruction>(op6, doubleParams2[count]);

                kernel->addInstruction(i5);
                kernel->addInstruction(i6);
            }
        }
        count++;
    }

	// std::cout << "KERNEL: \n" << kernel->toString("") << "\n";
	// Create the FermionIR to pass to our transformation.
	auto fermionir = std::make_shared<FermionIR>();
	fermionir->addKernel(kernel);

	xacc::info("Done constructing UCCSD Fermion Operator.");
	xacc::info("Mapping UCCSD Fermion Operator to Spin. ");

	std::shared_ptr<FermionToSpinTransformation> transform;
	if (xacc::optionExists("fermion-transformation")) {
		auto transformStr = xacc::getOption("fermion-transformation");
		transform = xacc::getService<FermionToSpinTransformation>(
				transformStr);
	} else {
		transform = xacc::getService<FermionToSpinTransformation>(
				"jw");
	}

	auto compositeResult = transform->transform(*kernel.get());
//	auto resultsStr = compositeResult.toString();
//	boost::replace_all(resultsStr, "+", "+\n");

	// Create the Spin Hamiltonian
	auto transformedIR = compositeResult.toXACCIR();
	xacc::info("Done mapping UCCSD Fermion Operator to Spin.");

	std::unordered_map<std::string, Term> terms = compositeResult.getTerms();

	CommutingSetGenerator gen;
	auto commutingSets = gen.getCommutingSet(compositeResult, nQubits);
	auto pi = 3.14159265358979323; //boost::math::constants::pi<double>();
	auto gateRegistry = xacc::getService<IRProvider>("gate");

	auto uccsdGateFunction = gateRegistry->createFunction("uccsdPrep",{},
			variables);

	// Perform Trotterization...
	for (auto s : commutingSets) {

		for (auto inst : s) {
			Term spinInst = inst;

			// Get the individual pauli terms
			auto termsMap = std::get<2>(spinInst);

			std::vector<std::pair<int,std::string>> terms;
			for (auto& kv : termsMap) {
				if (kv.second != "I" && !kv.second.empty()) {
					terms.push_back({kv.first, kv.second});
				}
			}
			// The largest qubit index is on the last term
			int largestQbitIdx = terms[terms.size() - 1].first;
			auto tempFunction = gateRegistry->createFunction("temp", {}, {});

			for (int i = 0; i < terms.size(); i++) {

				auto qbitIdx = terms[i].first;
				auto gateName = terms[i].second;

				if (i < terms.size() - 1) {
					auto cnot =
							gateRegistry->createInstruction(
									"CNOT",
									std::vector<int> { qbitIdx,
											terms[i + 1].first });
					tempFunction->addInstruction(cnot);
				}

				if (gateName == "X") {
					auto hadamard =
							gateRegistry->createInstruction(
									"H", std::vector<int> { qbitIdx });
					tempFunction->insertInstruction(0, hadamard);
				} else if (gateName == "Y") {
					auto rx =
							gateRegistry->createInstruction(
									"Rx", std::vector<int> { qbitIdx });
					InstructionParameter p(pi / 2.0);
					rx->setParameter(0, p);
					tempFunction->insertInstruction(0, rx);
				}

				// Add the Rotation for the last term
				if (i == terms.size() - 1) {
					// FIXME DONT FORGET DIVIDE BY 2
					std::stringstream ss;
					ss << 2*std::imag(std::get<0>(spinInst)) << " * "
							<< std::get<1>(spinInst);
					auto rz =
							gateRegistry->createInstruction(
									"Rz", std::vector<int> { qbitIdx });

					InstructionParameter p(ss.str());
					rz->setParameter(0, p);
					tempFunction->addInstruction(rz);

				}
			}

			int counter = tempFunction->nInstructions();
			// Add the instruction on the backend of the circuit
			for (int i = terms.size() - 1; i >= 0; i--) {

				auto qbitIdx = terms[i].first;
				auto gateName = terms[i].second;

				if (i < terms.size() - 1) {
					auto cnot =
							gateRegistry->createInstruction(
									"CNOT",
									std::vector<int> { qbitIdx,
											terms[i + 1].first });
					tempFunction->insertInstruction(counter, cnot);
					counter++;
				}

				if (gateName == "X") {
					auto hadamard =
							gateRegistry->createInstruction(
									"H", std::vector<int> { qbitIdx });
					tempFunction->addInstruction(hadamard);
				} else if (gateName == "Y") {
					auto rx =
							gateRegistry->createInstruction(
									"Rx", std::vector<int> { qbitIdx });
					InstructionParameter p(-1.0 * (pi /2.0));
					rx->setParameter(0, p);
					tempFunction->addInstruction(rx);
				}

			}
			// Add to the total UCCSD State Prep function
			for (auto inst : tempFunction->getInstructions()) {
				uccsdGateFunction->addInstruction(inst);
			}
		}
	}

	for (int i = nElectrons-1; i >= 0; i--) {
		auto xGate = gateRegistry->createInstruction(
				"X", std::vector<int>{i});
		uccsdGateFunction->insertInstruction(0,xGate);
	}

	return uccsdGateFunction;
}

}
}


#include <iterator>
#include <string>

#include "tool/container/surjective_mapping.hpp"

using namespace nepomuk::tool::container;

// make sure we get a new main function here
#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_CASE(mapping_functionality)
{
    SurjectiveMapping<int, int> mapping(4, 2);
    mapping.map_codomain(0, 0);
    mapping.map_codomain(1, 0);
    mapping.map_codomain(2, 0);
    mapping.map_codomain(3, 1);

    BOOST_CHECK_EQUAL(mapping.codomain_mapping(0), 0);
    BOOST_CHECK_EQUAL(mapping.codomain_mapping(1), 0);
    BOOST_CHECK_EQUAL(mapping.codomain_mapping(2), 0);
    BOOST_CHECK_EQUAL(mapping.codomain_mapping(3), 1);

    auto itr_range = mapping.domain_mapping(0);
    BOOST_CHECK_EQUAL(std::distance(itr_range.begin(), itr_range.end()), 3);
    auto itr_range_2 = mapping.domain_mapping(1);
    BOOST_CHECK_EQUAL(std::distance(itr_range_2.begin(), itr_range_2.end()), 1);
    BOOST_CHECK_EQUAL(mapping.domain_size(), 2);
    BOOST_CHECK_EQUAL(mapping.codomain_size(), 4);
}


#include "intcode.hpp"

#include <boost/multiprecision/cpp_int.hpp>
#include <iostream>
#include <map>
#include <memory>

using Color = bool;
using Data = boost::multiprecision::cpp_int;
using Point = std::pair<int, int>;

inline auto read(const std::map<Point, Color>& grid, Point where)
{
    auto it = grid.find(where);
    return it != grid.end() && it->second;
}

std::map<Point, Color> paint(Color starting_color = false)
{
    auto inputs = std::make_shared<IOQueue<Data>>();
    auto outputs = std::make_shared<IOQueue<Data>>();

    Process<Data> brain {read_program<Data>("input.txt"), inputs, outputs};

    std::map<Point, Color> grid;
    Point pos {0, 0};
    Point dir {0, 1};

    grid[pos] = starting_color;

    while (true)
    {
        inputs->push(read(grid, pos));
        auto color = outputs->pop_if_alive(brain);

        if (!color.has_value())
            break;

        grid[pos] = (Color) *color;
        dir = outputs->pop() ? Point {dir.second, -dir.first}
                             : Point {-dir.second, dir.first};
        pos = Point {pos.first + dir.first, pos.second + dir.second};
    }
    brain.join();
    return grid;
}

int main()
{
    std::cout << "Part 1: " << paint().size() << std::endl;
    std::cout << "Part 2:\n";

    // Get the bounding boxes of the grid.
    auto grid = paint(true);
    Point xlim {0, 0};
    Point ylim {0, 0};
    for (const auto [pos, _] : grid)
    {
        xlim.first = std::min(xlim.first, pos.first);
        xlim.second = std::max(xlim.second, pos.first);
        ylim.first = std::min(ylim.first, pos.second);
        ylim.second = std::max(ylim.second, pos.second);
    }

    // Write painted area to terminal:
    for (int y = ylim.second; y >= ylim.first; --y)
    {
        for (int x = xlim.first; x <= xlim.second; ++x)
        {
            std::cout << (grid[Point {x, y}] ? "\u2588" : " ");
        }
        std::cout << '\n';
    }
}


#ifndef AIKIDO_COMMON_VANDERCORPUT_HPP_
#define AIKIDO_COMMON_VANDERCORPUT_HPP_

#include <limits>
#include <utility>
#include <boost/iterator/iterator_facade.hpp>

namespace aikido {
namespace common {

/// Generator for the Van der Corput sequence, a low-discripancy sequence
/// defined over a real interval. This sequence can be thought of as performing
/// successive level-order traversals of a binary search tree over the
/// interval
class VanDerCorput
{
public:
  class const_iterator;

  /// Construts the Van der Corput sequence over an interval of length \c span
  /// that terminates when the maximum gap between samples is less than
  /// \c minResolution. The sequence may be inclusive or exclusive of its end
  /// point; i.e. is an open or closed interval.
  ///
  /// \param span length of the interval
  /// \param includeStartpoint whether or not to include the startpoint
  /// \param includeEndpoint whether or not to include the endpoint
  /// \param minResolution resolution at which to terminate
  VanDerCorput(
      double span = 1.0,
      bool includeStartpoint = false,
      bool includeEndpoint = false,
      double minResolution = 0.0);

  /// Returns an iterator to the first element of the sequence.
  ///
  /// \return iterator to the first element of the sequence
  const_iterator begin() const;

  /// Returns an iterator to the element following the last element of the
  /// sequence.
  ///
  /// \return iterator followin the last element of the sequence
  const_iterator end() const;

  /// Returns the \c n-th element of the sequence (first element in the pair)
  /// and the current resolution (second element in the pair). Resolution is
  /// defined to be the maximum gap between samples thus far.
  ///
  /// \return pair of sample and maximum gap in samples
  std::pair<double, double> operator[](int n) const;

  /// Returns the total length of sequence.
  ///
  /// \return Non-negative number of the tatal length of sequence.
  std::size_t getLength() const;

private:
  constexpr static int BASE{2};
  constexpr static int MAX{std::numeric_limits<int>::max()};

  std::pair<double, double> computeVanDerCorput(int n) const;

  const double mSpan;
  const bool mIncludeStartpoint;
  const bool mIncludeEndpoint;
  double mMinResolution;
};

class VanDerCorput::const_iterator
    : public boost::iterator_facade<VanDerCorput::const_iterator,
                                    const double,
                                    boost::forward_traversal_tag,
                                    const double>
{
public:
  /// Dereference implementation for boost::iterator_facade
  double dereference() const;

  /// Increment implementation for boost::iterator_facade
  void increment();

  /// equal implementation for boost::iterator_facade
  bool equal(const VanDerCorput::const_iterator& other) const;

private:
  friend class VanDerCorput;

  /// Private constructor that should always be constructed from
  /// VanDerCorput::begin()
  const_iterator(const VanDerCorput* seq);

  const VanDerCorput* mSeq;
  int mN;
  bool mFinalIter;
  std::pair<double, double> mCurr;
};

} // namespace common
} // namespace aikido

#endif // AIKIDO_COMMON_VANDERCORPUT_HPP_


#include <UnitTest++/UnitTest++.h>
#include "../pixel_array_routines.h"

#include <boost/filesystem.hpp>
#include <iostream>
#include <sstream>
#include <cmath>
using namespace coela;
using namespace std;

SUITE(pixel_array_routines)
{

    string test_suite_output_dir= string(UnitTestSuite::GetSuiteName()) + "_tests/";
    TEST(Run_Standard_Suite_Setup) {
        cout << "*** \""<< UnitTestSuite::GetSuiteName() <<"\" unit tests running ***" <<endl;
        boost::filesystem::create_directories(test_suite_output_dir);
    }

    TEST(RegionCentroid) {
        PixelArray2d<double> img(20,20, 0.0);

        PixelIndex first_pixel(5,10);
        img(first_pixel) = 1.0;

        CHECK(PixelPosition::centre_of_pixel(first_pixel) ==
              pixel_array_routines::centroid(img, img.range())) ;

        PixelIndex second_pixel(15,10);
        img(second_pixel) = 1.0;

        PixelPosition avg_posn = PixelPosition::mid_point(
                                     PixelPosition::centre_of_pixel(first_pixel) ,
                                     PixelPosition::centre_of_pixel(second_pixel));

        CHECK(avg_posn == pixel_array_routines::centroid(img, img.range()));

        PixelRange cutoff_box = img.range();
        cutoff_box.high.x = second_pixel.x -1;

        CHECK(PixelPosition::centre_of_pixel(first_pixel) ==
              pixel_array_routines::centroid(img,cutoff_box));

    }

    TEST(bin_image) {
        //Tested complete with co-ord system under image_utils tests.
    }

}


/* Boost interval/compare/tribool.hpp template implementation file
 *
 * Copyright Guillaume Melquiond 2002-2003
 * Permission to use, copy, modify, sell, and distribute this software
 * is hereby granted without fee provided that the above copyright notice
 * appears in all copies and that both that copyright notice and this
 * permission notice appear in supporting documentation,
 *
 * None of the above authors make any representation about the
 * suitability of this software for any purpose. It is provided "as
 * is" without express or implied warranty.
 *
 * $Id: tribool.hpp,v 1.2 2003/02/05 17:34:31 gmelquio Exp $
 */

#ifndef BOOST_NUMERIC_INTERVAL_COMPARE_TRIBOOL_HPP
#define BOOST_NUMERIC_INTERVAL_COMPARE_TRIBOOL_HPP

#include <boost/numeric/interval/detail/interval_prototype.hpp>
#include <boost/numeric/interval/detail/test_input.hpp>
#include <boost/tribool.hpp>

namespace boost {
namespace numeric {
namespace interval_lib {
namespace compare {
namespace tribool {

template<class T, class Policies1, class Policies2> inline
tribool operator<(const interval<T, Policies1>& x, const interval<T, Policies2>& y)
{
  if (detail::test_input(x, y)) throw comparison_error();
  if (x.upper() < y.lower()) return true;
  if (x.lower() >= y.upper()) return false;
  return indeterminate;
}

template<class T, class Policies> inline
tribool operator<(const interval<T, Policies>& x, const T& y)
{
  if (detail::test_input(x, y)) throw comparison_error();
  if (x.upper() < y) return true;
  if (x.lower() >= y) return false;
  return indeterminate;
}

template<class T, class Policies1, class Policies2> inline
tribool operator<=(const interval<T, Policies1>& x, const interval<T, Policies2>& y)
{
  if (detail::test_input(x, y)) throw comparison_error();
  if (x.upper() <= y.lower()) return true;
  if (x.lower() > y.upper()) return false;
  return indeterminate;
}

template<class T, class Policies> inline
tribool operator<=(const interval<T, Policies>& x, const T& y)
{
  if (detail::test_input(x, y)) throw comparison_error();
  if (x.upper() <= y) return true;
  if (x.lower() > y) return false;
  return indeterminate;
}

template<class T, class Policies1, class Policies2> inline
tribool operator>(const interval<T, Policies1>& x, const interval<T, Policies2>& y)
{
  if (detail::test_input(x, y)) throw comparison_error();
  if (x.lower() > y.upper()) return true;
  if (x.upper() <= y.lower()) return false;
  return indeterminate;
}

template<class T, class Policies> inline
tribool operator>(const interval<T, Policies>& x, const T& y)
{
  if (detail::test_input(x, y)) throw comparison_error();
  if (x.lower() > y) return true;
  if (x.upper() <= y) return false;
  return indeterminate;
}

template<class T, class Policies1, class Policies2> inline
tribool operator>=(const interval<T, Policies1>& x, const interval<T, Policies2>& y)
{
  if (detail::test_input(x, y)) throw comparison_error();
  if (x.lower() >= y.upper()) return true;
  if (x.upper() < y.lower()) return false;
  return indeterminate;
}

template<class T, class Policies> inline
tribool operator>=(const interval<T, Policies>& x, const T& y)
{
  if (detail::test_input(x, y)) throw comparison_error();
  if (x.lower() >= y) return true;
  if (x.upper() < y) return false;
  return indeterminate;
}

template<class T, class Policies1, class Policies2> inline
tribool operator==(const interval<T, Policies1>& x, const interval<T, Policies2>& y)
{
  if (detail::test_input(x, y)) throw comparison_error();
  if (x.upper() == y.lower() && x.lower() == y.upper()) return true;
  if (x.upper() < y.lower() || x.lower() > y.upper()) return false;
  return indeterminate;
}

template<class T, class Policies> inline
tribool operator==(const interval<T, Policies>& x, const T& y)
{
  if (detail::test_input(x, y)) throw comparison_error();
  if (x.upper() == y && x.lower() == y) return true;
  if (x.upper() < y || x.lower() > y) return false;
  return indeterminate;
}

template<class T, class Policies1, class Policies2> inline
tribool operator!=(const interval<T, Policies1>& x, const interval<T, Policies2>& y)
{
  if (detail::test_input(x, y)) throw comparison_error();
  if (x.upper() < y.lower() || x.lower() > y.upper()) return true;
  if (x.upper() == y.lower() && x.lower() == y.upper()) return false;
  return indeterminate;
}

template<class T, class Policies> inline
tribool operator!=(const interval<T, Policies>& x, const T& y)
{
  if (detail::test_input(x, y)) throw comparison_error();
  if (x.upper() < y || x.lower() > y) return true;
  if (x.upper() == y && x.lower() == y) return false;
  return indeterminate;
}

} // namespace tribool
} // namespace compare
} // namespace interval_lib
} // namespace numeric
} // namespace boost

#endif // BOOST_NUMERIC_INTERVAL_COMPARE_TRIBOOL_HPP


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>

int main()
{
    using namespace mtl;
    typedef dense2D<double>  mat;
    typedef dense_vector<double, mtl::parameters<mtl::tag::row_major> >  row_vector;
    typedef operations::bracket_proxy<mat, mat&, double&> proxy_type;

    mat    A(2, 3);
    A= 1, 2, 3,
       4, 5, 6;

    row_vector   v(A[1][iall]);
    v[1]= 7;

    std::cout << "A is\n" << A;

    proxy_type row_1(A[1]);
    v[1]= 8;

    std::cout << "A is\n" << A;

    return 0;
}


// Copyright (C) 2004 Jeremy Siek <jsiek@cs.indiana.edu>
// 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)

#include <iostream>
#include <vector>
#include <deque>
#include <algorithm>
#include <boost/iterator/permutation_iterator.hpp>
#include <boost/cstdlib.hpp>
#include <assert.h>

int main() {
  using namespace boost;
  int i = 0;

  typedef std::vector< int > element_range_type;
  typedef std::deque< int > index_type;

  static const int element_range_size = 10;
  static const int index_size = 4;

  element_range_type elements( element_range_size );
  for(element_range_type::iterator el_it = elements.begin() ; el_it != elements.end() ; ++el_it)
    *el_it = std::distance(elements.begin(), el_it);

  index_type indices( index_size );
  for(index_type::iterator i_it = indices.begin() ; i_it != indices.end() ; ++i_it )
    *i_it = element_range_size - index_size + std::distance(indices.begin(), i_it);
  std::reverse( indices.begin(), indices.end() );

  typedef permutation_iterator< element_range_type::iterator, index_type::iterator > permutation_type;
  permutation_type begin = make_permutation_iterator( elements.begin(), indices.begin() );
  permutation_type it = begin;
  permutation_type end = make_permutation_iterator( elements.begin(), indices.end() );

  std::cout << "The original range is : ";
  std::copy( elements.begin(), elements.end(), std::ostream_iterator< int >( std::cout, " " ) );
  std::cout << "\n";

  std::cout << "The reindexing scheme is : ";
  std::copy( indices.begin(), indices.end(), std::ostream_iterator< int >( std::cout, " " ) );
  std::cout << "\n";

  std::cout << "The permutated range is : ";
  std::copy( begin, end, std::ostream_iterator< int >( std::cout, " " ) );
  std::cout << "\n";

  std::cout << "Elements at even indices in the permutation : ";
  it = begin;
  for(i = 0; i < index_size / 2 ; ++i, it+=2 ) std::cout << *it << " ";
  std::cout << "\n";

  std::cout << "Permutation backwards : ";
  it = begin + (index_size);
  assert( it != begin );
  for( ; it-- != begin ; ) std::cout << *it << " ";
  std::cout << "\n";

  std::cout << "Iterate backward with stride 2 : ";
  it = begin + (index_size - 1);
  for(i = 0 ; i < index_size / 2 ; ++i, it-=2 ) std::cout << *it << " ";
  std::cout << "\n";

  return boost::exit_success;
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#define NT2_UNIT_MODULE "boost::simd::constants true/false"

#include <boost/simd/include/constants/true.hpp>
#include <boost/simd/include/constants/false.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/dispatch/meta/as_unsigned.hpp>

////////////////////////////////////////////////////////////////////////////////
// Test value of true/false constant for integral types
////////////////////////////////////////////////////////////////////////////////
NT2_TEST_CASE_TPL( true_false_value, BOOST_SIMD_SIMD_TYPES )
{
  using boost::simd::logical;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef boost::simd::native<T,ext_t>                vT;
  typedef boost::simd::native< boost::simd::logical<T>, ext_t> vlT;
  for(std::size_t i=0; i< boost::simd::meta::cardinal_of<vT>::value;++i){
    std::cout << "for i =  " << i << std::endl;
    NT2_TEST_EQUAL( (boost::simd::False<vlT>())[i], boost::simd::logical<T>(false) );
  }
  for(std::size_t i=0; i< boost::simd::meta::cardinal_of<vT>::value;++i){
    std::cout << "for i =  " << i << std::endl;
    NT2_TEST_EQUAL( (boost::simd::True<vlT>())[i],  boost::simd::logical<T>(true) );
  }
}

NT2_TEST_CASE_TPL( true_false_logical_value, BOOST_SIMD_SIMD_TYPES )
{
  using boost::simd::logical;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef boost::simd::native< logical<T>,ext_t>                dst_t;
  typedef typename dst_t::type                vT;
  typedef typename boost::dispatch::meta::as_unsigned<T>::type  uns_t;
  std::cout << boost::simd::meta::cardinal_of<vT>::value << std::endl;
  std::cout << boost::simd::meta::cardinal_of<dst_t>::value << std::endl;

  for(std::size_t i=0; i< boost::simd::meta::cardinal_of<vT>::value;++i){
    std::cout << "for i =  " << i << std::endl;
    NT2_TEST_EQUAL( (boost::simd::False<dst_t>())[i], boost::simd::logical<T>(false) );
  }
  for(std::size_t i=0; i< boost::simd::meta::cardinal_of<vT>::value;++i){
    std::cout << "for i =  " << i << std::endl;
    NT2_TEST_EQUAL( (boost::simd::True<dst_t>())[i], boost::simd::logical<T>(true) );
  }
}


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

#define BOOST_TEST_MODULE lpc_test

#include <string>

#include <boost/test/unit_test.hpp>
#include <boost/test/data/test_case.hpp>
#include <boost/test/data/monomorphic.hpp>

#include <nil/crypto3/algebra/curves/bls12.hpp>
#include <nil/crypto3/algebra/fields/arithmetic_params/bls12.hpp>
#include <nil/crypto3/algebra/curves/params/multiexp/bls12.hpp>
#include <nil/crypto3/algebra/curves/params/wnaf/bls12.hpp>
#include <nil/crypto3/algebra/random_element.hpp>

#include <nil/crypto3/math/algorithms/unity_root.hpp>
#include <nil/crypto3/math/domains/evaluation_domain.hpp>
#include <nil/crypto3/math/algorithms/make_evaluation_domain.hpp>

#include <nil/crypto3/zk/commitments/polynomial/lpc.hpp>
#include <nil/crypto3/zk/commitments/polynomial/batched_lpc.hpp>
#include <nil/crypto3/zk/commitments/polynomial/fri.hpp>
#include <nil/crypto3/zk/commitments/polynomial/batched_fri.hpp>
#include <nil/crypto3/zk/snark/systems/plonk/placeholder/params.hpp>
#include <nil/crypto3/zk/commitments/type_traits.hpp>

using namespace nil::crypto3;
using namespace nil::crypto3::zk::snark;

BOOST_AUTO_TEST_SUITE(lpc_test_suite)

BOOST_AUTO_TEST_CASE(lpc_basic_test) {

    // setup
    typedef algebra::curves::bls12<381> curve_type;
    typedef typename curve_type::scalar_field_type FieldType;

    typedef hashes::sha2<256> merkle_hash_type;
    typedef hashes::sha2<256> transcript_hash_type;

    typedef typename containers::merkle_tree<merkle_hash_type, 2> merkle_tree_type;

    constexpr static const std::size_t lambda = 40;
    constexpr static const std::size_t k = 1;

    constexpr static const std::size_t d = 16;

    constexpr static const std::size_t r = boost::static_log2<(d - k)>::value;
    constexpr static const std::size_t m = 2;

    typedef zk::commitments::fri<FieldType, merkle_hash_type, transcript_hash_type, m> fri_type;

    typedef zk::commitments::list_polynomial_commitment_params<merkle_hash_type, transcript_hash_type, lambda, r, m> lpc_params_type;
    typedef zk::commitments::list_polynomial_commitment<FieldType, lpc_params_type> lpc_type;

    static_assert(zk::is_commitment<fri_type>::value);
    static_assert(zk::is_commitment<lpc_type>::value);
    static_assert(!zk::is_commitment<merkle_hash_type>::value);
    static_assert(!zk::is_commitment<merkle_tree_type>::value);
    static_assert(!zk::is_commitment<std::size_t>::value);

    typedef typename lpc_type::proof_type proof_type;

    constexpr static const std::size_t d_extended = d;
    std::size_t extended_log = boost::static_log2<d_extended>::value;
    std::vector<std::shared_ptr<math::evaluation_domain<FieldType>>> D =
        zk::commitments::detail::calculate_domain_set<FieldType>(extended_log, r);

    typename fri_type::params_type fri_params;

    math::polynomial<typename FieldType::value_type> q = {0, 0, 1};
    fri_params.r = r;
    fri_params.D = D;
    fri_params.q = q;
    fri_params.max_degree = d - 1;

    // commit

    math::polynomial<typename FieldType::value_type> f = {1, 3, 4, 1, 5, 6, 7, 2, 8, 7, 5, 6, 1, 2, 1, 1};

    merkle_tree_type tree = lpc_type::precommit(f, D[0]);

    // TODO: take a point outside of the basic domain
    std::vector<typename FieldType::value_type> evaluation_points = {
        algebra::fields::arithmetic_params<FieldType>::multiplicative_generator};

    std::array<std::uint8_t, 96> x_data {};
    zk::transcript::fiat_shamir_heuristic_sequential<transcript_hash_type> transcript(x_data);

    auto proof = lpc_type::proof_eval(evaluation_points, tree, f, fri_params, transcript);

    // verify
    zk::transcript::fiat_shamir_heuristic_sequential<transcript_hash_type> transcript_verifier(x_data);

    BOOST_CHECK(lpc_type::verify_eval(evaluation_points, proof, fri_params, transcript_verifier));
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(batched_lpc_test_suite)

BOOST_AUTO_TEST_CASE(batched_lpc_basic_test) {

    // setup
    typedef algebra::curves::bls12<381> curve_type;
    typedef typename curve_type::scalar_field_type FieldType;

    typedef hashes::sha2<256> merkle_hash_type;
    typedef hashes::sha2<256> transcript_hash_type;

    typedef typename containers::merkle_tree<merkle_hash_type, 2> merkle_tree_type;

    constexpr static const std::size_t leaf_size = 1;
    constexpr static const std::size_t lambda = 40;
    constexpr static const std::size_t k = 1;

    constexpr static const std::size_t d = 16;

    constexpr static const std::size_t r = boost::static_log2<(d - k)>::value;
    constexpr static const std::size_t m = 2;

    typedef zk::commitments::batched_fri<FieldType, merkle_hash_type, transcript_hash_type, m, leaf_size> fri_type;

    typedef zk::commitments::list_polynomial_commitment_params<merkle_hash_type, transcript_hash_type, lambda, r, m> lpc_params_type;
    typedef zk::commitments::batched_list_polynomial_commitment<FieldType, lpc_params_type, leaf_size> lpc_type;

    typedef typename lpc_type::proof_type proof_type;

    constexpr static const std::size_t d_extended = d;
    std::size_t extended_log = boost::static_log2<d_extended>::value;
    std::vector<std::shared_ptr<math::evaluation_domain<FieldType>>> D =
        zk::commitments::detail::calculate_domain_set<FieldType>(extended_log, r);

    typename fri_type::params_type fri_params;

    math::polynomial<typename FieldType::value_type> q = {0, 0, 1};
    fri_params.r = r;
    fri_params.D = D;
    fri_params.q = q;
    fri_params.max_degree = d - 1;

    // commit

    std::array<math::polynomial<typename FieldType::value_type>, leaf_size> f =
        {{
            {1, 3, 4, 1, 5, 6, 7, 2, 8, 7, 5, 6, 1, 2, 1, 1}
        }};

    merkle_tree_type tree = lpc_type::precommit(f, D[0]);

    // TODO: take a point outside of the basic domain
    std::array<std::vector<typename FieldType::value_type>, leaf_size> evaluation_points =
        {{
            {algebra::fields::arithmetic_params<FieldType>::multiplicative_generator}
        }};

    std::array<std::uint8_t, 96> x_data {};
    zk::transcript::fiat_shamir_heuristic_sequential<transcript_hash_type> transcript(x_data);

    auto proof = lpc_type::proof_eval(evaluation_points, tree, f, fri_params, transcript);

    // verify
    zk::transcript::fiat_shamir_heuristic_sequential<transcript_hash_type> transcript_verifier(x_data);

    BOOST_CHECK(lpc_type::verify_eval(evaluation_points, proof, fri_params, transcript_verifier));
}

BOOST_AUTO_TEST_CASE(batched_lpc_basic_test_2) {

    // setup
    typedef algebra::curves::bls12<381> curve_type;
    typedef typename curve_type::scalar_field_type FieldType;

    typedef hashes::sha2<256> merkle_hash_type;
    typedef hashes::sha2<256> transcript_hash_type;

    typedef typename containers::merkle_tree<merkle_hash_type, 2> merkle_tree_type;

    constexpr static const std::size_t leaf_size = 2;
    constexpr static const std::size_t lambda = 40;
    constexpr static const std::size_t k = 1;

    constexpr static const std::size_t d = 16;

    constexpr static const std::size_t r = boost::static_log2<(d - k)>::value;
    constexpr static const std::size_t m = 2;

    typedef zk::commitments::batched_fri<FieldType, merkle_hash_type, transcript_hash_type, m, leaf_size> fri_type;

    typedef zk::commitments::list_polynomial_commitment_params<merkle_hash_type, transcript_hash_type, lambda, r, m> lpc_params_type;
    typedef zk::commitments::batched_list_polynomial_commitment<FieldType, lpc_params_type, leaf_size> lpc_type;

    typedef typename lpc_type::proof_type proof_type;

    constexpr static const std::size_t d_extended = d;
    std::size_t extended_log = boost::static_log2<d_extended>::value;
    std::vector<std::shared_ptr<math::evaluation_domain<FieldType>>> D =
        zk::commitments::detail::calculate_domain_set<FieldType>(extended_log, r);

    typename fri_type::params_type fri_params;

    math::polynomial<typename FieldType::value_type> q = {0, 0, 1};
    fri_params.r = r;
    fri_params.D = D;
    fri_params.q = q;
    fri_params.max_degree = d - 1;

    // commit

    std::array<math::polynomial<typename FieldType::value_type>, leaf_size> f =
        {{
            {1, 3, 4, 1, 5, 6, 7, 2, 8, 7, 5, 6, 1, 2, 1, 1},
            {1, 2, 5, 1, 5, 6, 7, 2, 8, 7, 5, 6, 1, 2, 1, 1}
        }};

    merkle_tree_type tree = lpc_type::precommit(f, D[0]);

    // TODO: take a point outside of the basic domain
    std::array<std::vector<typename FieldType::value_type>, leaf_size> evaluation_points =
        {{
            {algebra::fields::arithmetic_params<FieldType>::multiplicative_generator},
            {algebra::fields::arithmetic_params<FieldType>::multiplicative_generator}
        }};

    std::array<std::uint8_t, 96> x_data {};
    zk::transcript::fiat_shamir_heuristic_sequential<transcript_hash_type> transcript(x_data);

    auto proof = lpc_type::proof_eval(evaluation_points, tree, f, fri_params, transcript);

    // verify
    zk::transcript::fiat_shamir_heuristic_sequential<transcript_hash_type> transcript_verifier(x_data);

    BOOST_CHECK(lpc_type::verify_eval(evaluation_points, proof, fri_params, transcript_verifier));
}

BOOST_AUTO_TEST_CASE(batched_lpc_basic_test_runtime_size) {

    // setup
    typedef algebra::curves::bls12<381> curve_type;
    typedef typename curve_type::scalar_field_type FieldType;

    typedef hashes::sha2<256> merkle_hash_type;
    typedef hashes::sha2<256> transcript_hash_type;

    typedef typename containers::merkle_tree<merkle_hash_type, 2> merkle_tree_type;

    constexpr static const std::size_t leaf_size = 2;
    constexpr static const std::size_t lambda = 40;
    constexpr static const std::size_t k = 1;

    constexpr static const std::size_t d = 16;

    constexpr static const std::size_t r = boost::static_log2<(d - k)>::value;
    constexpr static const std::size_t m = 2;

    typedef zk::commitments::batched_fri<FieldType, merkle_hash_type, transcript_hash_type, m, leaf_size> fri_type;

    typedef zk::commitments::list_polynomial_commitment_params<merkle_hash_type, transcript_hash_type, lambda, r, m> lpc_params_type;
    typedef zk::commitments::batched_list_polynomial_commitment<FieldType, lpc_params_type, 0, true> lpc_type;

    typedef typename lpc_type::proof_type proof_type;

    constexpr static const std::size_t d_extended = d;
    std::size_t extended_log = boost::static_log2<d_extended>::value;
    std::vector<std::shared_ptr<math::evaluation_domain<FieldType>>> D =
        zk::commitments::detail::calculate_domain_set<FieldType>(extended_log, r);

    typename fri_type::params_type fri_params;

    math::polynomial<typename FieldType::value_type> q = {0, 0, 1};
    fri_params.r = r;
    fri_params.D = D;
    fri_params.q = q;
    fri_params.max_degree = d - 1;

    // commit

    std::vector<math::polynomial<typename FieldType::value_type>> f =
        {
            {1, 3, 4, 1, 5, 6, 7, 2, 8, 7, 5, 6, 1, 2, 1, 1},
            {1, 2, 5, 1, 5, 6, 7, 2, 8, 7, 5, 6, 1, 2, 1, 1}
        };

    merkle_tree_type tree = lpc_type::precommit(f, D[0]);

    // TODO: take a point outside of the basic domain
    std::vector<std::vector<typename FieldType::value_type>> evaluation_points =
        {
            {algebra::fields::arithmetic_params<FieldType>::multiplicative_generator},
            {algebra::fields::arithmetic_params<FieldType>::multiplicative_generator}
        };

    std::array<std::uint8_t, 96> x_data {};
    zk::transcript::fiat_shamir_heuristic_sequential<transcript_hash_type> transcript(x_data);

    auto proof = lpc_type::proof_eval(evaluation_points, tree, f, fri_params, transcript);

    // verify
    zk::transcript::fiat_shamir_heuristic_sequential<transcript_hash_type> transcript_verifier(x_data);

    BOOST_CHECK(lpc_type::verify_eval(evaluation_points, proof, fri_params, transcript_verifier));
}

BOOST_AUTO_TEST_SUITE_END()


#include "gtest/gtest.h"

#include <Configuration.h>
#include <Eigen/Dense>
#include <Utils/IO/IOUtilities.hpp>
#include <Utils/Math/MathUtilities.hpp>

// Robot, mpc and whole body controller
#include <PnC/RobotSystem/RobotSystem.hpp>
#include <PnC/WBC/IHWBC/IHWBC.hpp>
#include <PnC/MPC/CMPC.hpp>

// Draco Specific
#include <PnC/DracoPnC/ContactSet/ContactSet.hpp>
#include <PnC/DracoPnC/CtrlSet/CtrlSet.hpp>
#include <PnC/DracoPnC/TaskSet/TaskSet.hpp>
#include <PnC/DracoPnC/DracoDefinition.hpp>

void getInitialConfiguration(RobotSystem* & robot, Eigen::VectorXd & q_out, Eigen::VectorXd & qdot_out) {
    int lKneeIdx = robot->getDofIdx("lKnee");
    int lHipPitchIdx = robot->getDofIdx("lHipPitch");
    int rKneeIdx = robot->getDofIdx("rKnee");
    int rHipPitchIdx = robot->getDofIdx("rHipPitch");
    int lAnkleIdx = robot->getDofIdx("lAnkle");
    int rAnkleIdx = robot->getDofIdx("rAnkle");

    Eigen::VectorXd q = robot->getQ();
    Eigen::VectorXd qdot(robot->getNumDofs());
    qdot.setZero();

    q[2] = 0.893; //1.193;
    double alpha(-M_PI / 4.);
    double beta(M_PI / 5.5);
    q[lHipPitchIdx] = alpha;
    q[lKneeIdx] = beta - alpha;
    q[rHipPitchIdx] = alpha;
    q[rKneeIdx] = beta - alpha;
    q[lAnkleIdx] = M_PI / 2 - beta;
    q[rAnkleIdx] = M_PI / 2 - beta;

    q_out = q;
    qdot_out = qdot;
}

TEST(MPC, toy_mpc){
  CMPC convex_mpc;

  Eigen::MatrixXd I_robot_body = 5.0*Eigen::MatrixXd::Identity(3,3); // Body Inertia matrix

  // System Params
  double robot_mass = 50; //kg
  convex_mpc.setRobotMass(robot_mass); // (kilograms)
  convex_mpc.setRobotInertia(I_robot_body);

  // MPC Params
  int horizon = 1;
  convex_mpc.setHorizon(horizon); // horizon timesteps
  convex_mpc.setDt(0.025); // (seconds) per horizon
  convex_mpc.setMu(0.7); //  friction coefficient
  convex_mpc.setMaxFz(500); // (Newtons) maximum vertical reaction force per foot.

  // Set the cost vector
  Eigen::VectorXd cost_vec(13);
  // Vector cost indexing: <<  th1,  th2,  th3,  px,  py,  pz,   w1,  w2,   w3,   dpx,  dpy,  dpz,  g
  // cost_vec << 0.25, 0.25, 10.0, 2.0, 2.0, 50.0, 0.0, 0.0, 0.30, 0.20, 0.2, 0.10, 0.0;
  cost_vec << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0;
  convex_mpc.setCostVec(cost_vec);
  convex_mpc.setControlAlpha(1e-12);

  // Starting robot state
  // Current reduced state of the robot
  // x = [Theta, p, omega, pdot, g] \in \mathbf{R}^13
  Eigen::VectorXd x0(13);

  double init_roll(0), init_pitch(0), init_yaw(0),
         init_com_x(0), init_com_y(0), init_com_z(0.75),
         init_roll_rate(0), init_pitch_rate(0), init_yaw_rate(0),
         init_com_x_rate(0), init_com_y_rate(0), init_com_z_rate(0);

  x0 = convex_mpc.getx0(init_roll, init_pitch, init_yaw,
                        init_com_x, init_com_y, init_com_z,
                        init_roll_rate, init_pitch_rate, init_yaw_rate,
                        init_com_x_rate, init_com_y_rate, init_com_z_rate);

  // Feet Configuration
  // Set Foot contact locations w.r.t world
  Eigen::MatrixXd r_feet(3, 4); // Each column is a reaction force in x,y,z
  r_feet.setZero();
  double foot_length = 0.05; // 5cm distance between toe and heel
  double nominal_width = 0.1; // 10cm distance between left and right feet

  // 4 Contact Configuration
  // Right Foot Front
  r_feet(0, 0) = foot_length/2.0;  // x
  r_feet(1, 0) = -nominal_width/2.0; // y
  // Right Foot Back
  r_feet(0, 1) = -foot_length/2.0; // x
  r_feet(1, 1) = -nominal_width/2.0; //y
  // Left Foot Front
  r_feet(0, 2) = foot_length/2.0;  // x
  r_feet(1, 2) = nominal_width/2.0; //y
  // Left Foot Back
  r_feet(0, 3) = -foot_length/2.0; // x
  r_feet(1, 3) = nominal_width/2.0; //y

  int n_Fr = r_feet.cols(); // Number of contacts
  int n = 13;
  int m = 3*n_Fr;

  Eigen::VectorXd f_vec_out(m*horizon);
  Eigen::MatrixXd f_Mat(3, n_Fr); f_Mat.setZero();

  // Get constant desired reference
  Eigen::VectorXd x_des(n);

  x_des.setZero();
  x_des[0] = 0.0; //M_PI/8; //des roll orientation
  x_des[1] = 0.0 ;//-M_PI/8; //des pitch orientation
  x_des[2] = M_PI/12; // Yaw orientation

  x_des[3] = 0.0;//-0.1;//;0.75; // Set desired z height to be 0.75m from the ground
  x_des[5] = 0.75;//;0.75; // Set desired z height to be 0.75m from the ground

  Eigen::VectorXd X_des(n*horizon);
  convex_mpc.get_constant_desired_x(x_des, X_des);

  Eigen::VectorXd x_pred(n); // Container to hold the predicted state after 1 horizon timestep

  // Solve the MPC
  convex_mpc.solve_mpc(x0, X_des, r_feet, x_pred, f_vec_out);
  myUtils::pretty_print(x0, std::cout, "x0");
  myUtils::pretty_print(x_pred, std::cout, "x_pred");
  // convex_mpc.print_f_vec(n_Fr, f_vec_out);

  double total_z_force = 0.0;
  f_Mat = convex_mpc.getMatComputedGroundForces();
  for (int j = 0; j < f_Mat.cols(); j++){
    std::cout << "  f" << j << ":" << f_Mat.col(j).transpose() << std::endl;
    total_z_force += f_Mat.col(j)[2];
  }

  std::cout << "total z force = " << total_z_force << std::endl;

  double robot_weight = robot_mass*9.81;
  ASSERT_GE((total_z_force+1e-3), robot_weight);
}

TEST(MPC_IHWBC, zyx_rates_to_ang_vel){
    Eigen::Vector3d rpy;
    Eigen::Vector3d rpy_rate;
    rpy << 0, M_PI/4.0, M_PI/4.0;
    rpy_rate << 0.0, 0.0, 5.0;

    Eigen::Vector3d ang_vel = myUtils::EulerZYXRatestoAngVel(rpy[0], rpy[1], rpy[2],
                                                             rpy_rate[0], rpy_rate[1], rpy_rate[2]);

    ASSERT_DOUBLE_EQ(ang_vel[0], 0.0);
    ASSERT_DOUBLE_EQ(ang_vel[1], 0.0);
    ASSERT_DOUBLE_EQ(ang_vel[2], 5.0);

}

// Torque limit test
TEST(MPC_IHWBC, mpc_ihwbc_test) {
    RobotSystem* robot;
    robot = new RobotSystem(6, THIS_COM "RobotModel/Robot/Draco/DracoPnC_Dart.urdf");

    // Initialize and Update the robot
    Eigen::VectorXd q, qdot;
    getInitialConfiguration(robot, q, qdot);
    robot->updateSystem(q, qdot);
    // myUtils::pretty_print(q, std::cout, "q");

    // Get the initial robot inertia
    Eigen::MatrixXd Ig_o = robot->getCentroidInertia();
    Eigen::MatrixXd I_robot_body = Ig_o.block(0,0,3,3);

    Eigen::VectorXd robot_com = robot->getCoMPosition();
    Eigen::VectorXd robot_com_rate = robot->getCoMVelocity();

    myUtils::pretty_print(robot_com, std::cout, "robot_com");
    myUtils::pretty_print(robot_com_rate, std::cout, "robot_com_rate");

    // Initialize Convex MPC
    CMPC convex_mpc;

    // System Params
    double robot_mass = robot->getRobotMass(); //kg
    convex_mpc.setRobotMass(robot_mass); // (kilograms)
    convex_mpc.setRobotInertia(I_robot_body);

    // MPC Params
    int horizon = 10;
    convex_mpc.setHorizon(horizon); // horizon timesteps
    convex_mpc.setDt(0.025); // (seconds) per horizon
    convex_mpc.setMu(0.7); //  friction coefficient
    convex_mpc.setMaxFz(500); // (Newtons) maximum vertical reaction force per foot.
    convex_mpc.rotateBodyInertia(false); // Assume we are always providing the world inertia

    // Set the cost vector
    Eigen::VectorXd cost_vec(13);
    // Vector cost indexing: <<  th1,  th2,  th3,  px,  py,  pz,   w1,  w2,   w3,   dpx,  dpy,  dpz,  g
    cost_vec << 0.25, 0.25, 10.0, 2.0, 2.0, 50.0, 0.0, 0.0, 0.30, 0.20, 0.2, 0.10, 0.0;
    double cost_factor = 8.0;
    cost_vec *= cost_factor;
    convex_mpc.setCostVec(cost_vec);

    // Starting robot state
    // Current reduced state of the robot
    // x = [Theta, p, omega, pdot, g] \in \mathbf{R}^13
    Eigen::VectorXd x0(13);

    double init_roll(q[3]), init_pitch(q[4]), init_yaw(q[5]),
         init_com_x(robot_com[0]), init_com_y(robot_com[1]), init_com_z(robot_com[2]),
         init_roll_rate(qdot[3]), init_pitch_rate(qdot[4]), init_yaw_rate(qdot[5]),
         init_com_x_rate(robot_com_rate[0]), init_com_y_rate(robot_com_rate[1]), init_com_z_rate(robot_com_rate[2]);

    x0 = convex_mpc.getx0(init_roll, init_pitch, init_yaw,
                        init_com_x, init_com_y, init_com_z,
                        init_roll_rate, init_pitch_rate, init_yaw_rate,
                        init_com_x_rate, init_com_y_rate, init_com_z_rate);

    myUtils::pretty_print(x0, std::cout, "x0");

    // Feet Configuration
    // Set Foot contact locations w.r.t world
    Eigen::MatrixXd r_feet(3, 4); // Each column is a reaction force in x,y,z
    r_feet.setZero();

   // Get the feet configuration
    Eigen::Vector3d feet_pos = robot->getBodyNodeCoMIsometry(DracoBodyNode::rFootFront).translation();
    r_feet.col(0) = robot->getBodyNodeCoMIsometry(DracoBodyNode::rFootFront).translation().transpose();
    r_feet.col(1) = robot->getBodyNodeCoMIsometry(DracoBodyNode::rFootBack).translation().transpose();
    r_feet.col(2) = robot->getBodyNodeCoMIsometry(DracoBodyNode::lFootFront).translation().transpose();
    r_feet.col(3) = robot->getBodyNodeCoMIsometry(DracoBodyNode::lFootBack).translation().transpose();

    myUtils::pretty_print(feet_pos, std::cout, "feet_pos");
    myUtils::pretty_print(r_feet, std::cout, "r_feet");

    int n_Fr = r_feet.cols(); // Number of contacts
    int n = 13;
    int m = 3*n_Fr;

    Eigen::VectorXd f_vec_out(m*horizon);
    Eigen::MatrixXd f_Mat(3, n_Fr); f_Mat.setZero();

    // Get constant desired reference
    Eigen::VectorXd x_des(n);

    // x = [Theta, p, omega, pdot, g] \in \mathbf{R}^13
    x_des.setZero();
    x_des[0] = q[3]; //M_PI/8; //des roll orientation
    x_des[1] = q[4];//-M_PI/8; //des pitch orientation
    x_des[2] = q[5]; // Yaw orientation

    x_des[3] = robot_com[0] - 0.05; // Set desired com x pos
    x_des[4] = robot_com[1]; // Set desired com y pos
    x_des[5] = robot_com[2] + 0.045;//;0.75; // Set desired z height to be the current + a small increment

    myUtils::pretty_print(x_des, std::cout, "x_des");

    Eigen::VectorXd X_des(n*horizon);
    convex_mpc.get_constant_desired_x(x_des, X_des);

    Eigen::VectorXd x_pred(n); // Container to hold the predicted state after 1 horizon timestep

    // Solve the MPC
    convex_mpc.solve_mpc(x0, X_des, r_feet, x_pred, f_vec_out);

    myUtils::pretty_print(x_pred, std::cout, "x_pred");

    double total_z_force = 0.0;
    f_Mat = convex_mpc.getMatComputedGroundForces();
    std::cout << "Desired Forces on the feet" << std::endl;
    for (int j = 0; j < f_Mat.cols(); j++){
        std::cout << "  f" << j << ":" << f_Mat.col(j).transpose() << std::endl;
        total_z_force += f_Mat.col(j)[2];
    }

    Eigen::VectorXd Fd_current = convex_mpc.getComputedGroundForces();

    std::cout << "total z force = " << total_z_force << std::endl;
    myUtils::pretty_print(Fd_current , std::cout, "Fd_current");

    // Set desired com and body orientation from predicted state
    double des_roll = x_pred[0];
    double des_pitch = x_pred[1];
    double des_yaw = x_pred[2];

    double des_pos_x = x_pred[3];
    double des_pos_y = x_pred[4];
    double des_pos_z = x_pred[5];

    double des_roll_rate = x_pred[6];
    double des_pitch_rate = x_pred[7];
    double des_yaw_rate = x_pred[8];

    double des_vel_x = x_pred[9];
    double des_vel_y = x_pred[10];
    double des_vel_z = x_pred[11];

    //  Initialize IHWBC
    //  Initialize actuated list
    std::vector<bool> act_list;
    act_list.resize(robot->getNumDofs(), true);
    for (int i(0); i < robot->getNumVirtualDofs(); ++i){
        act_list[i] = false;
    }
    IHWBC* ihwbc = new IHWBC(act_list);

    // Task and Contact list
    std::vector<Task*> task_list;
    std::vector<ContactSpec*> contact_list;

    // Create Tasks

    // Body RxRyZ tasks or CoM xyz task or Linear Momentum Task
    // Body Rx Ry Rz
    // Foot location task.
    // Joint Position Task. It appears that it's important to have this task to ensure that uncontrolled qddot does not blow up
    Task* com_task_ = new BasicTask(robot, BasicTaskType::COM, 3);
    Task* body_ori_task_ = new BasicTask(robot, BasicTaskType::LINKORI, 3, DracoBodyNode::Torso);

    Task* rfoot_center_rz_xyz_task = new FootRzXYZTask(robot, DracoBodyNode::rFootCenter);
    Task* lfoot_center_rz_xyz_task = new FootRzXYZTask(robot, DracoBodyNode::lFootCenter);
    Task* total_joint_task = new BasicTask(robot, BasicTaskType::JOINT, Draco::n_adof);

    task_list.push_back(com_task_);
    task_list.push_back(body_ori_task_);
    task_list.push_back(rfoot_center_rz_xyz_task);
    task_list.push_back(lfoot_center_rz_xyz_task);
    task_list.push_back(total_joint_task);

    // Set Task Gains
    Eigen::VectorXd kp_foot = 100*Eigen::VectorXd::Ones(4);
    Eigen::VectorXd kd_foot = 1.0*Eigen::VectorXd::Ones(4);
    rfoot_center_rz_xyz_task->setGain(kp_foot, kd_foot);
    lfoot_center_rz_xyz_task->setGain(kp_foot, kd_foot);

    Eigen::VectorXd kp_jp = Eigen::VectorXd::Ones(Draco::n_adof);
    Eigen::VectorXd kd_jp = 0.1*Eigen::VectorXd::Ones(Draco::n_adof);
    total_joint_task->setGain(kp_jp, kd_jp);

    Eigen::VectorXd kp_com = 100*Eigen::VectorXd::Ones(3);
    Eigen::VectorXd kd_com = 1.0*Eigen::VectorXd::Ones(3);
    com_task_->setGain(kp_com, kd_com);

    Eigen::VectorXd kp_body_rpy = 100*Eigen::VectorXd::Ones(3);
    Eigen::VectorXd kd_body_rpy = 1.0*Eigen::VectorXd::Ones(3);
    body_ori_task_->setGain(kp_body_rpy, kd_body_rpy);

    // Set the desired task values
    // COM Task
    Eigen::VectorXd com_pos_des(3); com_pos_des.setZero();
    Eigen::VectorXd com_vel_des(3); com_vel_des.setZero();
    Eigen::VectorXd com_acc_des(3); com_acc_des.setZero();

    com_pos_des[0] = des_pos_x;
    com_pos_des[1] = des_pos_y;
    com_pos_des[2] = des_pos_z;

    com_vel_des[0] = des_vel_x;
    com_vel_des[1] = des_vel_y;
    com_vel_des[2] = des_vel_z;

    com_task_->updateTask(com_pos_des, com_vel_des, com_acc_des);
    myUtils::pretty_print(com_pos_des, std::cout, "com_pos_des");

    // Set Body Orientation task values
    Eigen::VectorXd body_ori_des(4); body_ori_des.setZero();
    Eigen::VectorXd body_ori_vel_des(3); body_ori_vel_des.setZero();
    Eigen::VectorXd body_ori_acc_des(3); body_ori_acc_des.setZero();
    Eigen::Quaternion<double> body_ori_des_quat(1, 0, 0, 0);

    body_ori_des_quat = myUtils::EulerZYXtoQuat(des_roll, des_pitch, des_yaw);
    body_ori_des[0] = body_ori_des_quat.w();
    body_ori_des[1] = body_ori_des_quat.x();
    body_ori_des[2] = body_ori_des_quat.y();
    body_ori_des[3] = body_ori_des_quat.z();

    body_ori_vel_des = myUtils::EulerZYXRatestoAngVel(des_roll, des_pitch, des_yaw,
                                                      des_roll_rate, des_pitch_rate, des_yaw_rate);
    body_ori_task_->updateTask(body_ori_des, body_ori_vel_des, body_ori_acc_des);

    myUtils::pretty_print(body_ori_des, std::cout, "body_ori_des");
    myUtils::pretty_print(body_ori_vel_des, std::cout, "body_ori_vel_des");

    // Foot Task
    Eigen::VectorXd rfoot_pos_des(7); rfoot_pos_des.setZero();
    Eigen::VectorXd lfoot_pos_des(7); lfoot_pos_des.setZero();
    Eigen::VectorXd foot_vel_des(6); foot_vel_des.setZero();
    Eigen::VectorXd foot_acc_des(6); foot_acc_des.setZero();

    Eigen::Quaternion<double> rfoot_ori_act(robot->getBodyNodeCoMIsometry(DracoBodyNode::rFootCenter).linear());
    Eigen::Quaternion<double> lfoot_ori_act(robot->getBodyNodeCoMIsometry(DracoBodyNode::lFootCenter).linear());

    rfoot_pos_des[0] = rfoot_ori_act.w();
    rfoot_pos_des[1] = rfoot_ori_act.x();
    rfoot_pos_des[2] = rfoot_ori_act.y();
    rfoot_pos_des[3] = rfoot_ori_act.z();
    rfoot_pos_des.tail(3) = robot->getBodyNodeCoMIsometry(DracoBodyNode::rFootCenter).translation();
    rfoot_center_rz_xyz_task->updateTask(rfoot_pos_des, foot_vel_des, foot_acc_des);

    lfoot_pos_des[0] = lfoot_ori_act.w();
    lfoot_pos_des[1] = lfoot_ori_act.x();
    lfoot_pos_des[2] = lfoot_ori_act.y();
    lfoot_pos_des[3] = lfoot_ori_act.z();
    lfoot_pos_des.tail(3) = robot->getBodyNodeCoMIsometry(DracoBodyNode::lFootCenter).translation();
    lfoot_center_rz_xyz_task->updateTask(lfoot_pos_des, foot_vel_des, foot_acc_des);

    // Joint Position Task
    Eigen::VectorXd jpos_des = 0.95*robot->getQ().tail(Draco::n_adof);
    Eigen::VectorXd jvel_des(Draco::n_adof);  jvel_des.setZero();
    Eigen::VectorXd jacc_des(Draco::n_adof);  jacc_des.setZero();
    total_joint_task->updateTask(jpos_des, jvel_des, jacc_des);

    // myUtils::pretty_print(body_pos_des, std::cout, "body_pos_des");
    // myUtils::pretty_print(rfoot_pos_des, std::cout, "rfoot_pos_des");
    // myUtils::pretty_print(lfoot_pos_des, std::cout, "lfoot_pos_des");
    // myUtils::pretty_print(jpos_des, std::cout, "jpos_des");

    // Create the contacts
    ContactSpec* rfoot_front_contact = new PointContactSpec(robot, DracoBodyNode::rFootFront, 0.7);
    ContactSpec* rfoot_back_contact = new PointContactSpec(robot, DracoBodyNode::rFootBack, 0.7);
    ContactSpec* lfoot_front_contact = new PointContactSpec(robot, DracoBodyNode::lFootFront, 0.7);
    ContactSpec* lfoot_back_contact = new PointContactSpec(robot, DracoBodyNode::lFootBack, 0.7);

    contact_list.push_back(rfoot_front_contact);
    contact_list.push_back(rfoot_back_contact);
    contact_list.push_back(lfoot_front_contact);
    contact_list.push_back(lfoot_back_contact);

    // ((PointContactSpec*)rfoot_front_contact)->setMaxFz(100.0);

    rfoot_front_contact->updateContactSpec();
    rfoot_back_contact->updateContactSpec();
    lfoot_front_contact->updateContactSpec();
    lfoot_back_contact->updateContactSpec();

    // Update dynamics
    Eigen::MatrixXd A = robot->getMassMatrix();
    Eigen::MatrixXd Ainv = robot->getInvMassMatrix();
    Eigen::VectorXd grav = robot->getGravity();
    Eigen::MatrixXd coriolis = robot->getCoriolis();

    // myUtils::pretty_print(A, std::cout, "A");

    // Containers
    Eigen::VectorXd tau_cmd(robot->getNumDofs() - robot->getNumVirtualDofs()); // Torque Command output from IHBC
    Eigen::VectorXd qddot_cmd(robot->getNumDofs() - robot->getNumVirtualDofs()); // Joint Acceleration Command output from IHBC

    int contact_dim_size = 0;
    for(int i = 0; i < contact_list.size(); i++){
        contact_dim_size += contact_list[i]->getDim();
    }
    ASSERT_EQ(contact_dim_size, 12);

    Eigen::VectorXd Fd(contact_dim_size); Fd.setZero();

    // Set desired forces based on the MPC output
    Fd = Fd_current;

    // Initialize QP weights
    Eigen::VectorXd w_task_heirarchy(task_list.size());  // Vector of task priority weighs

    // Contact Tasks have weight 1.0
    // Pose Tasks have weight 1e-4
    // Joint Posture Tasks have weight 1e-6
    w_task_heirarchy[0] = 1e-4; // COM
    w_task_heirarchy[1] = 1e-4; // Body Ori
    w_task_heirarchy[2] = 1.0; // rfoot
    w_task_heirarchy[3] = 1.0; // lfoot
    w_task_heirarchy[4] = 1e-6; // joint

    // When Fd is nonzero, we need to make the contact weight large if we want to trust the output of the
    double w_contact_weight = 1e-2/(robot->getRobotMass()*9.81);

    // Regularization terms should always be the lowest cost.
    double lambda_qddot = 1e-16;
    double lambda_Fr = 1e-16;

    // Enable Torque Limits
    ihwbc->enableTorqueLimits(true);
    double tau_lim = 100.0;
    Eigen::VectorXd tau_min = -tau_lim*Eigen::VectorXd::Ones(Draco::n_adof);
    Eigen::VectorXd tau_max = tau_lim*Eigen::VectorXd::Ones(Draco::n_adof);
    ihwbc->setTorqueLimits(tau_min, tau_max);

    // Set QP weights
    ihwbc->setQPWeights(w_task_heirarchy, w_contact_weight);
    ihwbc->setRegularizationTerms(lambda_qddot, lambda_Fr);

    // Update and solve QP
    ihwbc->updateSetting(A, Ainv, coriolis, grav);
    ihwbc->solve(task_list, contact_list, Fd, tau_cmd, qddot_cmd);

    // QP dec variable results
    Eigen::VectorXd qddot_res;
    Eigen::VectorXd Fr_res;

    ihwbc->getQddotResult(qddot_res);
    ihwbc->getFrResult(Fr_res);

    int idx_offset = 0;
    double total_Fz = 0;
    idx_offset = 0;
    for(int i = 0; i < contact_list.size(); i++){
        total_Fz += Fr_res[idx_offset + contact_list[i]->getFzIndex()];
        idx_offset += contact_list[i]->getDim();
    }

    myUtils::pretty_print(Fd, std::cout, "Fd");
    myUtils::pretty_print(tau_cmd, std::cout, "tau_cmd");
    myUtils::pretty_print(qddot_cmd, std::cout, "qddot_cmd");
    myUtils::pretty_print(qddot_res, std::cout, "qddot_res");
    myUtils::pretty_print(Fr_res, std::cout, "Fr_res");
    std::cout << "robot mass = " << robot->getRobotMass() << std::endl;
    std::cout << "robot mass*grav = " << robot->getRobotMass()*9.81 << std::endl;
    std::cout << "total_Fz:" << total_Fz << std::endl;

    // Assert that the computed torques are less than the set limit
    for(int i = 0; i < tau_cmd.size(); i++){
       ASSERT_LE(std::fabs(tau_cmd[i]), (tau_lim+0.001));
    }

}


#include <iostream>
#include <cassert>
#include <vector>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/successive_shortest_path_nonnegative_weights.hpp>
#include <boost/graph/find_flow_cost.hpp>

typedef boost::adjacency_list_traits<boost::vecS, boost::vecS, boost::directedS> GraphTraits;
typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS, boost::no_property,
                              boost::property<boost::edge_capacity_t, long,
                                              boost::property<boost::edge_residual_capacity_t, long,
                                                              boost::property<boost::edge_reverse_t, GraphTraits::edge_descriptor,
                                                                              boost::property<boost::edge_weight_t, long>>>>>
    Graph;

const int debug_level = 0;

#define DEBUG(min_level, x)      \
  if (debug_level >= min_level)  \
  {                              \
    std::cerr << x << std::endl; \
  }

class EdgeAdder
{
  Graph &G;

public:
  explicit EdgeAdder(Graph &G) : G(G) {}
  void add_edge(int from, int to, long capacity, long cost)
  {
    auto c_map = boost::get(boost::edge_capacity, G);
    auto r_map = boost::get(boost::edge_reverse, G);
    auto w_map = boost::get(boost::edge_weight, G);
    const Graph::edge_descriptor e = boost::add_edge(from, to, G).first;
    const Graph::edge_descriptor rev_e = boost::add_edge(to, from, G).first;
    c_map[e] = capacity;
    c_map[rev_e] = 0; // reverse edge has no capacity!
    r_map[e] = rev_e;
    r_map[rev_e] = e;
    w_map[e] = cost;
    w_map[rev_e] = -cost;
  }
};

const long max_priority = 1 << 7;

struct Mission
{
  int x, y, q;
};

void testcase()
{
  int n, m, l;
  std::cin >> n >> m >> l;
  assert(n >= 2 && n <= 3e2 && m >= 1 && m <= 1e5 && l >= 1 && l <= 1e2);

  std::vector<Mission> missions(m);
  for (Mission &mission : missions)
  {
    std::cin >> mission.x >> mission.y >> mission.q;
    assert(mission.x >= 0 && mission.x < mission.y && mission.y <= n - 1 && mission.q >= 1 && mission.q <= max_priority);
  }

  int next_free_node = 0;
  const int node_source = next_free_node++;
  const auto get_node_for_stop = [next_free_node, n](int stop) {
    assert(stop >= 0 && stop < n);
    return next_free_node + stop;
  };
  next_free_node += n;
  const int node_sink = get_node_for_stop(n - 1);
  const auto get_node_for_mission = [next_free_node, m](int mission) {
    assert(mission >= 0 && mission < m);
    return next_free_node + mission;
  };
  next_free_node += m;
  const int num_nodes = next_free_node;

  Graph G(num_nodes);
  EdgeAdder adder(G);

  adder.add_edge(node_source, get_node_for_stop(0), l, 0);

  for (int i = 1; i < n; i++)
  {
    adder.add_edge(get_node_for_stop(i - 1), get_node_for_stop(i), l, max_priority);
  }

  for (int i = 0; i < m; i++)
  {
    const Mission &mission = missions.at(i);
    adder.add_edge(get_node_for_stop(mission.x), get_node_for_mission(i), 1, max_priority * (mission.y - mission.x) - mission.q);
    adder.add_edge(get_node_for_mission(i), get_node_for_stop(mission.y), 1, 0);
  }

  boost::successive_shortest_path_nonnegative_weights(G, node_source, node_sink);
  const int priority_sum = -(boost::find_flow_cost(G) - l * (n - 1) * max_priority);
  DEBUG(1, "priority_sum " << priority_sum);
  assert(priority_sum > 0);
  std::cout << priority_sum << "\n";
}

int main()
{
  std::ios_base::sync_with_stdio(false);

  int t;
  std::cin >> t;
  for (int i = 0; i < t; i++)
  {
    testcase();
  }

  return 0;
}


#include <armadillo>
#include <boost/program_options.hpp>
#include <json.hpp>
#include <HSMM.hpp>
#include <ProMPs_emission.hpp>

using namespace arma;
using namespace hsmm;
using namespace robotics;
using namespace std;
namespace po = boost::program_options;

mat fieldToMat(int njoints, field<mat> &samples) {
    mat ret(njoints, samples.n_elem);
    for(int i = 0; i < samples.n_elem; i++)
        ret.col(i) = samples(i);
    return ret;
}

field<mat> matToField(const mat& obs) {
    field<mat> ret(obs.n_cols);
    for(int i = 0; i < obs.n_cols; i++)
        ret(i) = obs.col(i);
    return ret;
}

int main(int argc, char *argv[]) {
    po::options_description desc("Options");
    desc.add_options()
        ("help,h", "Produce help message")
        ("params,p", po::value<string>(), "Path to the input promp hsmm params")
        ("input,i", po::value<string>(), "Path to input obs. (optional)")
        ("upto,u", po::value<int>()->default_value(-1),
                "The model will condition on this number of observations. "
                "All of them by default.")
        ("nsamples,n", po::value<int>()->default_value(1), "Number of samples "
                "to generate after conditioning on the input observations")
        ("ntimeseries,t", po::value<int>()->default_value(1), "Number of i.i.d."
                " time series to generate conditioned on the same obs")
        ("output,o", po::value<string>(), "Filename of the output samples")
        ("polybasisfun", po::value<int>()->default_value(2), "Order of the "
                "poly basis functions")
        ("rbfbasisfun", po::value<int>()->default_value(3), "Number of radial"
                " basis functions to use between 0 and 1. 0 and 1 are removed.")
        ("ms", po::value<string>(), "File name where a "
                "matrix containing the state marginals will be stored.")
        ("mr", po::value<string>(), "File name where a "
                "matrix containing the run length marginals will be stored.")
        ("md", po::value<string>(), "File name where a "
                "matrix containing the duration marginals will be stored.")
        ("savefiletype", po::value<string>()->default_value("arma_binary"),
                "File type to save the matrices after inference");
    vector<string> required_fields = {"params"};
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);
    if (vm.count("help")) {
        cout << desc << endl;
        return 0;
    }
    for(auto s: required_fields) {
        if (!vm.count(s)) {
            cerr << "Error: You must provide the argument: " << s << endl;
            return 1;
        }
    }
    // Reading the current parameters.
    string input_filename = vm["params"].as<string>();
    std::ifstream current_params_stream(input_filename);
    nlohmann::json current_params;
    current_params_stream >> current_params;
    int nstates = current_params["nstates"];
    int nduration = current_params["ndurations"];
    int njoints = current_params["emission_params"][0]["num_joints"];
    int min_duration = current_params["min_duration"];

    // Setting a combination of polynomial and rbf basis functions.
    auto poly = make_shared<ScalarPolyBasis>(vm["polybasisfun"].as<int>());
    auto comb = shared_ptr<ScalarCombBasis>(new ScalarCombBasis({poly}));
    int nrbf = vm["rbfbasisfun"].as<int>();
    if (nrbf > 0) {
        vec centers = linspace<vec>(0, 1.0, nrbf + 2);
        centers = centers.subvec(1, centers.n_elem - 2);
        auto rbf = shared_ptr<ScalarGaussBasis>(new ScalarGaussBasis(centers,
                    0.25));
        comb = shared_ptr<ScalarCombBasis>(new ScalarCombBasis({rbf, poly}));
    }
    int n_basis_functions = comb->dim();

    // Instantiating as many ProMPs as hidden states.
    vector<FullProMP> promps;
    for(int i = 0; i < nstates; i++) {
        vec mu_w(n_basis_functions * njoints);
        mu_w.randn();
        mat Sigma_w = eye<mat>(n_basis_functions * njoints,
                    n_basis_functions * njoints);
        mat Sigma_y = 0.001 * eye<mat>(njoints, njoints);
        ProMP promp(mu_w, Sigma_w, Sigma_y);
        FullProMP poly(comb, promp, njoints);
        promps.push_back(poly);
    }

    mat dummy_transition(nstates, nstates, fill::eye);
    vec dummy_pi(nstates, fill::eye);
    mat dummy_duration(nstates, nduration, fill::eye);

    // Creating the ProMP emission.
    shared_ptr<ProMPsEmission> ptr_emission(new ProMPsEmission(promps));
    OnlineHSMM online_promp_hsmm(std::static_pointer_cast<
            AbstractEmissionOnlineSetting>(ptr_emission), dummy_transition,
            dummy_pi, dummy_duration,
            min_duration);
    online_promp_hsmm.from_stream(current_params);
    online_promp_hsmm.debug_ = true;
    mat obs;
    if (vm.count("input")) {
        obs.load(vm["input"].as<string>(), raw_ascii);
        assert(obs.n_rows == njoints);
    }
    mat obs_for_cond(obs);
    int upto = vm["upto"].as<int>();
    if (!obs.is_empty() && upto >= 0)
        obs_for_cond = obs.cols(0, upto - 1);

    mat state_marginals_over_time(nstates, obs_for_cond.n_cols);
    mat runlength_marginals_over_time(min_duration + nduration,
            obs_for_cond.n_cols);
    mat duration_marginals_over_time(nduration, obs_for_cond.n_cols);

    for(int c = 0; c < obs_for_cond.n_cols; c++) {
        online_promp_hsmm.addNewObservation(obs_for_cond.col(c));
        if (vm.count("ms")) {
            vec s_marginal = online_promp_hsmm.getStateMarginal();
            state_marginals_over_time.col(c) = s_marginal;
        }
        if (vm.count("mr")) {
            vec r_marginal = online_promp_hsmm.getRunlengthMarginal();
            runlength_marginals_over_time.col(c) = r_marginal;
        }
        if (vm.count("md")) {
            vec d_marginal = online_promp_hsmm.getDurationMarginal();
            duration_marginals_over_time.col(c) = d_marginal;
        }
    }

    online_promp_hsmm.printTopKFromPosterior(15);

    int nsamples = vm["nsamples"].as<int>();

    for(int j = 0; j < vm["ntimeseries"].as<int>(); j++) {
        field<mat> fsamples = online_promp_hsmm.sampleNextObservations(nsamples);
        mat samples(njoints, fsamples.n_elem);
        for(int c = 0; c < fsamples.n_elem; c++)
            samples.col(c) = fsamples(c);
        mat whole_ts = join_horiz(obs_for_cond, samples);
        if (vm.count("output")) {
            string filename = vm["output"].as<string>() + "." + to_string(j);
            whole_ts.save(filename, raw_ascii);
        }
    }

    // Evaluation the likelihood of the observed data
    field<mat> fobs = matToField(obs_for_cond);
    field<field<mat>> field_obs = {fobs};
    cout << "loglikelihood: " << online_promp_hsmm.loglikelihood(
            field_obs) << endl;

    // Saving the marginals if required.
    auto file_type = vm["savefiletype"].as<string>().compare(
            "arma_binary") == 0 ? arma_binary : raw_ascii;
    if (vm.count("ms"))
        state_marginals_over_time.save(vm["ms"].as<string>(), file_type);
    if (vm.count("mr"))
        runlength_marginals_over_time.save(vm["mr"].as<string>(), file_type);
    if (vm.count("md"))
        duration_marginals_over_time.save(vm["md"].as<string>(), file_type);
    return 0;
}


// License: Apache 2.0. See LICENSE file in root directory.
// Copyright(c) 2019 Intel Corporation. All Rights Reserved.
#include <iostream>
#include <iomanip>
#include <cstring>
#include <math.h>
#include "../include/pose_t265.h"
#include <Eigen/EulerAngles>

using std::cout;
using std::endl;

/* Constructor for CameraT265 class */
CameraT265::CameraT265()
{
  _gx_cam    = Eigen::Vector3d::Zero();
  _gxd_cam   = Eigen::Vector3d::Zero();
  _gxdd_cam  = Eigen::Vector3d::Zero();
  _gq_cam    = Eigen::Vector4d::Zero();
  _gqd_cam   = Eigen::Vector3d::Zero();
  _gqdd_cam  = Eigen::Vector3d::Zero();

  _Cx_cam    = Eigen::Vector3d::Zero();
  _Cxd_cam   = Eigen::Vector3d::Zero();
  _Cxdd_cam  = Eigen::Vector3d::Zero();
  _Cq_cam    = Eigen::Quaterniond::Identity();
  _Cqd_cam   = Eigen::Vector3d::Zero();
  _Cqdd_cam  = Eigen::Vector3d::Zero();

  _th_cam = 0.0;
  _prev_th_cam = 0.0;
  _th_offset = 0.0;

  _mx = -0.210352;
  _my = 0.0;
  _mz = 0.0;
  _Mx_cam << _mx, _my, _mz;
}

/* Initialize the Intel T265 Camera */
int CameraT265::init()
{
  try
  {
      // Add pose stream
      cfg.enable_stream(RS2_STREAM_POSE, RS2_FORMAT_6DOF);
      // Start pipeline with chosen configuration
      pipe.start(cfg);

      void rs2_set_option(const rs2_sensor* sensor, rs2_option option, float value, rs2_error** error);
      return 1;
  }
  catch (const rs2::error & e)
  {
      std::cerr << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args() << "):\n    " << e.what() << std::endl;
      return 0;
  }
  catch (const std::exception& e)
  {
      std::cerr << e.what() << std::endl;
      return 0;
  }
}

/* Update the linear and angular position measurements of the camera */
int CameraT265::updatePose()
{
  try
  {
      /*************************************** READ CAMERA MEASUREMENTS *******************************************/
      // Wait for the next set of frames from the camera
      auto frames = pipe.wait_for_frames();
      // Get a frame from the pose stream
      auto f = frames.first_or_default(RS2_STREAM_POSE);
      // // Cast the frame to pose_frame and get its data
      auto pose_data = f.as<rs2::pose_frame>().get_pose_data();

      // Camera position measurements in global reference frame, N, with respect to initial camera frame origin, C0
      _Cx_cam << pose_data.translation.z, pose_data.translation.x, pose_data.translation.y;

      _Cxd_cam << pose_data.velocity.z, pose_data.velocity.x, pose_data.velocity.y;

      _Cxdd_cam << pose_data.acceleration.z, pose_data.acceleration.x, pose_data.acceleration.y;

      // Camera orientation measurements in initial camera frame, C
      _Cq_cam = Eigen::Quaterniond(pose_data.rotation.w, pose_data.rotation.x, pose_data.rotation.y,
                pose_data.rotation.z);

      _Cqd_cam << pose_data.angular_velocity.x, pose_data.angular_velocity.y, pose_data.angular_velocity.z;

      _Cqdd_cam << pose_data.angular_acceleration.x, pose_data.angular_acceleration.y, pose_data.angular_acceleration.z;

      /************************* CONVERT CAMERA MEASUREMENTS TO GLOBAL FRAME ******************************/
      // Rotation from the initial camera frame, C, to the world frame
      Eigen::Matrix3d R_C2World;
      R_C2World << 0, 0, 1, 1, 0, 0, 0, 1, 0;
      Eigen::Quaterniond _quat_C2World = Eigen::Quaterniond(R_C2World) * _Cq_cam;

      _gqd_cam  = R_C2World * _Cqd_cam;  // TODO: CHECK IF THIS IS CORRECT
      _gqdd_cam = R_C2World * _Cqdd_cam; // TODO: CHECK IF THIS IS CORRECT

      /*************************************** ESTIMATE THETA *******************************************/
      // z-axis of the camera expressed in the world frame
      Eigen::Vector3d cam_z_axis = _quat_C2World * Eigen::Vector3d::UnitZ();

      double x1 = 1.0;
      double y1 = 0.0;
      double x2 = cam_z_axis(0);
      double y2 = cam_z_axis(1);

      double dot = x1*x2 + y1*y2;              // dot product
      double det = x1*y2 - y1*x2;              // determinant
      _th_cam = atan2(det, dot) + _th_offset;  // atan2(y, x) or atan2(sin, cos)

      // Calculate appropriate theta offset to deal with wrap around at PI and -PI
      // -3.0 value was chosen as a large enough jump to detec wrap around
      if ((_th_cam - _prev_th_cam) < -3.0) {
        _th_offset += 2.0 * M_PI;
        _th_cam += 2.0 * M_PI;
      } else if ((_th_cam - _prev_th_cam) > 3.0) {
        _th_offset += -2.0 * M_PI;
        _th_cam += -2.0 * M_PI;
      }
      _prev_th_cam = _th_cam;

      // Rotation from the current camera frame to the initial camera frame, C
      _Cq_cam.normalize();
      Eigen::Matrix3d R_cam2C = (_Cq_cam).toRotationMatrix();

      // Rotation from the mobile base frame, M, to the world frame, N
      Eigen::Matrix3d R_World2M;
      R_World2M << cos(_th_cam), sin(_th_cam), 0, -sin(_th_cam), cos(_th_cam), 0, 0, 0, 1;

      /*************************************** ESTIMATE POSITION *******************************************/
      // Position of the camera with respect to C0 in world frame, N
      Eigen::Vector3d p_cam_C0_N = _Cx_cam;

      // Position of the camera frame origin, C0, with respect to the world frame origin, N0, in the world frame, N
      Eigen::Vector3d p_C0_N0_N = Eigen::Vector3d(_mx, _my, _mz);

      // Position of the camera with respect to the world frame origin, N0, in the world frame, N
      Eigen::Vector3d p_cam_N0_N = p_cam_C0_N + p_C0_N0_N;

      // Position of the camera with respect to the mobile base origin in the mobile base frame, M
      Eigen::Vector3d p_cam_M0_M = Eigen::Vector3d(_mx, _my, _mz);

      // Position of the camera with respect to the mobile base origin in the world frame, N
      Eigen::Vector3d p_cam_M0_N = R_World2M.transpose() * p_cam_M0_M;

      // Position of the mobile base origin with respect to the world frame origin, N0, in the world frame
      Eigen::Vector3d p_M0_N0_N = p_cam_N0_N - p_cam_M0_N;

      _gx_cam =  p_M0_N0_N;

      /*************************************** ESTIMATE VELOCITY *******************************************/
      // Velocity of the camera in world frame, N
      _gxd_cam = _Cxd_cam;

      /*************************************** ESTIMATE ACCELERATION *******************************************/
      // Acceleration of the camera in world frame, N
      _gxdd_cam = _Cxdd_cam;

      return 1;
  }
  catch (const rs2::error & e)
  {
      std::cerr << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args() << "):\n    " << e.what() << std::endl;
      return 0;
  }
  catch (const std::exception& e)
  {
      std::cerr << e.what() << std::endl;
      return 0;
  }
}

/* Get the camera's estimate of the position of the mobile base operational point */
Eigen::Vector3d CameraT265::getCameraPosition() const
{
  return _gx_cam;
}

/* Get the camera's estimate of the velocity of the mobile base operational point */
Eigen::Vector3d CameraT265::getCameraVelocity() const
{
  return _gxd_cam;
}

/* Get the camera's estimate of the acceleration of the mobile base operational point */
Eigen::Vector3d CameraT265::getCameraAcceleration() const
{
  return _gxdd_cam;
}

/* Get the camera's estimate of the angular position of the mobile base */
Eigen::Vector4d CameraT265::getCameraAngularPosition() const
{
  return _gq_cam;
}

/* Get the camera's estimate of the angular velocity of the mobile base */
Eigen::Vector3d CameraT265::getCameraAngularVelocity() const
{
  return _gqd_cam;
}

/* Get the camera's estimate of the angular acceleration of the mobile base */
Eigen::Vector3d CameraT265::getCameraAngularAcceleration() const
{
  return _gqdd_cam;
}

/* Get the camera's estimate of theta */
float CameraT265::getThCam() const
{
  return _th_cam;
}