/*
 Copyright (C) 2018 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include <boost/test/unit_test.hpp>
#include <oret/toplevelfixture.hpp>

#include <boost/make_shared.hpp>

#include <ql/currencies/america.hpp>
#include <ql/math/interpolations/linearinterpolation.hpp>
#include <ql/termstructures/yield/flatforward.hpp>

#include <qle/termstructures/pricecurve.hpp>

#include <ored/marketdata/marketimpl.hpp>
#include <ored/portfolio/builders/commodityforward.hpp>
#include <ored/portfolio/commodityforward.hpp>
#include <ored/portfolio/portfolio.hpp>

using namespace std;
using namespace boost::unit_test_framework;
using namespace QuantLib;
using namespace QuantExt;
using namespace ore::data;

namespace {

// testTolerance for Real comparison
Real testTolerance = 1e-10;

class TestMarket : public MarketImpl {
public:
    TestMarket() {
        // Reference date and common day counter
        asof_ = Date(19, Feb, 2018);
        Actual365Fixed dayCounter;

        // Add USD discount curve, discount = 1.0 everywhere
        Handle<YieldTermStructure> discount(boost::make_shared<FlatForward>(asof_, 0.0, dayCounter));
        yieldCurves_[make_tuple(Market::defaultConfiguration, YieldCurveType::Discount, "USD")] = discount;

        // Add GOLD_USD price curve
        vector<Date> dates = {asof_, Date(19, Feb, 2019)};
        vector<Real> prices = {1346.0, 1348.0};
        Handle<PriceTermStructure> priceCurve(
            boost::make_shared<InterpolatedPriceCurve<Linear>>(asof_, dates, prices, dayCounter, USDCurrency()));
        Handle<CommodityIndex> commIdx(boost::make_shared<CommoditySpotIndex>("GOLD_USD", NullCalendar(), priceCurve));
        commodityIndices_[make_pair(Market::defaultConfiguration, "GOLD_USD")] = commIdx;
    }
};

} // namespace

BOOST_FIXTURE_TEST_SUITE(OREDataTestSuite, ore::test::TopLevelFixture)

BOOST_AUTO_TEST_SUITE(CommodityForwardTests)

BOOST_AUTO_TEST_CASE(testCommodityForwardTradeBuilding) {

    BOOST_TEST_MESSAGE("Testing commodity forward trade building");

    // Create market
    boost::shared_ptr<Market> market = boost::make_shared<TestMarket>();
    Settings::instance().evaluationDate() = market->asofDate();

    // Create engine factory
    boost::shared_ptr<EngineData> engineData = boost::make_shared<EngineData>();
    engineData->model("CommodityForward") = "DiscountedCashflows";
    engineData->engine("CommodityForward") = "DiscountingCommodityForwardEngine";
    boost::shared_ptr<EngineFactory> engineFactory = boost::make_shared<EngineFactory>(engineData, market);

    // Base commodity values
    string position = "Long";
    string commodityName = "GOLD_USD";
    string currency = "USD";
    Real quantity = 100.0;
    string maturity = "2019-02-19";
    Real strike = 1340.0;

    // Test the building of a commodity forward doesn't throw
    Envelope envelope;
    boost::shared_ptr<ore::data::CommodityForward> forward = boost::make_shared<ore::data::CommodityForward>(
        envelope, position, commodityName, currency, quantity, maturity, strike);
    BOOST_CHECK_NO_THROW(forward->build(engineFactory));

    // Check the instrument was built as expected
    boost::shared_ptr<Instrument> qlInstrument = forward->instrument()->qlInstrument();
    boost::shared_ptr<QuantExt::CommodityForward> commodityForward =
        boost::dynamic_pointer_cast<QuantExt::CommodityForward>(qlInstrument);
    BOOST_CHECK(commodityForward);
    BOOST_CHECK_EQUAL(commodityForward->position(), Position::Type::Long);
    BOOST_CHECK_EQUAL(commodityForward->index()->name(), "COMM-GOLD_USD");
    BOOST_CHECK_EQUAL(commodityForward->currency(), USDCurrency());
    BOOST_CHECK_CLOSE(commodityForward->quantity(), 100.0, testTolerance);
    BOOST_CHECK_EQUAL(commodityForward->maturityDate(), Date(19, Feb, 2019));
    BOOST_CHECK_CLOSE(commodityForward->strike(), 1340.0, testTolerance);

    // Check the price (simple because DF = 1.0, 100 * (1348 - 1340))
    BOOST_CHECK_CLOSE(commodityForward->NPV(), 800.0, testTolerance);

    // Check short
    forward = boost::make_shared<ore::data::CommodityForward>(envelope, "Short", commodityName, currency, quantity,
                                                              maturity, strike);
    BOOST_CHECK_NO_THROW(forward->build(engineFactory));
    qlInstrument = forward->instrument()->qlInstrument();
    commodityForward = boost::dynamic_pointer_cast<QuantExt::CommodityForward>(qlInstrument);
    BOOST_CHECK(commodityForward);
    BOOST_CHECK_EQUAL(commodityForward->position(), Position::Type::Short);
    BOOST_CHECK_CLOSE(commodityForward->NPV(), -800.0, testTolerance);

    // Check that negative quantity throws an error
    forward = boost::make_shared<ore::data::CommodityForward>(envelope, position, commodityName, currency, -quantity,
                                                              maturity, strike);
    BOOST_CHECK_THROW(forward->build(engineFactory), Error);

    // Check that negative strike throws an error
    forward = boost::make_shared<ore::data::CommodityForward>(envelope, position, commodityName, currency, quantity,
                                                              maturity, -strike);
    BOOST_CHECK_THROW(forward->build(engineFactory), Error);

    // Check that build fails when commodity name does not match that in the market
    forward = boost::make_shared<ore::data::CommodityForward>(envelope, position, "GOLD", currency, quantity, maturity,
                                                              strike);
    BOOST_CHECK_THROW(forward->build(engineFactory), Error);
}

BOOST_AUTO_TEST_CASE(testCommodityForwardFromXml) {

    BOOST_TEST_MESSAGE("Testing parsing of commodity forward trade from XML");

    // Create an XML string representation of the trade
    string tradeXml;
    tradeXml.append("<Portfolio>");
    tradeXml.append("  <Trade id=\"CommodityForward_WTI_Oct_21\">");
    tradeXml.append("  <TradeType>CommodityForward</TradeType>");
    tradeXml.append("  <Envelope>");
    tradeXml.append("    <CounterParty>CPTY_A</CounterParty>");
    tradeXml.append("    <NettingSetId>CPTY_A</NettingSetId>");
    tradeXml.append("    <AdditionalFields/>");
    tradeXml.append("  </Envelope>");
    tradeXml.append("  <CommodityForwardData>");
    tradeXml.append("    <Position>Short</Position>");
    tradeXml.append("    <Maturity>2021-10-31</Maturity>");
    tradeXml.append("    <Name>COMDTY_WTI_USD</Name>");
    tradeXml.append("    <Currency>USD</Currency>");
    tradeXml.append("    <Strike>49.75</Strike>");
    tradeXml.append("    <Quantity>500000</Quantity>");
    tradeXml.append("  </CommodityForwardData>");
    tradeXml.append("  </Trade>");
    tradeXml.append("</Portfolio>");

    // Load portfolio from XML string
    Portfolio portfolio;
    portfolio.loadFromXMLString(tradeXml);

    // Extract CommodityForward trade from portfolio
    boost::shared_ptr<Trade> trade = portfolio.trades()[0];
    boost::shared_ptr<ore::data::CommodityForward> commodityForward =
        boost::dynamic_pointer_cast<ore::data::CommodityForward>(trade);

    // Check fields after checking that the cast was successful
    BOOST_CHECK(commodityForward);
    BOOST_CHECK_EQUAL(commodityForward->tradeType(), "CommodityForward");
    BOOST_CHECK_EQUAL(commodityForward->id(), "CommodityForward_WTI_Oct_21");
    BOOST_CHECK_EQUAL(commodityForward->position(), "Short");
    BOOST_CHECK_EQUAL(commodityForward->maturityDate(), "2021-10-31");
    BOOST_CHECK_EQUAL(commodityForward->commodityName(), "COMDTY_WTI_USD");
    BOOST_CHECK_EQUAL(commodityForward->currency(), "USD");
    BOOST_CHECK_CLOSE(commodityForward->strike(), 49.75, testTolerance);
    BOOST_CHECK_CLOSE(commodityForward->quantity(), 500000.0, testTolerance);
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE_END()


#if defined(WITH_CPLEX) || defined(WITH_GUROBI)

#include <boost/python.hpp>
#include <boost/python/module.hpp>
#include <opengm/python/opengmpython.hxx>
#include <opengm/python/converter.hxx>
#include <opengm/python/numpyview.hxx>

#include <opengm/inference/icm.hxx>
#include <opengm/learning/struct-max-margin.hxx>

#define DefaultErrorFn DefaultErrorFn_TrwsExternal_SMM
#include "helper.hxx"

namespace bp = boost::python;
namespace op = opengm::python;
namespace ol = opengm::learning;

namespace opengm{

    template<class PARAM>
    PARAM * pyStructMaxMarginBundleParamConstructor(
        double regularizerWeight,
        op::GmValueType minEps,
        unsigned int steps,
        bool eps_from_gap = true
    ){
        PARAM * p  = new PARAM();
        p->optimizerParameter_.lambda  = regularizerWeight;
        p->optimizerParameter_.min_eps = minEps;
        p->optimizerParameter_.steps   = steps;
        if(eps_from_gap)
            p->optimizerParameter_.epsStrategy = ol::BundleOptimizer<op::GmValueType>::EpsFromGap;
        else
            p->optimizerParameter_.epsStrategy = ol::BundleOptimizer<op::GmValueType>::EpsFromChange;
        return p;
    }

    template<class DATASET, class OPTIMIZER>
    void export_struct_max_margin_bundle_learner(const std::string & clsName){
        typedef learning::StructMaxMargin<DATASET, OPTIMIZER> PyLearner;
        typedef typename PyLearner::Parameter PyLearnerParam;
        typedef typename PyLearner::DatasetType DatasetType;

        const std::string paramClsName = clsName + std::string("Parameter");

        bp::class_<PyLearnerParam>(paramClsName.c_str(), bp::init<>())
            .def("__init__", make_constructor(&pyStructMaxMarginBundleParamConstructor<PyLearnerParam> ,boost::python::default_call_policies()))
        ;

        boost::python::class_<PyLearner>( clsName.c_str(), boost::python::init<DatasetType &, const PyLearnerParam &>() )
            .def(LearnerInferenceSuite<PyLearner>())
        ;
    }

    template void
    export_struct_max_margin_bundle_learner<op::GmAdderFlexibleLossDataset, ol::BundleOptimizer<op::GmValueType> > (const std::string& className);

}

#endif


#include <string>
#include <map>
#include <time.h>
#include <vector>
#include <fstream>
#include <boost/timer.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/program_options/options_description.hpp>
#include <boost/program_options/parsers.hpp>
#include <boost/program_options/cmdline.hpp>
#include <boost/program_options/variables_map.hpp>
#include <boost/timer.hpp>
#include <boost/lexical_cast.hpp>

#include <alien/AlienExternalPackages.h>
#include <alien/AlienIFPENSolvers.h>
#include <alien/move/AlienMoveSemantic.h>

#include <alien/kernels/simple_csr/algebra/SimpleCSRLinearAlgebra.h>

#include <Tests/Environment.h>
#include <Tests/Solver.h>

int
main(int argc, char** argv)
{
  return Environment::execute(argc, argv, [&] {

    using namespace boost::program_options;
    options_description desc;
    desc.add_options()("help", "produce help")("file",
        value<std::string>()->default_value("SystemFile.txt"),
        "system imput file")("nrows", value<int>()->default_value(0), "nrow")("nx",
        value<int>()->default_value(10), "nx")("ny", value<int>()->default_value(10),
        "ny")("nbth", value<int>()->default_value(1), "number of threads")(
        "solver-package", value<std::string>()->default_value("petsc"),
        "solver package name")("solver", value<std::string>()->default_value("bicgs"),
        "solver algo name")("precond", value<std::string>()->default_value("none"),
        "preconditioner id diag ilu ddml poly")(
        "max-iter", value<int>()->default_value(1000), "max iterations")(
        "tol", value<double>()->default_value(1.e-10), "solver tolerance")(
        "niter", value<int>()->default_value(1), "nb of tests for perf measure")(
        "sym", value<int>()->default_value(1), "0->nsym, 1->sym")("kernel",
        value<std::string>()->default_value("mcgkernel"),
        "mcgsolver kernel name")("builder", value<int>()->default_value(0),
        "matrix builder type 0->direct 1->profile 2->stream")(
        "output-level", value<int>()->default_value(0), "output level");
    variables_map vm;
    store(parse_command_line(argc, argv, desc), vm);
    notify(vm);

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

    std::string filename = vm["file"].as<std::string>();
    int nrows = vm["nrows"].as<int>();
    int nx = vm["nx"].as<int>();
    int ny = vm["ny"].as<int>();
    //  int nb_threads = vm["nbth"].as<int>();
    //  bool sym = vm["sym"].as<int>() == 1;
    int builder_type = vm["builder"].as<int>();

    auto* trace_mng = Environment::traceMng();
    auto* parallel_mng = Environment::parallelMng();

    const int comm_rank = parallel_mng->commRank();
    const int comm_size = parallel_mng->commSize();

    Alien::setTraceMng(trace_mng);

    nrows = nx * ny;

    Alien::VectorDistribution vdist(nrows, parallel_mng);
    Alien::MatrixDistribution mdist(nrows, nrows, parallel_mng);

    Alien::Space space(nrows, "TestSpace");

    Alien::Move::VectorData vectorB(space, vdist);
    Alien::Move::VectorData vectorX(space, vdist);

    Alien::Move::MatrixData matrixA(space, mdist);

    int bd = 2;
    double fii = 1;
    double fij = 1;
    double diag = fii + 2 * bd * fij;
    double offdiag = fij;
    int offset = vdist.offset();
    int local_size = vdist.localSize();
    switch (builder_type) {
    case 0: {
      Alien::Move::DirectMatrixBuilder builder(
          std::move(matrixA), Alien::DirectMatrixOptions::eResetValues);
      builder.reserve(5);
      builder.allocate();
      for (int irow = offset; irow < offset + local_size; ++irow) {
        builder.setData(irow, irow, diag);
        for (int j = 1; j < bd; ++j) {
          if (irow - j >= 0)
            builder.setData(irow, irow - j, -offdiag / j);
          if (irow + j < nrows)
            builder.setData(irow, irow + j, -offdiag / j);
        }
      }
      builder.finalize();
      matrixA = builder.release();
    } break;
    case 1:
    default: {
      // DEFINE MATRIX PROFILE
      Alien::Move::MatrixProfiler profile(std::move(matrixA));
      for (int irow = offset; irow < offset + local_size; ++irow) {
        profile.addMatrixEntry(irow, irow);
        for (int j = 1; j < bd; ++j) {
          if (irow - j >= 0)
            profile.addMatrixEntry(irow, irow - j);
          if (irow + j < nrows)
            profile.addMatrixEntry(irow, irow + j);
        }
      }
      profile.allocate();
      matrixA = profile.release();

      // FILL MATRIX
      Alien::Move::ProfiledMatrixBuilder matrix(
          std::move(matrixA), Alien::ProfiledMatrixOptions::eResetValues);
      for (int irow = offset; irow < offset + local_size; ++irow) {
        matrix(irow, irow) = diag;
        for (int j = 1; j < bd; ++j) {
          if (irow - j >= 0)
            matrix(irow, irow - j) = -offdiag / j;
          if (irow + j < nrows)
            matrix(irow, irow + j) = -offdiag / j;
        }
      }
      matrix.finalize();
      matrixA = matrix.release();
    } break;
    }

    {
      Alien::Move::LocalVectorWriter v(std::move(vectorX));
      for (int i = 0; i < vdist.localSize(); ++i)
        v[i] = 1;
      vectorX = v.release();
    }
    double normx_ref = 0;
    if (!(builder_type == 0) || comm_size == 1) // le direct builder ne permet pas de
                                                // faire des produits matrix vector en
                                                // parallel
    {
      Alien::SimpleCSRLinearAlgebra alg;
      alg.mult(matrixA, vectorX, vectorB);
      double normb = alg.norm2(vectorB);
      normx_ref = alg.norm2(vectorX);
      if (comm_rank == 0) {
        std::cout << "Norme de X : " << normx_ref << std::endl;
        std::cout << "Norme de B = A * x : " << normb << std::endl;
        std::cout << "CHECK ERROR " << std::abs(normb - 30.2324) << std::endl;
      }
      assert(std::abs(normb - 30.2324) < 1.e-4);
    } else {
      Alien::Move::LocalVectorWriter v(std::move(vectorB));
      for (int i = 0; i < vdist.localSize(); ++i)
        v[i] = 1;
      vectorB = v.release();
    }

    {
      Alien::Move::LocalVectorWriter v(std::move(vectorX));
      for (int i = 0; i < vdist.localSize(); ++i)
        v[i] = 0;
      vectorX = v.release();
    }

    std::shared_ptr<Alien::ILinearSolver> solver = Environment::createSolver(vm);
    if (solver.get()) {
      solver->init();
      solver->solve(matrixA, vectorB, vectorX);
      const auto& status = solver->getStatus();
      if (status.succeeded) {
        if (comm_rank == 0) {
          std::cout << "Solver succeed   " << status.succeeded << std::endl;
          std::cout << "             residual " << status.residual << std::endl;
          std::cout << "             nb iters  " << status.iteration_count << std::endl;
          std::cout << "             error      " << status.error << std::endl;
        }

        if (!(builder_type == 0) || comm_size == 1) // le direct builder ne permet pas de
                                                    // faire des produits matrix vector en
                                                    // parallel
        {
          Alien::SimpleCSRLinearAlgebra alg;
          double normx = alg.norm2(vectorX);
          if (comm_rank == 0) {
            std::cout << "Norme de X : " << normx << std::endl;
          }

          Alien::Move::LocalVectorWriter v(std::move(vectorX));
          for (int i = 0; i < vdist.localSize(); ++i)
            v[i] -= 1.;
          vectorX = v.release();

          normx = alg.norm2(vectorX);
          double err = normx / normx_ref;
          if (comm_rank == 0) {
            std::cout << "Relative Error : " << err << std::endl;
          }
          assert(err < 1.e-10);
        }
      } else {
        if (comm_rank == 0) {
          std::cout << "Solver failed      " << status.succeeded << std::endl;
          std::cout << "             error      " << status.error << std::endl;
        }
      }
    } else
      std::cout << "No solver provided\n";

    return 0;
  });
}


// Copyright (c) 2020 INRA 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 <hayai.hpp>

#include <irritator/core.hpp>

#include <boost/ut.hpp>

#include <fmt/format.h>

#include <cstdio>

#include <fstream>

using namespace std;

struct file_output
{
    std::FILE* os = nullptr;
    std::string filename;

    file_output(const std::string_view name)
      : filename(name)
    {
        os = std::fopen(filename.c_str(), "w");
    }

    ~file_output()
    {
        if (os)
            std::fclose(os);
    }

    void operator()(const irt::observer& obs,
                    const irt::dynamics_type /*type*/,
                    const irt::time tl,
                    const irt::time t,
                    const irt::observer::status s) noexcept
    {
        switch (s) {
        case irt::observer::status::initialize:
            fmt::print(os, "t,{}\n", obs.name.c_str());
            break;

        case irt::observer::status::run:
            fmt::print(os, "{},{}\n", t, obs.msg.real[0]);
            break;

        case irt::observer::status::finalize:
            break;
        }
    }
};

struct neuron
{
    irt::dynamics_id sum;
    irt::dynamics_id integrator;
    irt::dynamics_id quantifier;
    irt::dynamics_id constant;
    irt::dynamics_id cross;
    irt::dynamics_id constant_cross;
};

struct neuron
make_neuron(irt::simulation* sim, double quantum) noexcept
{
    using namespace boost::ut;
    double tau_lif = 10;
    double Vr_lif = 0.0;
    double Vt_lif = 10.0;

    auto& sum_lif = sim->adder_2_models.alloc();
    auto& integrator_lif = sim->integrator_models.alloc();
    auto& quantifier_lif = sim->quantifier_models.alloc();
    auto& constant_lif = sim->constant_models.alloc();
    auto& constant_cross_lif = sim->constant_models.alloc();
    auto& cross_lif = sim->cross_models.alloc();

    sum_lif.default_input_coeffs[0] = -1.0 / tau_lif;
    sum_lif.default_input_coeffs[1] = 20.0 / tau_lif;

    constant_lif.default_value = 1.0;
    constant_cross_lif.default_value = Vr_lif;

    integrator_lif.default_current_value = 0.0;

    quantifier_lif.default_adapt_state = irt::quantifier::adapt_state::possible;
    quantifier_lif.default_zero_init_offset = true;
    quantifier_lif.default_step_size = quantum;
    quantifier_lif.default_past_length = 3;

    cross_lif.default_threshold = Vt_lif;

    sim->alloc(sum_lif, sim->adder_2_models.get_id(sum_lif));
    sim->alloc(integrator_lif, sim->integrator_models.get_id(integrator_lif));
    sim->alloc(quantifier_lif, sim->quantifier_models.get_id(quantifier_lif));
    sim->alloc(constant_lif, sim->constant_models.get_id(constant_lif));
    sim->alloc(cross_lif, sim->cross_models.get_id(cross_lif));
    sim->alloc(constant_cross_lif,
               sim->constant_models.get_id(constant_cross_lif));

    struct neuron neuron_model = {
        sim->adder_2_models.get_id(sum_lif),
        sim->integrator_models.get_id(integrator_lif),
        sim->quantifier_models.get_id(quantifier_lif),
        sim->constant_models.get_id(constant_lif),
        sim->cross_models.get_id(cross_lif),
        sim->constant_models.get_id(constant_cross_lif),
    };

    // Connections
    expect(sim->connect(quantifier_lif.y[0], integrator_lif.x[0]) ==
           irt::status::success);
    expect(sim->connect(sum_lif.y[0], integrator_lif.x[1]) ==
           irt::status::success);
    expect(sim->connect(cross_lif.y[0], integrator_lif.x[2]) ==
           irt::status::success);
    expect(sim->connect(cross_lif.y[0], quantifier_lif.x[0]) ==
           irt::status::success);
    expect(sim->connect(cross_lif.y[0], sum_lif.x[0]) == irt::status::success);
    expect(sim->connect(integrator_lif.y[0], cross_lif.x[0]) ==
           irt::status::success);
    expect(sim->connect(integrator_lif.y[0], cross_lif.x[2]) ==
           irt::status::success);
    expect(sim->connect(constant_cross_lif.y[0], cross_lif.x[1]) ==
           irt::status::success);
    expect(sim->connect(constant_lif.y[0], sum_lif.x[1]) ==
           irt::status::success);
    return neuron_model;
}

void
lif_benchmark(double simulation_duration, double quantum)
{
    using namespace boost::ut;

    irt::simulation sim;
    expect(irt::is_success(sim.init(2600lu, 40000lu)));

    struct neuron neuron_model = make_neuron(&sim, quantum);

    irt::time t = 0.0;
    std::string file_name = "output_lif_aqss_sd_" +
                            std::to_string(simulation_duration) + "_q_" +
                            std::to_string(quantum) + ".csv";
    file_output fo_a(file_name.c_str());
    expect(fo_a.os != nullptr);

    auto& obs_a = sim.observers.alloc("A", fo_a);
    sim.observe(sim.models.get(
                  sim.qss2_integrator_models.get(neuron_model.integrator).id),
                obs_a);

    expect(irt::status::success == sim.initialize(t));

    do {

        irt::status st = sim.run(t);
        expect(st == irt::status::success);

    } while (t < simulation_duration);
}
void
izhikevich_benchmark(double simulation_duration,
                     double quantum,
                     double a,
                     double b,
                     double c,
                     double d,
                     double I,
                     double vini)
{
    using namespace boost::ut;
    irt::simulation sim;

    expect(irt::is_success(sim.init(1000lu, 1000lu)));
    expect(sim.constant_models.can_alloc(3));
    expect(sim.adder_2_models.can_alloc(3));
    expect(sim.adder_4_models.can_alloc(1));
    expect(sim.mult_2_models.can_alloc(1));
    expect(sim.integrator_models.can_alloc(2));
    expect(sim.quantifier_models.can_alloc(2));
    expect(sim.cross_models.can_alloc(2));

    auto& constant = sim.constant_models.alloc();
    auto& constant2 = sim.constant_models.alloc();
    auto& constant3 = sim.constant_models.alloc();
    auto& sum_a = sim.adder_2_models.alloc();
    auto& sum_b = sim.adder_2_models.alloc();
    auto& sum_c = sim.adder_4_models.alloc();
    auto& sum_d = sim.adder_2_models.alloc();
    auto& product = sim.mult_2_models.alloc();
    auto& integrator_a = sim.integrator_models.alloc();
    auto& integrator_b = sim.integrator_models.alloc();
    auto& quantifier_a = sim.quantifier_models.alloc();
    auto& quantifier_b = sim.quantifier_models.alloc();
    auto& cross = sim.cross_models.alloc();
    auto& cross2 = sim.cross_models.alloc();

    double vt = 30.0;

    constant.default_value = 1.0;
    constant2.default_value = c;
    constant3.default_value = I;

    cross.default_threshold = vt;
    cross2.default_threshold = vt;

    integrator_a.default_current_value = vini;

    quantifier_a.default_adapt_state = irt::quantifier::adapt_state::possible;
    quantifier_a.default_zero_init_offset = true;
    quantifier_a.default_step_size = quantum;
    quantifier_a.default_past_length = 3;

    integrator_b.default_current_value = 0.0;

    quantifier_b.default_adapt_state = irt::quantifier::adapt_state::possible;
    quantifier_b.default_zero_init_offset = true;
    quantifier_b.default_step_size = quantum;
    quantifier_b.default_past_length = 3;

    product.default_input_coeffs[0] = 1.0;
    product.default_input_coeffs[1] = 1.0;

    sum_a.default_input_coeffs[0] = 1.0;
    sum_a.default_input_coeffs[1] = -1.0;
    sum_b.default_input_coeffs[0] = -a;
    sum_b.default_input_coeffs[1] = a * b;
    sum_c.default_input_coeffs[0] = 0.04;
    sum_c.default_input_coeffs[1] = 5.0;
    sum_c.default_input_coeffs[2] = 140.0;
    sum_c.default_input_coeffs[3] = 1.0;
    sum_d.default_input_coeffs[0] = 1.0;
    sum_d.default_input_coeffs[1] = d;

    expect(sim.models.can_alloc(14));
    expect((irt::is_success(
      sim.alloc(constant3, sim.constant_models.get_id(constant3)))) >> fatal);
    expect((irt::is_success(
      sim.alloc(constant, sim.constant_models.get_id(constant)))) >> fatal);
    expect((irt::is_success(
      sim.alloc(constant2, sim.constant_models.get_id(constant2)))) >> fatal);

    expect((
      irt::is_success(sim.alloc(sum_a, sim.adder_2_models.get_id(sum_a)))) >> fatal);
    expect((
      irt::is_success(sim.alloc(sum_b, sim.adder_2_models.get_id(sum_b)))) >> fatal);
    expect((
      irt::is_success(sim.alloc(sum_c, sim.adder_4_models.get_id(sum_c)))) >> fatal);
    expect((
      irt::is_success(sim.alloc(sum_d, sim.adder_2_models.get_id(sum_d)))) >> fatal);

    expect((
      irt::is_success(sim.alloc(product, sim.mult_2_models.get_id(product)))) >> fatal);
    expect((irt::is_success(
      sim.alloc(integrator_a, sim.integrator_models.get_id(integrator_a)))) >> fatal);
    expect((irt::is_success(
      sim.alloc(integrator_b, sim.integrator_models.get_id(integrator_b)))) >> fatal);
    expect((irt::is_success(
      sim.alloc(quantifier_a, sim.quantifier_models.get_id(quantifier_a)))) >> fatal);
    expect((irt::is_success(
      sim.alloc(quantifier_b, sim.quantifier_models.get_id(quantifier_b)))) >> fatal);
    expect((irt::is_success(sim.alloc(cross, sim.cross_models.get_id(cross)))) >> fatal);
    expect((
      irt::is_success(sim.alloc(cross2, sim.cross_models.get_id(cross2)))) >> fatal);

    expect((sim.models.size() == 14_ul) >> fatal);

    expect(sim.connect(integrator_a.y[0], cross.x[0]) == irt::status::success);
    expect(sim.connect(constant2.y[0], cross.x[1]) == irt::status::success);
    expect(sim.connect(integrator_a.y[0], cross.x[2]) == irt::status::success);

    expect(sim.connect(cross.y[0], quantifier_a.x[0]) == irt::status::success);
    expect(sim.connect(cross.y[0], product.x[0]) == irt::status::success);
    expect(sim.connect(cross.y[0], product.x[1]) == irt::status::success);
    expect(sim.connect(product.y[0], sum_c.x[0]) == irt::status::success);
    expect(sim.connect(cross.y[0], sum_c.x[1]) == irt::status::success);
    expect(sim.connect(cross.y[0], sum_b.x[1]) == irt::status::success);

    expect(sim.connect(constant.y[0], sum_c.x[2]) == irt::status::success);
    expect(sim.connect(constant3.y[0], sum_c.x[3]) == irt::status::success);

    expect(sim.connect(sum_c.y[0], sum_a.x[0]) == irt::status::success);
    expect(sim.connect(integrator_b.y[0], sum_a.x[1]) == irt::status::success);
    expect(sim.connect(cross2.y[0], sum_a.x[1]) == irt::status::success);
    expect(sim.connect(sum_a.y[0], integrator_a.x[1]) == irt::status::success);
    expect(sim.connect(cross.y[0], integrator_a.x[2]) == irt::status::success);
    expect(sim.connect(quantifier_a.y[0], integrator_a.x[0]) ==
           irt::status::success);

    expect(sim.connect(cross2.y[0], quantifier_b.x[0]) == irt::status::success);
    expect(sim.connect(cross2.y[0], sum_b.x[0]) == irt::status::success);
    expect(sim.connect(quantifier_b.y[0], integrator_b.x[0]) ==
           irt::status::success);
    expect(sim.connect(sum_b.y[0], integrator_b.x[1]) == irt::status::success);

    expect(sim.connect(cross2.y[0], integrator_b.x[2]) == irt::status::success);
    expect(sim.connect(integrator_a.y[0], cross2.x[0]) == irt::status::success);
    expect(sim.connect(integrator_b.y[0], cross2.x[2]) == irt::status::success);
    expect(sim.connect(sum_d.y[0], cross2.x[1]) == irt::status::success);
    expect(sim.connect(integrator_b.y[0], sum_d.x[0]) == irt::status::success);
    expect(sim.connect(constant.y[0], sum_d.x[1]) == irt::status::success);

    std::string file_name =
      "output_izhikevitch_aqss_a_sd_" + std::to_string(simulation_duration) +
      "_q_" + std::to_string(quantum) + "_a_" + std::to_string(a) + "_b_" +
      std::to_string(b) + "_c_" + std::to_string(c) + "_d_" +
      std::to_string(d) + ".csv";
    file_output fo_a(file_name.c_str());
    expect(fo_a.os != nullptr);

    auto& obs_a = sim.observers.alloc("A", fo_a);
    file_name = "output_izhikevitch_aqss_b_sd_" +
                std::to_string(simulation_duration) + "_q_" +
                std::to_string(quantum) + "_a_" + std::to_string(a) + "_b_" +
                std::to_string(b) + "_c_" + std::to_string(c) + "_d_" +
                std::to_string(d) + ".csv";
    file_output fo_b(file_name.c_str());
    expect(fo_b.os != nullptr);
    auto& obs_b = sim.observers.alloc("B", fo_b);

    sim.observe(sim.models.get(integrator_a.id), obs_a);
    sim.observe(sim.models.get(integrator_b.id), obs_b);

    irt::time t = 0.0;

    expect(irt::status::success == sim.initialize(t));
    expect(sim.sched.size() == 14_ul);

    do {
        irt::status st = sim.run(t);
        expect(st == irt::status::success);
    } while (t < simulation_duration);
};
BENCHMARK_P(LIF, AQSS, 10, 1, (double simulation_duration, double quantum))
{
    lif_benchmark(simulation_duration, quantum);
}
BENCHMARK_P(Izhikevich,
            AQSS,
            1,
            1,
            (double simulation_duration,
             double quantum,
             double a,
             double b,
             double c,
             double d,
             double I,
             double vini))
{
    izhikevich_benchmark(simulation_duration, quantum, a, b, c, d, I, vini);
}

BENCHMARK_P_INSTANCE(LIF, AQSS, (1000, 1e-2));
// Regular spiking (RS)
BENCHMARK_P_INSTANCE(Izhikevich,
                     AQSS,
                     (1000, 1e-2, 0.02, 0.2, -65.0, 8.0, 10.0, 0.0));
// Intrinsical bursting (IB)
BENCHMARK_P_INSTANCE(Izhikevich,
                     AQSS,
                     (1000, 1e-2, 0.02, 0.2, -55.0, 4.0, 10.0, 0.0));
// Chattering spiking (CH)
BENCHMARK_P_INSTANCE(Izhikevich,
                     AQSS,
                     (1000, 1e-2, 0.02, 0.2, -50.0, 2.0, 10.0, 0.0));
// Fast spiking (FS)
BENCHMARK_P_INSTANCE(Izhikevich,
                     AQSS,
                     (1000, 1e-2, 0.1, 0.2, -65.0, 2.0, 10.0, 0.0));
// Thalamo-Cortical (TC)
BENCHMARK_P_INSTANCE(Izhikevich,
                     AQSS,
                     (1000, 1e-2, 0.02, 0.25, -65.0, 0.05, 10.0, -87.0));
// Rezonator (RZ)
BENCHMARK_P_INSTANCE(Izhikevich,
                     AQSS,
                     (1000, 1e-2, 0.1, 0.26, -65.0, 2.0, 10.0, -63.0));
// Low-threshold spiking (LTS)
BENCHMARK_P_INSTANCE(Izhikevich,
                     AQSS,
                     (1000, 1e-2, 0.02, 0.25, -65.0, 2.0, 10.0, -63.0));
// Problematic (P)
BENCHMARK_P_INSTANCE(Izhikevich,
                     AQSS,
                     (1000, 1e-2, 0.2, 2, -56.0, -16.0, -99.0, 0.0));

int
main()
{
    hayai::ConsoleOutputter consoleOutputter;

    hayai::Benchmarker::AddOutputter(consoleOutputter);
    hayai::Benchmarker::RunAllTests();
}


#pragma once

#include <cstdlib>
#include <utility>
#include <tuple>
#include <numeric>

#include <boost/unordered_map.hpp>
#include "types.hh"
#include "zone.hh"

/*!
  @brief Generate a zone automaton from a timed automaton
  @tparam NVar the number of variable in TA

  TA to ZA adds states with BFS. Initial configuration is the initial states of ZA. The ZA contain only the states reachable from initial states.
 */
template <int NVar>
void ta2za (const TimedAutomaton<NVar> &TA,ZoneAutomaton &ZA, Zone initialZone = Zone::zero(NVar + 1))
{
  auto initialStates = TA.initialStates;
  if (!ZA.abstractedStates.empty()) {
    for (auto it = initialStates.begin(); it != initialStates.end();) {
      if (std::find(ZA.abstractedStates.begin(), ZA.abstractedStates.end(),std::pair<ZAState,Zone>{TAState(*it),initialZone}) != ZA.abstractedStates.end()) {
        it = initialStates.erase(it);
      } else {
        it++;
      }
    }
  }
  if (initialStates.empty()) {
    return;
  }

  ZA.numOfVariables = NVar;
  //! number of states of ZA
  const State numOfStatesInitial = ZA.abstractedStates.size();
  State numOfStates = numOfStatesInitial;
  if (NVar > 0) {
    initialZone.M = Bounds{*std::max_element(TA.max_constraints.begin(), TA.max_constraints.end()), true};
  } else {
    initialZone.M = Bounds(0, true);
  }

  // make initial state
  numOfStates += initialStates.size();
  ZA.edges.resize(numOfStates);
  const State numOfInitialStatesInitial = ZA.initialStates.size();
  ZA.initialStates.resize (numOfStates);
  iota (ZA.initialStates.begin() + numOfInitialStatesInitial, ZA.initialStates.end(),numOfStatesInitial);

  ZA.abstractedStates.reserve (numOfStates);
  for (const auto &conf : initialStates ) {
    ZA.abstractedStates.push_back (std::make_pair(conf,initialZone));
  }

  /*!
    @brief Current configuration of BFS

    A configuration consistes of a tuple ((conf,\alpha),conf,alpha). Remark (conf,\alpha) is not a pair but just a number representing a state of RA.
   */
  std::vector<std::tuple<RAState,TAState,Zone> > nextConf;
  nextConf.resize (initialStates.size());
  for (std::size_t i = 0; i < nextConf.size();i++) {
    nextConf[i] = std::make_tuple(i + numOfStatesInitial,
                                  ZA.abstractedStates[i+numOfStatesInitial].first,
                                  ZA.abstractedStates[i+numOfStatesInitial].second);
  }

  /*!
    @brief translater from TAState and Region to its corresponding state in RA.

    The type is like this.
    (TAState,Zone) -> RAState
  */
  while (!nextConf.empty ()) {
    std::vector<std::tuple<RAState,TAState, Zone> > currentConf = nextConf;
    nextConf.clear();
    for (const auto &conf : currentConf) {
      Zone nowZone = std::get<2>(conf);
      nowZone.elapse();
      for (const auto &edge : TA.edges.at(std::get<1>(conf))) {
        Zone nextZone = nowZone;

        for (const auto &delta : edge.guard) {
          switch (delta.odr) {
          case Constraint::Order::lt:
            nextZone.tighten(delta.x,-1,{delta.c, false});
            break;
          case Constraint::Order::le:
            nextZone.tighten(delta.x,-1,{delta.c, true});
            break;
          case Constraint::Order::gt:
            nextZone.tighten(-1,delta.x,{-delta.c, false});
            break;
          case Constraint::Order::ge:
            nextZone.tighten(-1,delta.x,{-delta.c, true});
            break;
          }
        }

        if (nextZone.isSatisfiable()) {
          for (auto x : edge.resetVars) {
            nextZone.reset(x);
          }
          nextZone.abstractize();
          nextZone.canonize();
          // nextRegion state is new
          const auto targetStateInZA = ZA.zones_in_za.find(std::make_pair(edge.target, nextZone.toTuple()));

          // targetRegionState is already added
          if (targetStateInZA != ZA.zones_in_za.end()) {
            const NFA::Edge newEdge = {std::get<0>(conf),targetStateInZA->second,edge.c};
            ZA.edges[std::get<0>(conf)].push_back (newEdge);
            ZoneAutomaton::TAEdge taEdge = {edge.source, edge.target, edge.c, edge.resetVars, edge.guard};
            ZA.edgeMap[newEdge.toTuple()] = taEdge;

          } else {
            const NFA::Edge newEdge = {std::get<0>(conf),static_cast<State>(numOfStates),edge.c};
            ZA.edges[std::get<0>(conf)].push_back (newEdge);
            ZoneAutomaton::TAEdge taEdge = {edge.source, edge.target, edge.c, edge.resetVars, edge.guard};
            ZA.edgeMap[newEdge.toTuple()] = taEdge;

            if (binary_search (TA.acceptingStates.begin(),
                               TA.acceptingStates.end (),edge.target)) {
              ZA.acceptingStates.push_back (numOfStates);
            }
            ZA.zones_in_za[std::make_pair(edge.target, nextZone.toTuple())] = numOfStates;
            ZA.abstractedStates.push_back (std::make_pair (edge.target,nextZone));
            nextConf.push_back (std::make_tuple (numOfStates,edge.target, nextZone));
            numOfStates++;
            ZA.edges.resize(numOfStates);
          }
        }
      }
    }
  }
}


/***************************************************************************************************************************
 * aruco_det_v2.cpp
 * Author: Jario
 * Update Time: 2020.12.11
 *
 * 说明: 单个二维码识别程序，可识别的二维码在Prometheus/Modules/object_detection/config/aruco_images文件夹中
 *      视野里只允许存在一个二维码 且二维码的字典类型要对应
 *      默认二维码的边长为0.2m
 *      1. 【订阅】图像话题 (默认来自web_cam)
 *         /prometheus/camera/rgb/image_raw
 *      2. 【发布】目标位置，发布话题见 Prometheus/Modules/msgs/msg/DetectionInfo.msg
 *         /prometheus/object_detection/aruco_det
 *      3. 【发布】检测结果的可视化图像话题
 *         /prometheus/camera/rgb/image_aruco_det
***************************************************************************************************************************/
#include <pthread.h>
#include <thread>
#include <chrono>
#include <numeric>
#include <boost/thread/mutex.hpp>
#include <boost/thread/shared_mutex.hpp>

#include <ros/ros.h>
#include <ros/package.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>
#include <yaml-cpp/yaml.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <geometry_msgs/PoseStamped.h>
#include <std_msgs/Bool.h>
#include <std_msgs/String.h>
#include <prometheus_msgs/DetectionInfo.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/aruco.hpp>
#include <opencv2/aruco/dictionary.hpp>
#include <opencv2/aruco/charuco.hpp>
#include <opencv2/calib3d.hpp>

#include <Eigen/Dense>
#include <opencv2/core/eigen.hpp>

// #include "message_utils.h"

using namespace std;
using namespace cv;

//【订阅】运行状态转换开关
ros::Subscriber switch_subscriber;
//【订阅】输入图像
image_transport::Subscriber image_subscriber;
//【发布】检测得到的位置与姿态信息
ros::Publisher pose_pub;
//【发布】检测结果图像
image_transport::Publisher aruco_pub;

// 使用cout打印消息
bool local_print = true;

// 相机话题中的图像同步相关变量
int frame_width, frame_height;
std_msgs::Header image_header;
cv::Mat cam_image_copy;
boost::shared_mutex mutex_image_callback;
bool image_status = false;
boost::shared_mutex mutex_image_status;

// 运行状态
// 0: 正常检测Aruco码，输出位姿
// 1: 世界坐标系标定，标定后，检测结果被转换到世界坐标系下
int run_state(0);

void switchCallback(const std_msgs::String::ConstPtr& msg)
{
    ROS_INFO("receiving [%s]", msg->data.c_str());
    if ("calibrate" == msg->data)
    {
        ROS_INFO("run_state = 1");
        run_state = 1;
    }
}

// 图像接收回调函数，接收web_cam的话题，并将图像保存在cam_image_copy中
void cameraCallback(const sensor_msgs::ImageConstPtr& msg)
{
    if (local_print) ROS_DEBUG("[ArucoDetector] USB image received.");

    cv_bridge::CvImagePtr cam_image;

    try {
        cam_image = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
        image_header = msg->header;
    } catch (cv_bridge::Exception& e) {
        if (local_print) ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }

    if (cam_image) {
        {
            boost::unique_lock<boost::shared_mutex> lockImageCallback(mutex_image_callback);
            cam_image_copy = cam_image->image.clone();
        }
        {
            boost::unique_lock<boost::shared_mutex> lockImageStatus(mutex_image_status);
            image_status = true;
        }
        frame_width = cam_image->image.size().width;
        frame_height = cam_image->image.size().height;
    }
    return;
}

// 用此函数查看是否收到图像话题
bool getImageStatus(void)
{
    boost::shared_lock<boost::shared_mutex> lock(mutex_image_status);
    return image_status;
}

static bool readCameraParameters(string filename, Mat &camMatrix, Mat &distCoeffs) {
    FileStorage fs(filename, FileStorage::READ);
    if(!fs.isOpened())
        return false;
    fs["camera_matrix"] >> camMatrix;
    fs["distortion_coefficients"] >> distCoeffs;
    return true;
}

float _vector_stdev(std::vector<float>& x)
{
    float sum, mean, accum, stdev;
    sum = std::accumulate(std::begin(x), std::end(x), 0.0);
    mean = sum / x.size();
    accum = 0.0;
    std::for_each (std::begin(x), std::end(x), [&](const float d) {
        accum += (d - mean) * (d - mean);
    });
    stdev = sqrt(accum / (x.size() - 1));
    return stdev;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "aruco_det_v2");
    ros::NodeHandle nh("~");
    image_transport::ImageTransport it(nh);
    // 更新频率为60HZ
    ros::Rate loop_rate(60);

    std::string camera_topic = "/prometheus/camera/rgb/image_raw";
    std::string camera_params_yaml;
    std::string output_topic = "/prometheus/camera/rgb/image_aruco_det";

    int dictionaryId(2);
    float targetMarkerLength(0.0207);
    float calibMarkerLength(0.0207);
    float calibSquareLength(0.0345);  // Square side length (in meters)

    if (nh.getParam("camera_topic", camera_topic)) {
        if (local_print) ROS_INFO("camera_topic is %s", camera_topic.c_str());
    } else {
        if (local_print) ROS_WARN("didn't find parameter camera_topic");
    }
    if (nh.getParam("camera_parameters", camera_params_yaml)) {
        if (local_print) ROS_INFO("camera_parameters is %s", camera_params_yaml.c_str());
    } else {
        if (local_print) ROS_WARN("didn't find camera_parameters");
    }
    if (nh.getParam("output_topic", output_topic)) {
        if (local_print) ROS_INFO("output_topic is %s", output_topic.c_str());
    } else {
        if (local_print) ROS_WARN("didn't find parameter output_topic");
    }

    if (nh.getParam("dictionary_type", dictionaryId)) {
        if (local_print) ROS_INFO("dictionary_type is %d", dictionaryId);
    } else {
        if (local_print) ROS_WARN("didn't find parameter dictionary_type");
    }
    if (nh.getParam("target_marker_length", targetMarkerLength)) {
        if (local_print) ROS_INFO("target_marker_length is %f", targetMarkerLength);
    } else {
        if (local_print) ROS_WARN("didn't find parameter target_marker_length");
    }
    if (nh.getParam("calib_marker_length", calibMarkerLength)) {
        if (local_print) ROS_INFO("calib_marker_length is %f", calibMarkerLength);
    } else {
        if (local_print) ROS_WARN("didn't find parameter calib_marker_length");
    }
    if (nh.getParam("calib_square_length", calibSquareLength)) {
        if (local_print) ROS_INFO("calib_square_length is %f", calibSquareLength);
    } else {
        if (local_print) ROS_WARN("didn't find parameter calib_square_length");
    }

    //【订阅】运行状态转换开关
    switch_subscriber = nh.subscribe("/prometheus/object_detection/aruco_navigation_switch", 1, switchCallback);
    //【订阅】输入图像
    image_subscriber = it.subscribe(camera_topic.c_str(), 1, cameraCallback);

    //【发布】检测得到的位置与姿态信息
    pose_pub = nh.advertise<geometry_msgs::PoseStamped>("/prometheus/object_detection/aruco_det_v2", 1);
    //【发布】检测结果图像
    aruco_pub = it.advertise(output_topic.c_str(), 1);

    std::string ros_path = ros::package::getPath("prometheus_detection");
    if (local_print) ROS_INFO("DETECTION_PATH: %s", ros_path.c_str());

    cv::Mat camMatrix, distCoeffs;
    bool readOk = readCameraParameters(camera_params_yaml.c_str(), camMatrix, distCoeffs);
    if (!readOk) {
        cerr << "Invalid camera file" << endl;
        return 0;
    }

    if (local_print) {
        cout << "[camMatrix]:" << endl;
        cout << camMatrix << endl;
        cout << "[distCoeffs]:" << endl;
        cout << distCoeffs << endl;
    }

    Ptr<aruco::Dictionary> dictionary =
        aruco::getPredefinedDictionary(aruco::PREDEFINED_DICTIONARY_NAME(dictionaryId));
    Ptr<aruco::DetectorParameters> detectorParams = aruco::DetectorParameters::create();

    std::vector<float> collected_mtx, collected_mty, collected_mtz;
    std::vector<float> collected_mqx, collected_mqy, collected_mqz, collected_mqw;
    float mtx_calib, mty_calib, mtz_calib, mqx_calib, mqy_calib, mqz_calib, mqw_calib;

    cv::Mat frame, frameCopy;
    const auto wait_duration = std::chrono::milliseconds(1000);
    while (ros::ok())
	{
        while (!getImageStatus() && ros::ok())
        {
            if (local_print) cout << "Waiting for image." << endl;
            std::this_thread::sleep_for(wait_duration);
            ros::spinOnce();
        }

        {
            boost::unique_lock<boost::shared_mutex> lockImageCallback(mutex_image_callback);
            frame = cam_image_copy.clone();
        }

        if (!frame.empty())
        {
            vector< int > ids;
            vector< vector< Point2f > > corners, rejected;
            vector< Vec3d > rvecs, tvecs;

            float markerLength = targetMarkerLength;
            float squareLength = calibSquareLength;
            if (0 == run_state || 1 == run_state)
                markerLength = calibMarkerLength;
            if (2 == run_state)
                markerLength = targetMarkerLength;

            // detect markers and estimate pose
            aruco::detectMarkers(frame, dictionary, corners, ids, detectorParams, rejected);
            if (ids.size() > 0)
                aruco::estimatePoseSingleMarkers(corners, markerLength, camMatrix, distCoeffs, rvecs, tvecs);

            frame.copyTo(frameCopy);
            if(ids.size() > 0) {
                aruco::drawDetectedMarkers(frameCopy, corners, ids);

                std::vector<float> collected_tx, collected_ty, collected_tz;
                std::vector<float> collected_qx, collected_qy, collected_qz, collected_qw;

                for(unsigned int i = 0; i < ids.size(); i++) {
                    aruco::drawAxis(frameCopy, camMatrix, distCoeffs, rvecs[i], tvecs[i], markerLength * 0.5f);
                    cv::Mat rotation_matrix;
                    cv::Rodrigues(rvecs[i], rotation_matrix);
                    Eigen::Matrix3d rotation_matrix_eigen;
                    cv::cv2eigen(rotation_matrix, rotation_matrix_eigen);
                    Eigen::Quaterniond q = Eigen::Quaterniond(rotation_matrix_eigen);
                    q.normalize();

                    geometry_msgs::PoseStamped pose;
                    pose.header.frame_id = "camera";
                    pose.pose.position.x = tvecs[i][0];
                    pose.pose.position.y = tvecs[i][1];
                    pose.pose.position.z = tvecs[i][2];
                    pose.pose.orientation.x = q.x();
                    pose.pose.orientation.y = q.y();
                    pose.pose.orientation.z = q.z();
                    pose.pose.orientation.w = q.w();
                    pose_pub.publish(pose);

                    if (2 == run_state)
                    {
                        static tf::TransformBroadcaster br;
                        tf::Transform world2camera = tf::Transform(tf::Quaternion(q.x(), q.y(), q.z(), q.w()), tf::Vector3(tvecs[i][0], tvecs[i][1], tvecs[i][2]));
                        char obj_str[16];
                        sprintf(obj_str, "object-%d", ids[i]);
                        tf::StampedTransform trans_world2camera = tf::StampedTransform(world2camera, ros::Time(pose.header.stamp), "camera", obj_str);
                        br.sendTransform(trans_world2camera);
                    }

                    if (1 == run_state)
                    {
                        if (ids[i] >= 0 && ids[i] <= 16)
                        {
                            std::vector<double> vec_t{tvecs[i][0], tvecs[i][1], tvecs[i][2]};
	                        cv::Mat vec_t_mat{vec_t};
                            vec_t_mat = vec_t_mat;
                            vec_t_mat.convertTo(vec_t_mat, CV_32FC1);
                            // cout << "vec_t_mat.size():" << vec_t_mat.size() << endl;
                            // cout << "vec_t_mat.type():" << vec_t_mat.type() <<endl;
                            std::vector<double> id_to8_t(3);
                            if (ids[i] == 0)
                            {
                                id_to8_t[0] = squareLength;
                                id_to8_t[1] = -squareLength*3;
                                id_to8_t[2] = 0.;
                            }
                            else if (ids[i] == 1)
                            {
                                id_to8_t[0] = -squareLength;
                                id_to8_t[1] = -squareLength*3;
                                id_to8_t[2] = 0.;
                            }
                            else if (ids[i] == 2)
                            {
                                id_to8_t[0] = squareLength*2;
                                id_to8_t[1] = -squareLength*2;
                                id_to8_t[2] = 0.;
                            }
                            else if (ids[i] == 3)
                            {
                                id_to8_t[0] = 0.;
                                id_to8_t[1] = -squareLength*2;
                                id_to8_t[2] = 0.;
                            }
                            else if (ids[i] == 4)
                            {
                                id_to8_t[0] = -squareLength*2;
                                id_to8_t[1] = -squareLength*2;
                                id_to8_t[2] = 0.;
                            }
                            else if (ids[i] == 5)
                            {
                                id_to8_t[0] = squareLength;
                                id_to8_t[1] = -squareLength;
                                id_to8_t[2] = 0.;
                            }
                            else if (ids[i] == 6)
                            {
                                id_to8_t[0] = -squareLength;
                                id_to8_t[1] = -squareLength;
                                id_to8_t[2] = 0.;
                            }
                            else if (ids[i] == 7)
                            {
                                id_to8_t[0] = squareLength*2;
                                id_to8_t[1] = 0.;
                                id_to8_t[2] = 0.;
                            }
                            else if (ids[i] == 8)
                            {
                                collected_tx.push_back(tvecs[i][0]);
                                collected_ty.push_back(tvecs[i][1]);
                                collected_tz.push_back(tvecs[i][2]);

                                collected_qx.push_back(q.x());
                                collected_qy.push_back(q.y());
                                collected_qz.push_back(q.z());
                                collected_qw.push_back(q.w());
                                continue;
                            }
                            else if (ids[i] == 9)
                            {
                                id_to8_t[0] = -squareLength*2;
                                id_to8_t[1] = 0.;
                                id_to8_t[2] = 0.;
                            }
                            else if (ids[i] == 10)
                            {
                                id_to8_t[0] = squareLength;
                                id_to8_t[1] = squareLength;
                                id_to8_t[2] = 0.;
                            }
                            else if (ids[i] == 11)
                            {
                                id_to8_t[0] = -squareLength;
                                id_to8_t[1] = squareLength;
                                id_to8_t[2] = 0.;
                            }
                            else if (ids[i] == 12)
                            {
                                id_to8_t[0] = squareLength*2;
                                id_to8_t[1] = squareLength*2;
                                id_to8_t[2] = 0.;
                            }
                            else if (ids[i] == 13)
                            {
                                id_to8_t[0] = 0.;
                                id_to8_t[1] = squareLength*2;
                                id_to8_t[2] = 0.;
                            }
                            else if (ids[i] == 14)
                            {
                                id_to8_t[0] = -squareLength*2;
                                id_to8_t[1] = squareLength*2;
                                id_to8_t[2] = 0.;
                            }
                            else if (ids[i] == 15)
                            {
                                id_to8_t[0] = squareLength;
                                id_to8_t[1] = squareLength*3;
                                id_to8_t[2] = 0.;
                            }
                            else if (ids[i] == 16)
                            {
                                id_to8_t[0] = -squareLength;
                                id_to8_t[1] = squareLength*3;
                                id_to8_t[2] = 0.;
                            }
                            cv::Mat id_to8_t_mat{id_to8_t};
                            id_to8_t_mat.convertTo(id_to8_t_mat, CV_32FC1);

                            rotation_matrix.convertTo(rotation_matrix, CV_32FC1);
                            // cv::invert(rotation_matrix, rotation_matrix);
                            cv::Mat id_8_t = rotation_matrix * id_to8_t_mat + vec_t_mat;

                            collected_tx.push_back(id_8_t.at<float>(0));
                            collected_ty.push_back(id_8_t.at<float>(1));
                            collected_tz.push_back(id_8_t.at<float>(2));

                            collected_qx.push_back(q.x());
                            collected_qy.push_back(q.y());
                            collected_qz.push_back(q.z());
                            collected_qw.push_back(q.w());

                            // static tf::TransformBroadcaster br;
                            // tf::Transform world2camera = tf::Transform(tf::Quaternion(q.x(), q.y(), q.z(), q.w()),
                            //     tf::Vector3(id_8_t.at<float>(0), id_8_t.at<float>(1), id_8_t.at<float>(2)));
                            // char obj_str[16];
                            // sprintf(obj_str, "object-%d-8", ids[i]);
                            // tf::StampedTransform trans_world2camera = tf::StampedTransform(world2camera, ros::Time(pose.header.stamp), "camera", obj_str);
                            // br.sendTransform(trans_world2camera);
                        }
                    }
                }

                if (1 == run_state && collected_tx.size() > 8)
                {
                    float tx_sum = std::accumulate(std::begin(collected_tx), std::end(collected_tx), 0.0);
                    float tx_mean =  tx_sum / collected_tx.size();
                    float ty_sum = std::accumulate(std::begin(collected_ty), std::end(collected_ty), 0.0);
                    float ty_mean =  ty_sum / collected_ty.size();
                    float tz_sum = std::accumulate(std::begin(collected_tz), std::end(collected_tz), 0.0);
                    float tz_mean =  tz_sum / collected_tz.size();

                    float qx_sum = std::accumulate(std::begin(collected_qx), std::end(collected_qx), 0.0);
                    float qx_mean =  qx_sum / collected_qx.size();
                    float qy_sum = std::accumulate(std::begin(collected_qy), std::end(collected_qy), 0.0);
                    float qy_mean =  qy_sum / collected_qy.size();
                    float qz_sum = std::accumulate(std::begin(collected_qz), std::end(collected_qz), 0.0);
                    float qz_mean =  qz_sum / collected_qz.size();
                    float qw_sum = std::accumulate(std::begin(collected_qw), std::end(collected_qw), 0.0);
                    float qw_mean =  qw_sum / collected_qw.size();

                    static tf::TransformBroadcaster br;
                    tf::Transform world2camera = tf::Transform(tf::Quaternion(qx_mean, qy_mean, qz_mean, qw_mean), tf::Vector3(tx_mean, ty_mean, tz_mean));
                    tf::StampedTransform trans_world2camera = tf::StampedTransform(world2camera, ros::Time(), "camera", "calib-MEAN");
                    br.sendTransform(trans_world2camera);

                    collected_mtx.push_back(tx_mean);
                    collected_mty.push_back(ty_mean);
                    collected_mtz.push_back(tz_mean);
                    collected_mqx.push_back(qx_mean);
                    collected_mqy.push_back(qy_mean);
                    collected_mqz.push_back(qz_mean);
                    collected_mqw.push_back(qw_mean);
                }
            }

            if (1 == run_state && collected_mtx.size() >= 10)
            {

                float mtx_std = _vector_stdev(collected_mtx);
                float mty_std = _vector_stdev(collected_mty);
                float mtz_std = _vector_stdev(collected_mtz);

                float mqx_std = _vector_stdev(collected_mqx);
                float mqy_std = _vector_stdev(collected_mqy);
                float mqz_std = _vector_stdev(collected_mqz);
                float mqw_std = _vector_stdev(collected_mqw);

                // cout<<mtx_std<<", "<<mty_std<<", "<<mtz_std<<", "<<mqx_std<<", "<<mqy_std<<", "<<mqz_std<<", "<<mqw_std<<endl;
                if (mtx_std < 0.01 && mty_std < 0.01 && mtz_std < 0.01 && mqx_std < 0.01 && mqy_std < 0.01 && mqz_std < 0.01 && mqw_std < 0.01)
                {
                    ROS_INFO("Calibration completed!");
                    run_state = 2;

                    mtx_calib = std::accumulate(std::begin(collected_mtx), std::end(collected_mtx), 0.0) / collected_mtx.size();
                    mty_calib = std::accumulate(std::begin(collected_mty), std::end(collected_mty), 0.0) / collected_mty.size();
                    mtz_calib = std::accumulate(std::begin(collected_mtz), std::end(collected_mtz), 0.0) / collected_mtz.size();
                    mqx_calib = std::accumulate(std::begin(collected_mqx), std::end(collected_mqx), 0.0) / collected_mqx.size();
                    mqy_calib = std::accumulate(std::begin(collected_mqy), std::end(collected_mqy), 0.0) / collected_mqy.size();
                    mqz_calib = std::accumulate(std::begin(collected_mqz), std::end(collected_mqz), 0.0) / collected_mqz.size();
                    mqw_calib = std::accumulate(std::begin(collected_mqw), std::end(collected_mqw), 0.0) / collected_mqw.size();
                }

                collected_mtx.clear();
                collected_mty.clear();
                collected_mtz.clear();

                collected_mqx.clear();
                collected_mqy.clear();
                collected_mqz.clear();
                collected_mqw.clear();
            }

            if (2 == run_state)
            {
                static tf::TransformBroadcaster br;
                tf::Transform world2camera = tf::Transform(tf::Quaternion(mqx_calib, mqy_calib, mqz_calib, mqw_calib), tf::Vector3(mtx_calib, mty_calib, mtz_calib));
                tf::StampedTransform trans_world2camera = tf::StampedTransform(world2camera, ros::Time(), "camera", "map");
                br.sendTransform(trans_world2camera);

            }

            sensor_msgs::ImagePtr det_output_msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", frameCopy).toImageMsg();
            aruco_pub.publish(det_output_msg);
            // cv::imshow("frame", frame);
            // cv::waitKey(10);
        }

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

    return 0;
}


#ifndef AMGCL_VALUE_TYPE_EIGEN_HPP
#define AMGCL_VALUE_TYPE_EIGEN_HPP

/*
The MIT License

Copyright (c) 2012-2020 Denis Demidov <dennis.demidov@gmail.com>

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.
*/

/**
 * \file   amgcl/value_type/eigen.hpp
 * \author Denis Demidov <dennis.demidov@gmail.com>
 * \brief  Enable statically sized eigen matrices as value types.
 */

#include <Eigen/Dense>

#include <amgcl/backend/builtin.hpp>
#include <amgcl/value_type/interface.hpp>

namespace amgcl {
namespace backend {

/// Enable Eigen matrix as a value-type.
template <typename T, int N, int M>
struct is_builtin_vector< std::vector<Eigen::Matrix<T, N, M> > > : std::true_type {};

} // namespace backend

namespace math {

/// Scalar type of a non-scalar type.
template <class T, int N, int M>
struct scalar_of< Eigen::Matrix<T, N, M> > {
    typedef typename math::scalar_of<T>::type type;
};

/// Replace scalar type in the static matrix
template <class T, int N, int M, class S>
struct replace_scalar< Eigen::Matrix<T, N, M>, S> {
    typedef Eigen::Matrix<S, N, M> type;
};

/// RHS type corresponding to a non-scalar type.
template <class T, int N>
struct rhs_of< Eigen::Matrix<T, N, N> > {
    typedef Eigen::Matrix<T, N, 1> type;
};

/// Whether the value type is a statically sized matrix.
template <class T, int N, int M>
struct is_static_matrix< Eigen::Matrix<T, N, M> > : std::true_type {};

/// Number of rows for statically sized matrix types.
template <class T, int N, int M>
struct static_rows< Eigen::Matrix<T, N, M> > : std::integral_constant<int, N> {};

/// Number of columns for statically sized matrix types.
template <class T, int N, int M>
struct static_cols< Eigen::Matrix<T, N, M> > : std::integral_constant<int, M> {};

/// Specialization of conjugate transpose for eigen matrices.
template <typename T, int N, int M>
struct adjoint_impl< Eigen::Matrix<T, N, M> >
{
    typedef typename Eigen::Matrix<T, N, M>::AdjointReturnType return_type;

    static return_type get(const Eigen::Matrix<T, N, M> &x) {
        return x.adjoint();
    }
};

/// Inner-product result of two Eigen vectors.
template <class T, int N>
struct inner_product_impl< Eigen::Matrix<T, N, 1> >
{
    typedef T return_type;
    static T get(const Eigen::Matrix<T, N, 1> &x, const Eigen::Matrix<T, N, 1> &y) {
        return x.adjoint() * y;
    }
};

/// Inner-product result of two Eigen matrices.
template <class T, int N, int M>
struct inner_product_impl< Eigen::Matrix<T, N, M> >
{
    typedef Eigen::Matrix<T, M, M> return_type;

    static return_type get(const Eigen::Matrix<T, N, M> &x, const Eigen::Matrix<T, N, M> &y) {
        return x.adjoint() * y;
    }
};

/// Specialization of element norm for eigen matrices.
template <typename T, int N, int M>
struct norm_impl< Eigen::Matrix<T, N, M> >
{
    static typename math::scalar_of<T>::type get(const Eigen::Matrix<T, N, M> &x) {
        return x.norm();
    }
};

/// Specialization of zero element for eigen matrices.
template <typename T, int N, int M>
struct zero_impl< Eigen::Matrix<T, N, M> >
{
    static Eigen::Matrix<T, N, M> get() {
        return Eigen::Matrix<T, N, M>::Zero();
    }
};

/// Specialization of zero element for eigen matrices.
template <typename T, int N, int M>
struct is_zero_impl< Eigen::Matrix<T, N, M> >
{
    static bool get(const Eigen::Matrix<T, N, M> &x) {
        return x.isZero();
    }
};

/// Specialization of identity for eigen matrices.
template <typename T, int N>
struct identity_impl< Eigen::Matrix<T, N, N> >
{
    static Eigen::Matrix<T, N, N> get() {
        return Eigen::Matrix<T, N, N>::Identity();
    }
};

/// Specialization of constant for eigen matrices.
template <typename T, int N, int M>
struct constant_impl< Eigen::Matrix<T, N, M> >
{
    static Eigen::Matrix<T, N, M> get(T c) {
        return Eigen::Matrix<T, N, M>::Constant(c);
    }
};

/// Specialization of inversion for eigen matrices.
template <typename T, int N>
struct inverse_impl< Eigen::Matrix<T, N, N> >
{
    static Eigen::Matrix<T, N, N> get(const Eigen::Matrix<T, N, N> &x) {
        return x.inverse();
    }
};

} // namespace math
} // namespace amgcl

namespace Eigen {

template <class A, class B>
bool operator<(const MatrixBase<A> &a, const MatrixBase<B> &b) {
    return a.trace() < b.trace();
}

} // namespace Eigen

#endif