/*
 Copyright (C) 2016 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.
*/

/*! \file ored/utilities/indexparser.cpp
    \brief
    \ingroup utilities
*/

#include <boost/algorithm/string.hpp>
#include <boost/make_shared.hpp>
#include <map>
#include <ored/configuration/conventions.hpp>
#include <ored/utilities/indexparser.hpp>
#include <ored/utilities/parsers.hpp>
#include <ql/errors.hpp>
#include <ql/indexes/all.hpp>
#include <ql/time/calendars/target.hpp>
#include <ql/time/daycounters/all.hpp>
#include <qle/indexes/bmaindexwrapper.hpp>
#include <qle/indexes/dkcpi.hpp>
#include <qle/indexes/equityindex.hpp>
#include <qle/indexes/fxindex.hpp>
#include <qle/indexes/genericiborindex.hpp>
#include <qle/indexes/ibor/audbbsw.hpp>
#include <qle/indexes/ibor/brlcdi.hpp>
#include <qle/indexes/ibor/chfsaron.hpp>
#include <qle/indexes/ibor/chftois.hpp>
#include <qle/indexes/ibor/clpcamara.hpp>
#include <qle/indexes/ibor/copibr.hpp>
#include <qle/indexes/ibor/corra.hpp>
#include <qle/indexes/ibor/czkpribor.hpp>
#include <qle/indexes/ibor/demlibor.hpp>
#include <qle/indexes/ibor/dkkcibor.hpp>
#include <qle/indexes/ibor/dkkois.hpp>
#include <qle/indexes/ibor/hkdhibor.hpp>
#include <qle/indexes/ibor/hufbubor.hpp>
#include <qle/indexes/ibor/idridrfix.hpp>
#include <qle/indexes/ibor/idrjibor.hpp>
#include <qle/indexes/ibor/ilstelbor.hpp>
#include <qle/indexes/ibor/inrmifor.hpp>
#include <qle/indexes/ibor/krwcd.hpp>
#include <qle/indexes/ibor/krwkoribor.hpp>
#include <qle/indexes/ibor/mxntiie.hpp>
#include <qle/indexes/ibor/myrklibor.hpp>
#include <qle/indexes/ibor/noknibor.hpp>
#include <qle/indexes/ibor/nowa.hpp>
#include <qle/indexes/ibor/nzdbkbm.hpp>
#include <qle/indexes/ibor/plnpolonia.hpp>
#include <qle/indexes/ibor/phpphiref.hpp>
#include <qle/indexes/ibor/plnwibor.hpp>
#include <qle/indexes/ibor/rubmosprime.hpp>
#include <qle/indexes/ibor/saibor.hpp>
#include <qle/indexes/ibor/seksior.hpp>
#include <qle/indexes/ibor/sekstibor.hpp>
#include <qle/indexes/ibor/sgdsibor.hpp>
#include <qle/indexes/ibor/sgdsor.hpp>
#include <qle/indexes/ibor/skkbribor.hpp>
#include <qle/indexes/ibor/thbbibor.hpp>
#include <qle/indexes/ibor/tonar.hpp>
#include <qle/indexes/ibor/twdtaibor.hpp>
#include <qle/indexes/secpi.hpp>

using namespace QuantLib;
using namespace QuantExt;
using namespace std;
using ore::data::Convention;

namespace ore {
namespace data {

// Helper build classes for static map

class IborIndexParser {
public:
    virtual ~IborIndexParser() {}
    virtual boost::shared_ptr<IborIndex> build(Period p, const Handle<YieldTermStructure>& h) const = 0;
};

template <class T> class IborIndexParserWithPeriod : public IborIndexParser {
public:
    boost::shared_ptr<IborIndex> build(Period p, const Handle<YieldTermStructure>& h) const override {
        QL_REQUIRE(p != 1 * Days, "must have a period longer than 1D");
        return boost::make_shared<T>(p, h);
    }
};

// Specialise for MXN-TIIE. If tenor equates to 28 Days, i.e. tenor is 4W or 28D, ensure that the index is created
// with a tenor of 4W under the hood. Things work better this way especially cap floor stripping.
template <> class IborIndexParserWithPeriod<MXNTiie> : public IborIndexParser {
public:
    boost::shared_ptr<IborIndex> build(Period p, const Handle<YieldTermStructure>& h) const override {
        QL_REQUIRE(p != 1 * Days, "must have a period longer than 1D");
        if (p.units() == Days && p.length() == 28) {
            return boost::make_shared<MXNTiie>(4 * Weeks, h);
        } else {
            return boost::make_shared<MXNTiie>(p, h);
        }
    }
};

template <class T> class IborIndexParserOIS : public IborIndexParser {
public:
    boost::shared_ptr<IborIndex> build(Period p, const Handle<YieldTermStructure>& h) const override {
        QL_REQUIRE(p == 1 * Days, "must have period 1D");
        return boost::make_shared<T>(h);
    }
};

template <class T> class IborIndexParserBMA : public IborIndexParser {
public:
    boost::shared_ptr<IborIndex> build(Period p, const Handle<YieldTermStructure>& h) const override {
        QL_REQUIRE((p.length() == 7 && p.units() == Days) || (p.length() == 1 && p.units() == Weeks),
                   "BMA indexes are uniquely available with a tenor of 1 week.");
        const boost::shared_ptr<BMAIndex> bma = boost::make_shared<BMAIndex>(h);
        return boost::make_shared<T>(bma);
    }
};

boost::shared_ptr<FxIndex> parseFxIndex(const string& s) {
    std::vector<string> tokens;
    split(tokens, s, boost::is_any_of("-"));
    QL_REQUIRE(tokens.size() == 4, "four tokens required in " << s << ": FX-TAG-CCY1-CCY2");
    QL_REQUIRE(tokens[0] == "FX", "expected first token to be FX");
    return boost::make_shared<FxIndex>(tokens[0] + "/" + tokens[1], 0, parseCurrency(tokens[2]),
                                       parseCurrency(tokens[3]), NullCalendar());
}

boost::shared_ptr<EquityIndex> parseEquityIndex(const string& s) {
    std::vector<string> tokens;
    split(tokens, s, boost::is_any_of("-"));
    QL_REQUIRE(tokens.size() == 2, "two tokens required in " << s << ": EQ-NAME");
    QL_REQUIRE(tokens[0] == "EQ", "expected first token to be EQ");
    if (tokens.size() == 2) {
        return boost::make_shared<EquityIndex>(tokens[1], NullCalendar(), Currency());
    } else {
        QL_FAIL("Error parsing equity string " + s);
    }
}

bool tryParseIborIndex(const string& s, boost::shared_ptr<IborIndex>& index) {
    try {
        index = parseIborIndex(s);
    } catch (...) {
        return false;
    }
    return true;
}

boost::shared_ptr<IborIndex> parseIborIndex(const string& s, const Handle<YieldTermStructure>& h) {
    string dummy;
    return parseIborIndex(s, dummy, h);
}

boost::shared_ptr<IborIndex> parseIborIndex(const string& s, string& tenor, const Handle<YieldTermStructure>& h) {

    std::vector<string> tokens;
    split(tokens, s, boost::is_any_of("-"));

    QL_REQUIRE(tokens.size() == 2 || tokens.size() == 3,
               "Two or three tokens required in " << s << ": CCY-INDEX or CCY-INDEX-TERM");

    Period p;
    if (tokens.size() == 3) {
        tenor = tokens[2];
        p = parsePeriod(tokens[2]);
    } else {
        tenor = "";
        p = 1 * Days;
    }

    static map<string, boost::shared_ptr<IborIndexParser>> m = {
        {"EUR-EONIA", boost::make_shared<IborIndexParserOIS<Eonia>>()},
        {"GBP-SONIA", boost::make_shared<IborIndexParserOIS<Sonia>>()},
        {"JPY-TONAR", boost::make_shared<IborIndexParserOIS<Tonar>>()},
        {"CHF-TOIS", boost::make_shared<IborIndexParserOIS<CHFTois>>()},
        {"CHF-SARON", boost::make_shared<IborIndexParserOIS<CHFSaron>>()},
        {"USD-FedFunds", boost::make_shared<IborIndexParserOIS<FedFunds>>()},
        {"AUD-AONIA", boost::make_shared<IborIndexParserOIS<Aonia>>()},
        {"CAD-CORRA", boost::make_shared<IborIndexParserOIS<CORRA>>()},
        {"DKK-DKKOIS", boost::make_shared<IborIndexParserOIS<DKKOis>>()},
        {"DKK-TNR", boost::make_shared<IborIndexParserOIS<DKKOis>>()},
        {"SEK-SIOR", boost::make_shared<IborIndexParserOIS<SEKSior>>()},
        {"AUD-BBSW", boost::make_shared<IborIndexParserWithPeriod<AUDbbsw>>()},
        {"AUD-LIBOR", boost::make_shared<IborIndexParserWithPeriod<AUDLibor>>()},
        {"EUR-EURIBOR", boost::make_shared<IborIndexParserWithPeriod<Euribor>>()},
        {"EUR-EURIB", boost::make_shared<IborIndexParserWithPeriod<Euribor>>()},
        {"CAD-CDOR", boost::make_shared<IborIndexParserWithPeriod<Cdor>>()},
        {"CAD-BA", boost::make_shared<IborIndexParserWithPeriod<Cdor>>()},
        {"CZK-PRIBOR", boost::make_shared<IborIndexParserWithPeriod<CZKPribor>>()},
        {"EUR-LIBOR", boost::make_shared<IborIndexParserWithPeriod<EURLibor>>()},
        {"USD-LIBOR", boost::make_shared<IborIndexParserWithPeriod<USDLibor>>()},
        {"GBP-LIBOR", boost::make_shared<IborIndexParserWithPeriod<GBPLibor>>()},
        {"JPY-LIBOR", boost::make_shared<IborIndexParserWithPeriod<JPYLibor>>()},
        {"JPY-TIBOR", boost::make_shared<IborIndexParserWithPeriod<Tibor>>()},
        {"CAD-LIBOR", boost::make_shared<IborIndexParserWithPeriod<CADLibor>>()},
        {"CHF-LIBOR", boost::make_shared<IborIndexParserWithPeriod<CHFLibor>>()},
        {"SEK-LIBOR", boost::make_shared<IborIndexParserWithPeriod<SEKLibor>>()},
        {"SEK-STIBOR", boost::make_shared<IborIndexParserWithPeriod<SEKStibor>>()},
        {"NOK-NIBOR", boost::make_shared<IborIndexParserWithPeriod<NOKNibor>>()},
        {"HKD-HIBOR", boost::make_shared<IborIndexParserWithPeriod<HKDHibor>>()},
        {"SAR-SAIBOR", boost::make_shared<IborIndexParserWithPeriod<SAibor>>()},
        {"SGD-SIBOR", boost::make_shared<IborIndexParserWithPeriod<SGDSibor>>()},
        {"SGD-SOR", boost::make_shared<IborIndexParserWithPeriod<SGDSor>>()},
        {"DKK-CIBOR", boost::make_shared<IborIndexParserWithPeriod<DKKCibor>>()},
        {"DKK-LIBOR", boost::make_shared<IborIndexParserWithPeriod<DKKLibor>>()},
        {"HUF-BUBOR", boost::make_shared<IborIndexParserWithPeriod<HUFBubor>>()},
        {"IDR-IDRFIX", boost::make_shared<IborIndexParserWithPeriod<IDRIdrfix>>()},
        {"IDR-JIBOR", boost::make_shared<IborIndexParserWithPeriod<IDRJibor>>()},
        {"ILS-TELBOR", boost::make_shared<IborIndexParserWithPeriod<ILSTelbor>>()},
        {"INR-MIFOR", boost::make_shared<IborIndexParserWithPeriod<INRMifor>>()},
        {"MXN-TIIE", boost::make_shared<IborIndexParserWithPeriod<MXNTiie>>()},
        {"PLN-WIBOR", boost::make_shared<IborIndexParserWithPeriod<PLNWibor>>()},
        {"SKK-BRIBOR", boost::make_shared<IborIndexParserWithPeriod<SKKBribor>>()},
        {"NZD-BKBM", boost::make_shared<IborIndexParserWithPeriod<NZDBKBM>>()},
        {"TRY-TRLIBOR", boost::make_shared<IborIndexParserWithPeriod<TRLibor>>()},
        {"TWD-TAIBOR", boost::make_shared<IborIndexParserWithPeriod<TWDTaibor>>()},
        {"MYR-KLIBOR", boost::make_shared<IborIndexParserWithPeriod<MYRKlibor>>()},
        {"KRW-CD", boost::make_shared<IborIndexParserWithPeriod<KRWCd>>()},
        {"KRW-KORIBOR", boost::make_shared<IborIndexParserWithPeriod<KRWKoribor>>()},
        {"ZAR-JIBAR", boost::make_shared<IborIndexParserWithPeriod<Jibar>>()},
        {"RUB-MOSPRIME", boost::make_shared<IborIndexParserWithPeriod<RUBMosprime>>()},
        {"USD-SIFMA", boost::make_shared<IborIndexParserBMA<BMAIndexWrapper>>()},
        {"THB-BIBOR", boost::make_shared<IborIndexParserWithPeriod<THBBibor>>()},
        {"PHP-PHIREF", boost::make_shared<IborIndexParserWithPeriod<PHPPhiref>>()},
        {"COP-IBR", boost::make_shared<IborIndexParserOIS<COPIbr>>()},
        {"DEM-LIBOR", boost::make_shared<IborIndexParserWithPeriod<DEMLibor>>()},
        {"BRL-CDI", boost::make_shared<IborIndexParserOIS<BRLCdi>>()},
        {"NOK-NOWA", boost::make_shared<IborIndexParserOIS<Nowa>>()},
        {"CLP-CAMARA", boost::make_shared<IborIndexParserOIS<CLPCamara>>()},
        {"NZD-OCR", boost::make_shared<IborIndexParserOIS<Nzocr>>()},
        {"PLN-POLONIA", boost::make_shared<IborIndexParserOIS<PLNPolonia>>()}};

    auto it = m.find(tokens[0] + "-" + tokens[1]);
    if (it != m.end()) {
        return it->second->build(p, h);
    } else if (tokens[1] == "GENERIC") {
        // We have a generic index
        auto ccy = parseCurrency(tokens[0]);
        return boost::make_shared<GenericIborIndex>(p, ccy, h);
    } else {
        QL_FAIL("parseIborIndex \"" << s << "\" not recognized");
    }
}

bool isGenericIndex(const string& indexName) {
    return indexName.find("-GENERIC-") != string::npos;
}

bool isInflationIndex(const string& indexName) {
    try {
        // Currently, only way to have an inflation index is to have a ZeroInflationIndex
        parseZeroInflationIndex(indexName);
    } catch (...) {
        return false;
    }
    return true;
}

// Swap Index Parser base
class SwapIndexParser {
public:
    virtual ~SwapIndexParser() {}
    virtual boost::shared_ptr<SwapIndex> build(Period p, const Handle<YieldTermStructure>& f,
                                               const Handle<YieldTermStructure>& d) const = 0;
};

// We build with both a forwarding and discounting curve
template <class T> class SwapIndexParserDualCurve : public SwapIndexParser {
public:
    boost::shared_ptr<SwapIndex> build(Period p, const Handle<YieldTermStructure>& f,
                                       const Handle<YieldTermStructure>& d) const override {
        return boost::make_shared<T>(p, f, d);
    }
};

boost::shared_ptr<SwapIndex> parseSwapIndex(const string& s, const Handle<YieldTermStructure>& f,
                                            const Handle<YieldTermStructure>& d,
                                            boost::shared_ptr<data::IRSwapConvention> convention) {

    std::vector<string> tokens;
    split(tokens, s, boost::is_any_of("-"));

    QL_REQUIRE(tokens.size() == 3, "three tokens required in " << s << ": CCY-CMS-TENOR");
    QL_REQUIRE(tokens[0].size() == 3, "invalid currency code in " << s);
    QL_REQUIRE(tokens[1] == "CMS", "expected CMS as middle token in " << s);

    Period p = parsePeriod(tokens[2]);

    string familyName = tokens[0] + "LiborSwapIsdaFix";
    Currency ccy = parseCurrency(tokens[0]);

    boost::shared_ptr<IborIndex> index =
        f.empty() || !convention ? boost::shared_ptr<IborIndex>() : convention->index()->clone(f);
    QuantLib::Natural settlementDays = index ? index->fixingDays() : 0;
    QuantLib::Calendar calender = convention ? convention->fixedCalendar() : NullCalendar();
    Period fixedLegTenor = convention ? Period(convention->fixedFrequency()) : Period(1, Months);
    BusinessDayConvention fixedLegConvention = convention ? convention->fixedConvention() : ModifiedFollowing;
    DayCounter fixedLegDayCounter = convention ? convention->fixedDayCounter() : ActualActual();

    if (d.empty())
        return boost::make_shared<SwapIndex>(familyName, p, settlementDays, ccy, calender, fixedLegTenor,
                                             fixedLegConvention, fixedLegDayCounter, index);
    else
        return boost::make_shared<SwapIndex>(familyName, p, settlementDays, ccy, calender, fixedLegTenor,
                                             fixedLegConvention, fixedLegDayCounter, index, d);
}

// Zero Inflation Index Parser
class ZeroInflationIndexParserBase {
public:
    virtual ~ZeroInflationIndexParserBase() {}
    virtual boost::shared_ptr<ZeroInflationIndex> build(bool isInterpolated,
                                                        const Handle<ZeroInflationTermStructure>& h) const = 0;
};

template <class T> class ZeroInflationIndexParser : public ZeroInflationIndexParserBase {
public:
    boost::shared_ptr<ZeroInflationIndex> build(bool isInterpolated,
                                                const Handle<ZeroInflationTermStructure>& h) const override {
        return boost::make_shared<T>(isInterpolated, h);
    }
};

boost::shared_ptr<ZeroInflationIndex> parseZeroInflationIndex(const string& s, bool isInterpolated,
                                                              const Handle<ZeroInflationTermStructure>& h) {

    static map<string, boost::shared_ptr<ZeroInflationIndexParserBase>> m = {
        {"EUHICP", boost::make_shared<ZeroInflationIndexParser<EUHICP>>()},
        {"EU HICP", boost::make_shared<ZeroInflationIndexParser<EUHICP>>()},
        {"EUHICPXT", boost::make_shared<ZeroInflationIndexParser<EUHICPXT>>()},
        {"EU HICPXT", boost::make_shared<ZeroInflationIndexParser<EUHICPXT>>()},
        {"FRHICP", boost::make_shared<ZeroInflationIndexParser<FRHICP>>()},
        {"FR HICP", boost::make_shared<ZeroInflationIndexParser<FRHICP>>()},
        {"UKRPI", boost::make_shared<ZeroInflationIndexParser<UKRPI>>()},
        {"UK RPI", boost::make_shared<ZeroInflationIndexParser<UKRPI>>()},
        {"USCPI", boost::make_shared<ZeroInflationIndexParser<USCPI>>()},
        {"US CPI", boost::make_shared<ZeroInflationIndexParser<USCPI>>()},
        {"ZACPI", boost::make_shared<ZeroInflationIndexParser<ZACPI>>()},
        {"ZA CPI", boost::make_shared<ZeroInflationIndexParser<ZACPI>>()},
        {"SECPI", boost::make_shared<ZeroInflationIndexParser<SECPI>>()},
        {"DKCPI", boost::make_shared<ZeroInflationIndexParser<DKCPI>>()}};

    auto it = m.find(s);
    if (it != m.end()) {
        return it->second->build(isInterpolated, h);
    } else {
        QL_FAIL("parseZeroInflationIndex: \"" << s << "\" not recognized");
    }
}

boost::shared_ptr<Index> parseIndex(const string& s, const data::Conventions& conventions) {
    boost::shared_ptr<QuantLib::Index> ret_idx;
    try {
        ret_idx = parseIborIndex(s);
    } catch (...) {
    }
    if (!ret_idx) {
        try {
            auto c = boost::dynamic_pointer_cast<SwapIndexConvention>(conventions.get(s));
            QL_REQUIRE(c, "no swap index convention");
            auto c2 = boost::dynamic_pointer_cast<IRSwapConvention>(conventions.get(c->conventions()));
            QL_REQUIRE(c2, "no swap convention");
            ret_idx = parseSwapIndex(s, Handle<YieldTermStructure>(), Handle<YieldTermStructure>(), c2);
        } catch (...) {
        }
    }
    if (!ret_idx) {
        try {
            ret_idx = parseZeroInflationIndex(s);
        } catch (...) {
        }
    }
    if (!ret_idx) {
        try {
            ret_idx = parseFxIndex(s);
        } catch (...) {
        }
    }
    if (!ret_idx) {
        try {
            ret_idx = parseEquityIndex(s);
        } catch (...) {
        }
    }
    QL_REQUIRE(ret_idx, "parseIndex \"" << s << "\" not recognized");
    return ret_idx;
}

bool isOvernightIndex(const string& indexName) {

    boost::shared_ptr<IborIndex> index;
    if (tryParseIborIndex(indexName, index)) {
        auto onIndex = boost::dynamic_pointer_cast<OvernightIndex>(index);
        if (onIndex)
            return true;
    }

    return false;
}

} // namespace data
} // namespace ore


#include <glog/logging.h>
#include <pcl/features/integral_image_normal.h>
#include <pcl/filters/passthrough.h>
#include <pcl/io/pcd_io.h>
#include <pcl/recognition/cg/geometric_consistency.h>
#include <pcl/registration/correspondence_rejection_sample_consensus.h>
#include <pcl/registration/transformation_estimation_svd.h>
#include <v4r/common/intrinsics.h>
#include <v4r/common/miscellaneous.h>
#include <v4r/common/pcl_opencv.h>
#include <v4r/common/pcl_utils.h>
#include <v4r/config.h>
#include <v4r/features/FeatureDetector_KD_SIFTGPU.h>
#include <v4r/io/eigen.h>
#include <v4r/io/filesystem.h>
#include <v4r/registration/FeatureBasedRegistration.h>
#include <v4r/registration/fast_icp_with_gc.h>
#include <boost/filesystem.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/kruskal_min_spanning_tree.hpp>
#include <boost/program_options.hpp>

namespace bf = boost::filesystem;
namespace po = boost::program_options;

namespace std {
std::ostream &operator<<(std::ostream &, const std::vector<float> &);

std::ostream &operator<<(std::ostream &os, const std::vector<float> &vec) {
  for (auto item : vec) {
    os << item << " ";
  }
  return os;
}
}  // namespace std

struct CamConnect {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  Eigen::Matrix4f transformation_;
  float edge_weight;
  size_t source_id_, target_id_;

  CamConnect() : edge_weight(0.f) {}

  bool operator<(const CamConnect &e) const {
    return edge_weight < e.edge_weight;
  }

  bool operator<=(const CamConnect &e) const {
    return edge_weight <= e.edge_weight;
  }

  bool operator>(const CamConnect &e) const {
    return edge_weight > e.edge_weight;
  }

  bool operator>=(const CamConnect &e) const {
    return edge_weight >= e.edge_weight;
  }
};

struct View {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  typedef pcl::PointXYZRGB PointT;
  cv::Mat descriptors_;
  std::vector<int> keypoint_indices_;
  Eigen::Matrix4f camera_pose_;
  pcl::PointCloud<PointT>::ConstPtr cloud_;
};

class ViewRegistration {
 private:
  typedef pcl::PointXYZRGB PointT;
  std::vector<View, Eigen::aligned_allocator<View>> grph_;

  typedef boost::property<boost::edge_weight_t, CamConnect> EdgeWeightProperty;
  typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS, size_t, EdgeWeightProperty> Graph;
  typedef boost::graph_traits<Graph>::vertex_descriptor Vertex;
  typedef boost::graph_traits<Graph>::edge_descriptor Edge;
  typedef boost::graph_traits<Graph>::vertex_iterator vertex_iter;
  typedef boost::property_map<Graph, boost::vertex_index_t>::type IndexMap;

  Graph gs_;

  std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>> absolute_poses_;

  v4r::Intrinsics cam_;

 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  ViewRegistration() {}

  void calcEdgeWeightAndRefineTf(const typename pcl::PointCloud<PointT>::ConstPtr &cloud_src,
                                 const typename pcl::PointCloud<PointT>::ConstPtr &cloud_dst,
                                 const Eigen::Matrix4f &transform, float &registration_quality,
                                 Eigen::Matrix4f &refined_transform) {
    const float best_overlap_ = 0.75f;

    v4r::FastIterativeClosestPointWithGC<PointT> icp;

    bool use_normals = true;
    // compute normals
    if (use_normals) {
      pcl::PointCloud<pcl::Normal>::Ptr src_normals(new pcl::PointCloud<pcl::Normal>);
      pcl::PointCloud<pcl::Normal>::Ptr dst_normals(new pcl::PointCloud<pcl::Normal>);
      pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> normal_estimation;
      normal_estimation.setNormalEstimationMethod(
          pcl::IntegralImageNormalEstimation<PointT, pcl::Normal>::AVERAGE_3D_GRADIENT);
      normal_estimation.setNormalSmoothingSize(10.0);
      normal_estimation.setBorderPolicy(pcl::IntegralImageNormalEstimation<PointT, pcl::Normal>::BORDER_POLICY_MIRROR);
      normal_estimation.setInputCloud(cloud_src);
      normal_estimation.compute(*src_normals);
      normal_estimation.setInputCloud(cloud_dst);
      normal_estimation.compute(*dst_normals);
      icp.setSourceNormals(src_normals);
      icp.setTargetNormals(dst_normals);
    }

    icp.setCameraIntrinsics(cam_);
    icp.setMaxCorrespondenceDistance(0.02f);
    icp.setInputSource(cloud_src);
    icp.setInputTarget(cloud_dst);
    icp.useStandardCG(true);
    icp.setNoCG(true);
    icp.setOverlapPercentage(best_overlap_);
    icp.setKeepMaxHypotheses(5);
    icp.setMaximumIterations(10);
    icp.align(transform);
    float w_after_icp_ = icp.getFinalTransformation(refined_transform);

    if (w_after_icp_ < 0 || !pcl_isfinite(w_after_icp_))
      w_after_icp_ = std::numeric_limits<float>::max();
    else
      w_after_icp_ = best_overlap_ - w_after_icp_;

    //    transform = icp_trans; // refined transformation
    registration_quality = w_after_icp_;
  }

  void computeAbsolutePosesRecursive(
      const Graph &grph, const Vertex start, const Eigen::Matrix4f &accum,
      std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>> &absolute_poses,
      std::vector<bool> &hop_list) {
    boost::property_map<Graph, boost::edge_weight_t>::type weightmap = boost::get(boost::edge_weight, gs_);
    boost::graph_traits<Graph>::out_edge_iterator ei, ei_end;
    for (boost::tie(ei, ei_end) = boost::out_edges(start, grph); ei != ei_end; ++ei) {
      Vertex targ = boost::target(*ei, grph);
      size_t target_id = boost::target(*ei, grph);

      if (hop_list[target_id])
        continue;

      hop_list[target_id] = true;
      CamConnect my_e = weightmap[*ei];
      Eigen::Matrix4f intern_accum;
      Eigen::Matrix4f trans = my_e.transformation_;
      if (my_e.target_id_ != target_id) {
        Eigen::Matrix4f trans_inv;
        trans_inv = trans.inverse();
        trans = trans_inv;
      }
      intern_accum = accum * trans;
      absolute_poses[target_id] = intern_accum;
      computeAbsolutePosesRecursive(grph, targ, intern_accum, absolute_poses, hop_list);
    }
  }

  void computeAbsolutePoses(const Graph &grph,
                            std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>> &absolute_poses,
                            const Eigen::Matrix4f &initial_transform = Eigen::Matrix4f::Identity()) {
    size_t num_frames = boost::num_vertices(grph);
    absolute_poses.resize(num_frames);
    std::vector<bool> hop_list(num_frames, false);
    Vertex source_view = 0;
    hop_list[0] = true;
    Eigen::Matrix4f accum = initial_transform;
    absolute_poses[0] = accum;
    computeAbsolutePosesRecursive(grph, source_view, accum, absolute_poses, hop_list);
  }

  void addView(const pcl::PointCloud<PointT>::ConstPtr &cloud) {
    View view;
    view.cloud_ = cloud;

    v4r::PCLOpenCVConverter<PointT> pcl_opencv_converter;
    pcl_opencv_converter.setInputCloud(view.cloud_);
    auto color_image = pcl_opencv_converter.getRGBImage();

    std::vector<cv::KeyPoint> keypoints;
    v4r::FeatureDetector_KD_SIFTGPU sift;
    sift.detectAndCompute(color_image, keypoints, view.descriptors_);
    view.keypoint_indices_ = sift.getKeypointIndices();

    grph_.push_back(view);
  }

  void buildGraph() {
    for (size_t view_a = 0; view_a < grph_.size(); view_a++) {
      for (size_t view_b = 0; view_b < view_a; view_b++) {
        std::vector<CamConnect> transforms;
        CamConnect edge;
        edge.source_id_ = view_b;
        edge.target_id_ = view_a;

        const auto tfs = v4r::Registration::FeatureBasedRegistration<PointT>::estimateViewTransformationBySIFT(
            *grph_[view_a].cloud_, *grph_[view_b].cloud_, grph_[view_a].keypoint_indices_,
            grph_[view_b].keypoint_indices_, grph_[view_a].descriptors_, grph_[view_b].descriptors_);

        for (const auto &tf : tfs) {
          edge.transformation_ = tf;
          transforms.push_back(edge);
        }

        for (CamConnect &c : transforms) {
          Eigen::Matrix4f icp_refined_trans;
          calcEdgeWeightAndRefineTf(grph_[view_a].cloud_, grph_[view_b].cloud_, c.transformation_, c.edge_weight,
                                    icp_refined_trans);
          c.transformation_ = icp_refined_trans;
          LOG(INFO) << "Edge weight is " << c.edge_weight << " for edge connecting vertex " << c.source_id_ << " and "
                    << c.target_id_;
        }

        if (!transforms.empty()) {
          std::sort(transforms.begin(), transforms.end());
          boost::add_edge(transforms[0].source_id_, transforms[0].target_id_, transforms[0], gs_);
        }
      }
    }
  }

  void compute_mst() {
    boost::property_map<Graph, boost::edge_weight_t>::type weightmap = boost::get(boost::edge_weight, gs_);
    std::vector<Edge> spanning_tree;
    boost::kruskal_minimum_spanning_tree(gs_, std::back_inserter(spanning_tree));

    Graph grph_mst;
    std::cout << "Print the edges in the MST:" << std::endl;
    for (std::vector<Edge>::iterator ei = spanning_tree.begin(); ei != spanning_tree.end(); ++ei) {
      CamConnect my_e = weightmap[*ei];
      std::cout << "[" << source(*ei, gs_) << "->" << target(*ei, gs_) << "] with weight " << my_e.edge_weight
                << std::endl;
      boost::add_edge(source(*ei, gs_), target(*ei, gs_), weightmap[*ei], grph_mst);
    }

    computeAbsolutePoses(grph_mst, absolute_poses_);

    for (size_t view_id = 0; view_id < absolute_poses_.size(); view_id++) {
      grph_[view_id].camera_pose_ = absolute_poses_[view_id];
    }
  }

  std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>> getAbsolutePoses() const {
    return absolute_poses_;
  }

  void setCameraIntrinsics(const v4r::Intrinsics &cam) {
    cam_ = cam;
  }
};

int main(int argc, char **argv) {
  typedef pcl::PointXYZRGB PointT;
  bf::path input_dir;
  bool save_pose = false;
  double chop_z = std::numeric_limits<float>::max();
  int verbosity = 0;
  bf::path camera_calibration_file;

  po::options_description desc(
      "Feature-based View Registration via Minimum Spanning Tree\n======================================\n**Allowed "
      "options");
  desc.add_options()("help,h", "produce help message");
  desc.add_options()("input_dir,i", po::value<bf::path>(&input_dir)->required(),
                     "Directory with point clouds to be registered");
  desc.add_options()("chop_z,z", po::value<double>(&chop_z)->default_value(chop_z),
                     "Cut-off distance in meter in z direction.");
  desc.add_options()("save_pose,s", po::bool_switch(&save_pose),
                     "save computed camera pose in sensor header fields of input point clouds.");
  desc.add_options()("camera_calibration,c", po::value<bf::path>(&camera_calibration_file),
                     "Camera calibration file with intrinsic parameters");
  po::variables_map vm;
  po::parsed_options parsed = po::command_line_parser(argc, argv).options(desc).run();
  po::store(parsed, vm);
  if (vm.count("help")) {
    std::cout << desc << std::endl;
    return -1;
  }
  try {
    po::notify(vm);
  } catch (std::exception &e) {
    std::cerr << "Error: " << e.what() << std::endl << std::endl << desc << std::endl;
  }

  if (verbosity >= 0) {
    FLAGS_v = verbosity;
    std::cout << "Enabling verbose logging." << std::endl;
  }
  FLAGS_logtostderr = 1;
  google::InitGoogleLogging(argv[0]);

  LOG(INFO) << "Processing all point clouds in folder " << input_dir.string();

  const auto files_intern = v4r::io::getFilesInDirectory(input_dir, ".*.pcd", false);
  if (files_intern.empty()) {
    LOG(ERROR) << "No files in directory: " << input_dir.string();
    return -1;
  }

  v4r::Intrinsics cam = v4r::Intrinsics::PrimeSense();

  if (bf::exists(camera_calibration_file)) {
    try {
      cam = v4r::Intrinsics::load(camera_calibration_file.string());
    } catch (const std::runtime_error &e) {
      LOG(WARNING) << "Failed to load camera calibration file from " << camera_calibration_file.string()
                   << "! Will use Primesense default camera intrinsics parameters!";
    }
  }

  ViewRegistration view_reg;
  view_reg.setCameraIntrinsics(cam);
  for (const auto &fn : files_intern) {
    const bf::path full_path = input_dir / fn;
    pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
    pcl::io::loadPCDFile(full_path.string(), *cloud);

    pcl::PointCloud<PointT>::Ptr cloud_filtered(new pcl::PointCloud<PointT>);

    pcl::PassThrough<PointT> pass;
    pass.setFilterLimits(0.f, chop_z);
    pass.setFilterFieldName("z");
    pass.setInputCloud(cloud);
    pass.setKeepOrganized(true);
    pass.filter(*cloud_filtered);

    view_reg.addView(cloud_filtered);
  }

  view_reg.buildGraph();
  view_reg.compute_mst();

  const auto abs_poses = view_reg.getAbsolutePoses();

  if (save_pose) {
    for (size_t i = 0; i < files_intern.size(); i++) {
      const bf::path full_path = input_dir / files_intern[i];
      pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
      pcl::io::loadPCDFile(full_path.string(), *cloud);
      v4r::setCloudPose(abs_poses.at(i), *cloud);
      pcl::io::savePCDFileBinaryCompressed(full_path.string(), *cloud);
    }
  }
}


/*
 * This file is a part of the TChecker project.
 *
 * See files AUTHORS and LICENSE for copyright details.
 *
 */

#ifndef TCHECKER_REFZG_SEMANTICS_HH
#define TCHECKER_REFZG_SEMANTICS_HH

#include <boost/dynamic_bitset.hpp>

#include "tchecker/basictypes.hh"
#include "tchecker/dbm/db.hh"
#include "tchecker/variables/clocks.hh"

/*!
 \file semantics.hh
 \brief Operational semantics on DBMs with reference clocks
 */

namespace tchecker {

namespace refzg {

/*!
 \class semantics_t
 \brief Semantics for zone graphs implemented by DBMs with reference clocks
 */
class semantics_t {
public:
  /*!
  \brief Destructor
   */
  virtual ~semantics_t() = default;

  /*!
  \brief Compute initial zone with reference clocks
  \param rdbm : a DBM
  \param r : reference clocks for rdbm
  \param delay_allowed : set of reference clocks allowed to delay
  \param invariant : invariant
  \param spread : reference clocks spread
  \pre rdbm is not nullptr (checked by assertion).
  rdbm is a r.size()*r.size() array of difference bounds.
  rdbm is tight and consistent.
  the size of delay_allowed is the number of reference clocks in r
  \post dbm is the initial zone w.r.t. delay_allowed, invariant and spread
  \return STATE_OK if the resulting DBM is not empty, other values if the
  resulting DBM is empty (see details in implementations)
   */
  virtual tchecker::state_status_t initial(tchecker::dbm::db_t * rdbm, tchecker::reference_clock_variables_t const & r,
                                           boost::dynamic_bitset<> const & delay_allowed,
                                           tchecker::clock_constraint_container_t const & invariant) = 0;

  /*!
  \brief Compute next zone with reference clocks
  \param rdbm : a DBM
  \param r : reference clocks for rdbm
  \param src_delay_allowed : set of reference clocks allowed to delay in source
  state
  \param src_invariant : invariant in source state
  \param sync_ref_clocks : set of reference clocks to synchronize
  \param guard : transition guard
  \param clkreset : transition reset
  \param tgt_delay_allowed : set of reference clocks allowed to delay in target
  state
  \param tgt_invariant : invariant in target state
  \pre rdbm is not nullptr (checked by assertion).
  rdbm is a r.size()*r.size() array of difference bounds.
  rdbm is tight and consistent.
  the size of src_delay_allowed is the number of reference clocks in r.
  the size of sync_ref_clocks is the number of reference clocks in r.
  the size of tgt_delay_allowed is the number of reference clocks in r
  \post rdbm has been updated to its strongest postcondition w.r.t. src_delay_allowed,
  src_invariant, sync_ref_clocks, guard, clkreset, tgt_delay_allowed and tgt_invariant
  \return STATE_OK if the resulting DBM is not empty, other values if the resulting
  DBM is empty (see details in implementations)
   */
  virtual tchecker::state_status_t
  next(tchecker::dbm::db_t * rdbm, tchecker::reference_clock_variables_t const & r,
       boost::dynamic_bitset<> const & src_delay_allowed, tchecker::clock_constraint_container_t const & src_invariant,
       boost::dynamic_bitset<> const & sync_ref_clocks, tchecker::clock_constraint_container_t const & guard,
       tchecker::clock_reset_container_t const & clkreset, boost::dynamic_bitset<> const & tgt_delay_allowed,
       tchecker::clock_constraint_container_t const & tgt_invariant) = 0;
};

/*!
\class standard_semantics_t
\brief Standard semantics: each transition in the zone graph consists of a delay
(if allowed) followed by a transition from the timed automaton
*/
class standard_semantics_t final : public tchecker::refzg::semantics_t {
public:
  /*!
  \brief Destructor
  */
  virtual ~standard_semantics_t() = default;

  /*!
  \brief Compute initial zone with reference clocks
  \param rdbm : a DBM
  \param r : reference clocks for rdbm
  \param delay_allowed : set of reference clocks allowed to delay
  \param invariant : invariant
  \pre rdbm is not nullptr (checked by assertion).
  rdbm is a r.size()*r.size() array of difference bounds.
  rdbm is tight and consistent.
  the size of delay_allowed is the number of reference clocks in r (checked
  by assertion)
  \post rdbm is the zone that only containts the zero valuation
  \return tchecker::STATE_OK if the resulting DBM is not empty. Otherwise,
  tchecker::STATE_CLOCKS_SRC_INVARIANT_VIOLATED if the zero valuation does not
  satisfy invariant.
  */
  virtual tchecker::state_status_t initial(tchecker::dbm::db_t * rdbm, tchecker::reference_clock_variables_t const & r,
                                           boost::dynamic_bitset<> const & delay_allowed,
                                           tchecker::clock_constraint_container_t const & invariant);

  /*!
  \brief Compute next zone with reference clocks
  \param rdbm : a DBM
  \param r : reference clocks for dbm
  \param src_delay_allowed : set of reference clocks allowed to delay in source
  state
  \param src_invariant : invariant in source state
  \param sync_ref_clocks : set of reference clocks to synchronize
  \param guard : transition guard
  \param clkreset : transition reset
  \param tgt_delay_allowed : set of reference clocks allowed to delay in target
  state
  \param tgt_invariant : invariant in target state
  \pre rdbm is not nullptr (checked by assertion).
  rdbm is a r.size()*r.size() array of difference bounds.
  rdbm is not tight and consistent.
  the size of src_delay_allowed is the number of reference clocks in r
  (checked by assertion).
  The size of sync_ref_clocks is the number of reference clocks in r (checked by
  assertion).)
  The size of tgt_delay_allowed is the number of reference clocks in r (checked
  by assertion).
  \post rdbm has been delayed (only reference clocks which are allowed in
  src_delay_allowed), then intersected with src_invariant, then all reference
  clocks in sync_ref_clocks have been synchronized in rdbm, then rdbm has been
  intersected with guard, then dbm has been reset w.r.t clkreset, then
  intersected with tgt_invariant
  \return tchecker::STATE_OK if the resulting DBM is not empty. Otherwise,
  tchecker::STATE_CLOCKS_SRC_INVARIANT_VIOLATED if intersection with src_invariant
  result in an empty zone, tchecker::STATE_CLOCKS_SYNC_EMPTY if synchronization
  of reference clocks in sync_ref_clocks yield an empty zone,
  tchecker::STATE_CLOCKS_GUARD_VIOLATED if intersection with guard result in an
  empty zone, tchecker::STATE_EMPTY_SYNC if synchronization of reference clocks
  result in an empty zone, tchecker::STATE_CLOCKS_TGT_INVARIANT_VIOLATED if
  intersection with tgt_invariant result in an empty zone
  */
  virtual tchecker::state_status_t
  next(tchecker::dbm::db_t * rdbm, tchecker::reference_clock_variables_t const & r,
       boost::dynamic_bitset<> const & src_delay_allowed, tchecker::clock_constraint_container_t const & src_invariant,
       boost::dynamic_bitset<> const & sync_ref_clocks, tchecker::clock_constraint_container_t const & guard,
       tchecker::clock_reset_container_t const & clkreset, boost::dynamic_bitset<> const & tgt_delay_allowed,
       tchecker::clock_constraint_container_t const & tgt_invariant);
};

/*!
\class elapsed_semantics_t
\brief elapsed semantics: each transition in the zone graph consists of a
transition from the automaton, followed by a delay (if allowed)
*/
class elapsed_semantics_t final : public tchecker::refzg::semantics_t {
public:
  /*!
  \brief Destructor
  */
  virtual ~elapsed_semantics_t() = default;

  /*!
  \brief Compute initial zone with reference clocks
  \param rdbm : a DBM
  \param r : reference clocks for rdbm
  \param delay_allowed : set of reference clocks allowed to delay
  \param invariant : invariant
  \pre rdbm is not nullptr (checked by assertion).
  rdbm is a r.size()*r.size() array of difference bounds.
  rdbm is not tight and consistent.
  the size of delay_allowed is the number of reference clocks in r (checked
  by assertion))
  \post rdbm is the zone that contains all the time successors of the zero
  valuation (for reference clocks that are allowed to delay in delay_allowed),
  and that satisfy invariant
  \return tchecker::STATE_OK if the resulting DBM is not empty. Otherwise,
  tchecker::STATE_CLOCKS_SRC_INVARIANT_VIOLATED if the (time successors of the)
  zero valuation does not satisfy invariant.
  */
  virtual tchecker::state_status_t initial(tchecker::dbm::db_t * rdbm, tchecker::reference_clock_variables_t const & r,
                                           boost::dynamic_bitset<> const & delay_allowed,
                                           tchecker::clock_constraint_container_t const & invariant);

  /*!
  \brief Compute next zone with reference clocks
  \param rdbm : a DBM
  \param r : reference clocks for rdbm
  \param src_delay_allowed : set of reference clocks allowed to delay in source
  state
  \param src_invariant : invariant in source state
  \param sync_ref_clocks : set of reference clocks to synchronize
  \param guard : transition guard
  \param clkreset : transition reset
  \param tgt_delay_allowed : set of reference clocks allowed to delay in target
  state
  \param tgt_invariant : invariant in target state
  \pre rdbm is not nullptr (checked by assertion).
  rdbm is a r.size()*r.size() array of difference bounds.
  rdbm is not tight and consistent.
  the size of src_delay_allowed is the number of reference clocks in r
  (checked by assertion).
  The size of sync_ref_clocks is the number of reference clocks in r (checked by
  assertion).)
  The size of tgt_delay_allowed is the number of reference clocks in r (checked
  by assertion).
  \post rdbm has been intersected with src_invariant, then all reference clocks
  in sync_ref_clocks have been synchronized in rdbm, then rdbm has been
  intersected with guard, then rdbm has been reset w.r.t clkreset, then
  intersected with tgt_invariant, then rdbm has been delayed (only reference
  clocks that are allowed in tgt_delay_allowed) and intersected wth
  tgt_invariant again
  \return tchecker::STATE_OK if the resulting DBM is not empty. Otherwise,
  tchecker::STATE_CLOCKS_SRC_INVARIANT_VIOLATED if intersection with src_invariant
  result in an empty zone, tchecker::STATE_CLOCKS_EMPTY_SYNC if synchronization
  of reference clocks in sync_ref_clocks yields an empty zone,
  tchecker::STATE_CLOCKS_GUARD_VIOLATED if intersection with guard result in an
  empty zone, tchecker::STATE_EMPTY_SYNC if synchronization of reference clocks
  result in an empty zone, tchecker::STATE_CLOCKS_TGT_INVARIANT_VIOLATED if
  intersection with tgt_invariant result in an empty zone
  */
  virtual tchecker::state_status_t
  next(tchecker::dbm::db_t * rdbm, tchecker::reference_clock_variables_t const & r,
       boost::dynamic_bitset<> const & src_delay_allowed, tchecker::clock_constraint_container_t const & src_invariant,
       boost::dynamic_bitset<> const & sync_ref_clocks, tchecker::clock_constraint_container_t const & guard,
       tchecker::clock_reset_container_t const & clkreset, boost::dynamic_bitset<> const & tgt_delay_allowed,
       tchecker::clock_constraint_container_t const & tgt_invariant);
};

/*!
 \brief type of semantics
*/
enum semantics_type_t {
  STANDARD_SEMANTICS, /*!< see tchecker::refzg::standard_semantics_t */
  ELAPSED_SEMANTICS,  /*!< see tchecker::refzg::elapsed_semantics_t */
};

/*!
 \brief Zone semantics factory
 \param semantics_type : type zone graph semantics
 \return A zone semantics over DBMs with reference clocks of type semantics
 \note the returned semantics must be deallocated by the caller
 \throw std::invalid_argument : if semantics is unknown
*/
tchecker::refzg::semantics_t * semantics_factory(enum tchecker::refzg::semantics_type_t semantics_type);

} // end of namespace refzg

} // end of namespace tchecker

#endif // TCHECKER_REFZG_SEMANTICS_HH


// Boost.Geometry - gis-projections (based on PROJ4)

// Copyright (c) 2008-2015 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2017, 2018.
// Modifications copyright (c) 2017-2018, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Boost.Geometry by Barend Gehrels

// Last updated version of proj: 5.0.0

// Original copyright notice:

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

#ifndef BOOST_GEOMETRY_PROJECTIONS_IGH_HPP
#define BOOST_GEOMETRY_PROJECTIONS_IGH_HPP

#include <boost/geometry/util/math.hpp>
#include <boost/shared_ptr.hpp>

#include <boost/geometry/srs/projections/impl/base_static.hpp>
#include <boost/geometry/srs/projections/impl/base_dynamic.hpp>
#include <boost/geometry/srs/projections/impl/projects.hpp>
#include <boost/geometry/srs/projections/impl/factory_entry.hpp>
#include <boost/geometry/srs/projections/proj/gn_sinu.hpp>
#include <boost/geometry/srs/projections/proj/moll.hpp>

namespace boost { namespace geometry
{

namespace projections
{
    #ifndef DOXYGEN_NO_DETAIL
    namespace detail { namespace igh
    {
            // TODO: consider replacing dynamically created projections
            // with member objects
            template <typename T, typename Parameters>
            struct par_igh
            {
                boost::shared_ptr<base_v<T, Parameters> > pj[12];
                T dy0;
            };

            /* 40d 44' 11.8" [degrees] */
            template <typename T>
            inline T d4044118() { return (T(40) + T(44)/T(60.) + T(11.8)/T(3600.)) * geometry::math::d2r<T>(); }

            template <typename T>
            inline T d10() { return T(10) * geometry::math::d2r<T>(); }
            template <typename T>
            inline T d20() { return T(20) * geometry::math::d2r<T>(); }
            template <typename T>
            inline T d30() { return T(30) * geometry::math::d2r<T>(); }
            template <typename T>
            inline T d40() { return T(40) * geometry::math::d2r<T>(); }
            template <typename T>
            inline T d50() { return T(50) * geometry::math::d2r<T>(); }
            template <typename T>
            inline T d60() { return T(60) * geometry::math::d2r<T>(); }
            template <typename T>
            inline T d80() { return T(80) * geometry::math::d2r<T>(); }
            template <typename T>
            inline T d90() { return T(90) * geometry::math::d2r<T>(); }
            template <typename T>
            inline T d100() { return T(100) * geometry::math::d2r<T>(); }
            template <typename T>
            inline T d140() { return T(140) * geometry::math::d2r<T>(); }
            template <typename T>
            inline T d160() { return T(160) * geometry::math::d2r<T>(); }
            template <typename T>
            inline T d180() { return T(180) * geometry::math::d2r<T>(); }

            static const double epsilon = 1.e-10; // allow a little 'slack' on zone edge positions

            // Converted from #define SETUP(n, proj, x_0, y_0, lon_0)
            template <template <typename, typename, typename> class Entry, typename Params, typename Parameters, typename T>
            inline void do_setup(int n, Params const& params, Parameters const& par, par_igh<T, Parameters>& proj_parm,
                                 T const& x_0, T const& y_0,
                                 T const& lon_0)
            {
                // NOTE: in the original proj4 these projections are initialized
                // with zeroed parameters which could be done here as well instead
                // of initializing with parent projection's parameters.
                Entry<Params, T, Parameters> entry;
                proj_parm.pj[n-1].reset(entry.create_new(params, par));
                proj_parm.pj[n-1]->mutable_params().x0 = x_0;
                proj_parm.pj[n-1]->mutable_params().y0 = y_0;
                proj_parm.pj[n-1]->mutable_params().lam0 = lon_0;
            }

            // template class, using CRTP to implement forward/inverse
            template <typename T, typename Parameters>
            struct base_igh_spheroid
                : public base_t_fi<base_igh_spheroid<T, Parameters>, T, Parameters>
            {
                par_igh<T, Parameters> m_proj_parm;

                inline base_igh_spheroid(const Parameters& par)
                    : base_t_fi<base_igh_spheroid<T, Parameters>, T, Parameters>(*this, par)
                {}

                // FORWARD(s_forward)  spheroid
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(T lp_lon, T const& lp_lat, T& xy_x, T& xy_y) const
                {
                    static const T d4044118 = igh::d4044118<T>();
                    static const T d20  =  igh::d20<T>();
                    static const T d40  =  igh::d40<T>();
                    static const T d80  =  igh::d80<T>();
                    static const T d100 = igh::d100<T>();

                        int z;
                        if (lp_lat >=  d4044118) {          // 1|2
                          z = (lp_lon <= -d40 ? 1: 2);
                        }
                        else if (lp_lat >=  0) {            // 3|4
                          z = (lp_lon <= -d40 ? 3: 4);
                        }
                        else if (lp_lat >= -d4044118) {     // 5|6|7|8
                               if (lp_lon <= -d100) z =  5; // 5
                          else if (lp_lon <=  -d20) z =  6; // 6
                          else if (lp_lon <=   d80) z =  7; // 7
                          else z = 8;                       // 8
                        }
                        else {                              // 9|10|11|12
                               if (lp_lon <= -d100) z =  9; // 9
                          else if (lp_lon <=  -d20) z = 10; // 10
                          else if (lp_lon <=   d80) z = 11; // 11
                          else z = 12;                      // 12
                        }

                        lp_lon -= this->m_proj_parm.pj[z-1]->params().lam0;
                        this->m_proj_parm.pj[z-1]->fwd(lp_lon, lp_lat, xy_x, xy_y);
                        xy_x += this->m_proj_parm.pj[z-1]->params().x0;
                        xy_y += this->m_proj_parm.pj[z-1]->params().y0;
                }

                // INVERSE(s_inverse)  spheroid
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(T xy_x, T xy_y, T& lp_lon, T& lp_lat) const
                {
                    static const T d4044118 = igh::d4044118<T>();
                    static const T d10  =  igh::d10<T>();
                    static const T d20  =  igh::d20<T>();
                    static const T d40  =  igh::d40<T>();
                    static const T d50  =  igh::d50<T>();
                    static const T d60  =  igh::d60<T>();
                    static const T d80  =  igh::d80<T>();
                    static const T d90  =  igh::d90<T>();
                    static const T d100 = igh::d100<T>();
                    static const T d160 = igh::d160<T>();
                    static const T d180 = igh::d180<T>();

                    static const T c2 = 2.0;

                    const T y90 = this->m_proj_parm.dy0 + sqrt(c2); // lt=90 corresponds to y=y0+sqrt(2.0)

                        int z = 0;
                        if (xy_y > y90+epsilon || xy_y < -y90+epsilon) // 0
                          z = 0;
                        else if (xy_y >=  d4044118)       // 1|2
                          z = (xy_x <= -d40? 1: 2);
                        else if (xy_y >=  0)              // 3|4
                          z = (xy_x <= -d40? 3: 4);
                        else if (xy_y >= -d4044118) {     // 5|6|7|8
                               if (xy_x <= -d100) z =  5; // 5
                          else if (xy_x <=  -d20) z =  6; // 6
                          else if (xy_x <=   d80) z =  7; // 7
                          else z = 8;                     // 8
                        }
                        else {                            // 9|10|11|12
                               if (xy_x <= -d100) z =  9; // 9
                          else if (xy_x <=  -d20) z = 10; // 10
                          else if (xy_x <=   d80) z = 11; // 11
                          else z = 12;                    // 12
                        }

                        if (z)
                        {
                          int ok = 0;

                          xy_x -= this->m_proj_parm.pj[z-1]->params().x0;
                          xy_y -= this->m_proj_parm.pj[z-1]->params().y0;
                          this->m_proj_parm.pj[z-1]->inv(xy_x, xy_y, lp_lon, lp_lat);
                          lp_lon += this->m_proj_parm.pj[z-1]->params().lam0;

                          switch (z) {
                            case  1: ok = (lp_lon >= -d180-epsilon && lp_lon <=  -d40+epsilon) ||
                                         ((lp_lon >=  -d40-epsilon && lp_lon <=  -d10+epsilon) &&
                                          (lp_lat >=   d60-epsilon && lp_lat <=   d90+epsilon)); break;
                            case  2: ok = (lp_lon >=  -d40-epsilon && lp_lon <=  d180+epsilon) ||
                                         ((lp_lon >= -d180-epsilon && lp_lon <= -d160+epsilon) &&
                                          (lp_lat >=   d50-epsilon && lp_lat <=   d90+epsilon)) ||
                                         ((lp_lon >=  -d50-epsilon && lp_lon <=  -d40+epsilon) &&
                                          (lp_lat >=   d60-epsilon && lp_lat <=   d90+epsilon)); break;
                            case  3: ok = (lp_lon >= -d180-epsilon && lp_lon <=  -d40+epsilon); break;
                            case  4: ok = (lp_lon >=  -d40-epsilon && lp_lon <=  d180+epsilon); break;
                            case  5: ok = (lp_lon >= -d180-epsilon && lp_lon <= -d100+epsilon); break;
                            case  6: ok = (lp_lon >= -d100-epsilon && lp_lon <=  -d20+epsilon); break;
                            case  7: ok = (lp_lon >=  -d20-epsilon && lp_lon <=   d80+epsilon); break;
                            case  8: ok = (lp_lon >=   d80-epsilon && lp_lon <=  d180+epsilon); break;
                            case  9: ok = (lp_lon >= -d180-epsilon && lp_lon <= -d100+epsilon); break;
                            case 10: ok = (lp_lon >= -d100-epsilon && lp_lon <=  -d20+epsilon); break;
                            case 11: ok = (lp_lon >=  -d20-epsilon && lp_lon <=   d80+epsilon); break;
                            case 12: ok = (lp_lon >=   d80-epsilon && lp_lon <=  d180+epsilon); break;
                          }

                          z = (!ok? 0: z); // projectable?
                        }
                     // if (!z) pj_errno = -15; // invalid x or y
                        if (!z) lp_lon = HUGE_VAL;
                        if (!z) lp_lat = HUGE_VAL;
                }

                static inline std::string get_name()
                {
                    return "igh_spheroid";
                }

            };

            // Interrupted Goode Homolosine
            template <typename Params, typename Parameters, typename T>
            inline void setup_igh(Params const& params, Parameters& par, par_igh<T, Parameters>& proj_parm)
            {
                static const T d0   =  0;
                static const T d4044118 = igh::d4044118<T>();
                static const T d20  =  igh::d20<T>();
                static const T d30  =  igh::d30<T>();
                static const T d60  =  igh::d60<T>();
                static const T d100 = igh::d100<T>();
                static const T d140 = igh::d140<T>();
                static const T d160 = igh::d160<T>();

            /*
              Zones:

                -180            -40                       180
                  +--------------+-------------------------+    Zones 1,2,9,10,11 & 12:
                  |1             |2                        |      Mollweide projection
                  |              |                         |
                  +--------------+-------------------------+    Zones 3,4,5,6,7 & 8:
                  |3             |4                        |      Sinusoidal projection
                  |              |                         |
                0 +-------+------+-+-----------+-----------+
                  |5      |6       |7          |8          |
                  |       |        |           |           |
                  +-------+--------+-----------+-----------+
                  |9      |10      |11         |12         |
                  |       |        |           |           |
                  +-------+--------+-----------+-----------+
                -180    -100      -20         80          180
            */

                    T lp_lam = 0, lp_phi = d4044118;
                    T xy1_x, xy1_y;
                    T xy3_x, xy3_y;

                    // IMPORTANT: Force spherical sinu projection
                    // This is required because unlike in the original proj4 here
                    // parameters are used to initialize underlying projections.
                    // In the original code zeroed parameters are passed which
                    // could be done here as well though.
                    par.es = 0.;

                    // sinusoidal zones
                    do_setup<sinu_entry>(3, params, par, proj_parm, -d100, d0, -d100);
                    do_setup<sinu_entry>(4, params, par, proj_parm,   d30, d0,   d30);
                    do_setup<sinu_entry>(5, params, par, proj_parm, -d160, d0, -d160);
                    do_setup<sinu_entry>(6, params, par, proj_parm,  -d60, d0,  -d60);
                    do_setup<sinu_entry>(7, params, par, proj_parm,   d20, d0,   d20);
                    do_setup<sinu_entry>(8, params, par, proj_parm,  d140, d0,  d140);

                    // mollweide zones
                    do_setup<moll_entry>(1, params, par, proj_parm, -d100, d0, -d100);

                    // y0 ?
                     proj_parm.pj[0]->fwd(lp_lam, lp_phi, xy1_x, xy1_y); // zone 1
                     proj_parm.pj[2]->fwd(lp_lam, lp_phi, xy3_x, xy3_y); // zone 3
                    // y0 + xy1_y = xy3_y for lt = 40d44'11.8"
                    proj_parm.dy0 = xy3_y - xy1_y;

                    proj_parm.pj[0]->mutable_params().y0 = proj_parm.dy0;

                    // mollweide zones (cont'd)
                    do_setup<moll_entry>( 2, params, par, proj_parm,   d30,  proj_parm.dy0,   d30);
                    do_setup<moll_entry>( 9, params, par, proj_parm, -d160, -proj_parm.dy0, -d160);
                    do_setup<moll_entry>(10, params, par, proj_parm,  -d60, -proj_parm.dy0,  -d60);
                    do_setup<moll_entry>(11, params, par, proj_parm,   d20, -proj_parm.dy0,   d20);
                    do_setup<moll_entry>(12, params, par, proj_parm,  d140, -proj_parm.dy0,  d140);

                    // Already done before
                    //par.es = 0.;
            }

    }} // namespace detail::igh
    #endif // doxygen

    /*!
        \brief Interrupted Goode Homolosine projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Pseudocylindrical
         - Spheroid
        \par Example
        \image html ex_igh.gif
    */
    template <typename T, typename Parameters>
    struct igh_spheroid : public detail::igh::base_igh_spheroid<T, Parameters>
    {
        template <typename Params>
        inline igh_spheroid(Params const& params, Parameters const& par)
            : detail::igh::base_igh_spheroid<T, Parameters>(par)
        {
            detail::igh::setup_igh(params, this->m_par, this->m_proj_parm);
        }
    };

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail
    {

        // Static projection
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION(srs::spar::proj_igh, igh_spheroid, igh_spheroid)

        // Factory entry(s)
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_ENTRY_FI(igh_entry, igh_spheroid)

        BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_INIT_BEGIN(igh_init)
        {
            BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_INIT_ENTRY(igh, igh_entry)
        }

    } // namespace detail
    #endif // doxygen

} // namespace projections

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_PROJECTIONS_IGH_HPP


// This file automatically generated by create_export_module.py
#define NO_IMPORT_ARRAY

#include <NumpyEigenConverter.hpp>

#include <boost/cstdint.hpp>

void import_D_D_double()
{
	NumpyEigenConverter<Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > >::register_converter();
}

void import_D_1_double()
{
	NumpyEigenConverter<Eigen::Matrix< double, Eigen::Dynamic, 1 > >::register_converter();
}

void import_D_2_double()
{
	NumpyEigenConverter<Eigen::Matrix< double, Eigen::Dynamic, 2 > >::register_converter();
}

void import_D_3_double()
{
	NumpyEigenConverter<Eigen::Matrix< double, Eigen::Dynamic, 3 > >::register_converter();
}

void import_D_4_double()
{
	NumpyEigenConverter<Eigen::Matrix< double, Eigen::Dynamic, 4 > >::register_converter();
}

void import_D_5_double()
{
	NumpyEigenConverter<Eigen::Matrix< double, Eigen::Dynamic, 5 > >::register_converter();
}

void import_D_6_double()
{
	NumpyEigenConverter<Eigen::Matrix< double, Eigen::Dynamic, 6 > >::register_converter();
}

void import_1_D_double()
{
	NumpyEigenConverter<Eigen::Matrix< double, 1, Eigen::Dynamic > >::register_converter();
}

void import_1_1_double()
{
	NumpyEigenConverter<Eigen::Matrix< double, 1, 1 > >::register_converter();
}

void import_1_2_double()
{
	NumpyEigenConverter<Eigen::Matrix< double, 1, 2 > >::register_converter();
}

void import_1_3_double()
{
	NumpyEigenConverter<Eigen::Matrix< double, 1, 3 > >::register_converter();
}

void import_1_4_double()
{
	NumpyEigenConverter<Eigen::Matrix< double, 1, 4 > >::register_converter();
}

void import_1_5_double()
{
	NumpyEigenConverter<Eigen::Matrix< double, 1, 5 > >::register_converter();
}

void import_1_6_double()
{
	NumpyEigenConverter<Eigen::Matrix< double, 1, 6 > >::register_converter();
}

void import_2_D_double()
{
	NumpyEigenConverter<Eigen::Matrix< double, 2, Eigen::Dynamic > >::register_converter();
}

void import_2_1_double()
{
	NumpyEigenConverter<Eigen::Matrix< double, 2, 1 > >::register_converter();
}

void import_2_2_double()
{
	NumpyEigenConverter<Eigen::Matrix< double, 2, 2 > >::register_converter();
}

void import_2_3_double()
{
	NumpyEigenConverter<Eigen::Matrix< double, 2, 3 > >::register_converter();
}

void import_2_4_double()
{
	NumpyEigenConverter<Eigen::Matrix< double, 2, 4 > >::register_converter();
}

void import_2_5_double()
{
	NumpyEigenConverter<Eigen::Matrix< double, 2, 5 > >::register_converter();
}

void import_2_6_double()
{
	NumpyEigenConverter<Eigen::Matrix< double, 2, 6 > >::register_converter();
}

void import_3_D_double()
{
	NumpyEigenConverter<Eigen::Matrix< double, 3, Eigen::Dynamic > >::register_converter();
}

void import_3_1_double()
{
	NumpyEigenConverter<Eigen::Matrix< double, 3, 1 > >::register_converter();
}

void import_3_2_double()
{
	NumpyEigenConverter<Eigen::Matrix< double, 3, 2 > >::register_converter();
}

void import_3_3_double()
{
	NumpyEigenConverter<Eigen::Matrix< double, 3, 3 > >::register_converter();
}

void import_3_4_double()
{
	NumpyEigenConverter<Eigen::Matrix< double, 3, 4 > >::register_converter();
}

void import_3_5_double()
{
	NumpyEigenConverter<Eigen::Matrix< double, 3, 5 > >::register_converter();
}

void import_3_6_double()
{
	NumpyEigenConverter<Eigen::Matrix< double, 3, 6 > >::register_converter();
}

void import_4_D_double()
{
	NumpyEigenConverter<Eigen::Matrix< double, 4, Eigen::Dynamic > >::register_converter();
}

void import_4_1_double()
{
	NumpyEigenConverter<Eigen::Matrix< double, 4, 1 > >::register_converter();
}

void import_4_2_double()
{
	NumpyEigenConverter<Eigen::Matrix< double, 4, 2 > >::register_converter();
}

void import_4_3_double()
{
	NumpyEigenConverter<Eigen::Matrix< double, 4, 3 > >::register_converter();
}

void import_4_4_double()
{
	NumpyEigenConverter<Eigen::Matrix< double, 4, 4 > >::register_converter();
}

void import_4_5_double()
{
	NumpyEigenConverter<Eigen::Matrix< double, 4, 5 > >::register_converter();
}

void import_4_6_double()
{
	NumpyEigenConverter<Eigen::Matrix< double, 4, 6 > >::register_converter();
}

void import_5_D_double()
{
	NumpyEigenConverter<Eigen::Matrix< double, 5, Eigen::Dynamic > >::register_converter();
}

void import_5_1_double()
{
	NumpyEigenConverter<Eigen::Matrix< double, 5, 1 > >::register_converter();
}

void import_5_2_double()
{
	NumpyEigenConverter<Eigen::Matrix< double, 5, 2 > >::register_converter();
}

void import_5_3_double()
{
	NumpyEigenConverter<Eigen::Matrix< double, 5, 3 > >::register_converter();
}

void import_5_4_double()
{
	NumpyEigenConverter<Eigen::Matrix< double, 5, 4 > >::register_converter();
}

void import_5_5_double()
{
	NumpyEigenConverter<Eigen::Matrix< double, 5, 5 > >::register_converter();
}

void import_5_6_double()
{
	NumpyEigenConverter<Eigen::Matrix< double, 5, 6 > >::register_converter();
}

void import_6_D_double()
{
	NumpyEigenConverter<Eigen::Matrix< double, 6, Eigen::Dynamic > >::register_converter();
}

void import_6_1_double()
{
	NumpyEigenConverter<Eigen::Matrix< double, 6, 1 > >::register_converter();
}

void import_6_2_double()
{
	NumpyEigenConverter<Eigen::Matrix< double, 6, 2 > >::register_converter();
}

void import_6_3_double()
{
	NumpyEigenConverter<Eigen::Matrix< double, 6, 3 > >::register_converter();
}

void import_6_4_double()
{
	NumpyEigenConverter<Eigen::Matrix< double, 6, 4 > >::register_converter();
}

void import_6_5_double()
{
	NumpyEigenConverter<Eigen::Matrix< double, 6, 5 > >::register_converter();
}

void import_6_6_double()
{
	NumpyEigenConverter<Eigen::Matrix< double, 6, 6 > >::register_converter();
}


#ifndef MUSYCL_AUDIO_HPP
#define MUSYCL_AUDIO_HPP

/** \file SYCL abstraction for an audio pipe

    Based on RtAudio library.
*/

#include <array>
#include <cstdlib>
#include <iostream>
#include <string>

#include <sycl/sycl.hpp>

#include <boost/fiber/buffered_channel.hpp>

#include <range/v3/all.hpp>

#include "rtaudio/RtAudio.h"

#include "musycl/config.hpp"

namespace musycl {

/** An audio input/output interface exposed as a SYCL pipe.

    In SYCL the type is used to synthesize the connection between
    kernels, so there can be only 1 instance of a MIDI input
    interface. */
class audio {

public:

  /// Audio value type, with data in [ -1, +1 ]
  using value_type = double;

  /// Stereo mode: use 2 channels
  static constexpr auto channel_number = 2;

  /** Audio sample type

      In a stereo system, the element .x() or .s0() is the left voice
      and .y() or .s1() is the right voice */
  using sample_type = sycl::vec<value_type, channel_number>;

  /// The type of an audio frame
  /// \todo Use a movable type?
  using frame = std::array<sample_type, frame_size>;

private:

  /// Capacity of the MIDI message pipe
  static constexpr auto pipe_min_capacity = 2;

  /// The handler to control the audio input/output interface
  static inline std::optional<RtAudio> interface;

  /// A FIFO used to implement the pipe of MIDI messages
  static inline boost::fibers::buffered_channel<frame>
  output_frames { pipe_min_capacity };

  /// Check for RtAudio errors
  static constexpr auto check_error = [] (auto&& function) {
    try {
      return function();
    }
    catch (const RtAudioError &error) {
      error.printMessage();
      std::exit(EXIT_FAILURE);
    }
  };

/** Callback function used by RtAudio to read/write audio samples
*/
static inline int
audio_callback(void *output_buffer, void *input_buffer,
               unsigned int rtaudio_frame_size, double time_stamp,
               RtAudioStreamStatus status, void * /* user_data */) {
  if (status)
    std::cerr << "Stream underflow detected!" << std::endl;
  assert(rtaudio_frame_size == frame_size
         && "frame_size needs to be the same as the one used by RtAudio");
  // Copy 1 ready frame to the output
  ranges::copy(output_frames.value_pop(),
               static_cast<sample_type*>(output_buffer));
  // 0 to continue mormal operation
  return 0;
}

public:

  void open(const std::string& application_name,
            const std::string& port_name,
            const std::string& stream_name,
            RtAudio::Api backend) {
    check_error([&] { interface.emplace(backend); });
    auto device = interface->getDefaultOutputDevice();

    RtAudio::StreamParameters parameters;
    parameters.deviceId = device;
    parameters.nChannels = channel_number;
    // Use channel(s) starting at 0
    parameters.firstChannel = 0;

    RtAudio::StreamOptions options;
    options.streamName = stream_name;
    auto sample_rate = interface->getDeviceInfo(device).preferredSampleRate;
    if (sample_rate != sample_frequency) {
      std::cerr << "Warning: the preferred sample rate " << sample_rate
                << " of the audio interface is not the same as the one "
        "configured in musycl/config.hpp so the quality might be reduced."
                << std::endl;
      // Forcing the sampling frequency anyway
      sample_rate = sample_frequency;
    }
    unsigned int actual_frame_size = frame_size;

    check_error([&] {
      interface->openStream(&parameters, nullptr, RTAUDIO_FLOAT64, sample_rate,
                            &actual_frame_size, audio_callback, nullptr, &options,
                            [] (RtAudioError::Type type,
                                const std::string &error_text) {
                              std::cerr << error_text << std::endl;
                            });
    });
    if (sample_rate != sample_frequency
        && actual_frame_size != frame_size) {
      std::cerr << "Actual sample rate: " << sample_rate
                << "Requested sample rate: " << sample_frequency
                << "\nActual samples per frame: " << actual_frame_size
                << "\nRequested samples per frame: " << frame_size
                << '.' << std::endl
                << "Please update musycl/config.hpp accordingly." << std::endl;
      std::terminate();
    }
    // Start the audio streaming
    check_error([&] { interface->startStream(); });
  }

  /// The sycl::pipe::write-like interface to write a MIDI message
  template <typename MusyclAudioSample>
  static inline void write(MusyclAudioSample&& s) {
    // Check that the output lands in the authorized values
    auto min = ranges::min(ranges::views::transform
                           (s, [] (auto e) { return ranges::min(e); }));
    auto max = ranges::max(ranges::views::transform
                           (s, [] (auto e) { return ranges::max(e); }));
    if (min < -1)
      std::cerr << "Min saturation detected: " << min;
    if (max > 1)
      std::cerr << "Max saturation detected: " << max;

    output_frames.push(std::forward<MusyclAudioSample>(s));
  }
};

}

#endif // MUSYCL_AUDIO_HPP


// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2007-2008 Steven Watanabe
//
// 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_UNITS_DETAIL_CONVERSION_IMPL_HPP
#define BOOST_UNITS_DETAIL_CONVERSION_IMPL_HPP

#include <boost/mpl/bool.hpp>
#include <boost/mpl/and.hpp>
#include <boost/mpl/divides.hpp>
#include <boost/preprocessor/seq/enum.hpp>
#include <boost/type_traits/is_same.hpp>

#include <boost/units/heterogeneous_system.hpp>
#include <boost/units/homogeneous_system.hpp>
#include <boost/units/reduce_unit.hpp>
#include <boost/units/static_rational.hpp>
#include <boost/units/units_fwd.hpp>
#include <boost/units/detail/dimension_list.hpp>
#include <boost/units/detail/heterogeneous_conversion.hpp>
#include <boost/units/detail/one.hpp>
#include <boost/units/detail/static_rational_power.hpp>
#include <boost/units/detail/unscale.hpp>

#include <boost/units/units_fwd.hpp>

namespace boost {

namespace units {

namespace detail {

template<class Source, class Dest>
struct conversion_factor_helper;

template<class Source, class Dest>
struct call_base_unit_converter;

}

/// INTERNAL ONLY
struct undefined_base_unit_converter_base {
    static const bool is_defined = false;
};

/// INTERNAL ONLY
struct no_default_conversion {
    static const bool is_defined = false;
};

/// INTERNAL ONLY
template<class BaseUnit>
struct unscaled_get_default_conversion : no_default_conversion { };

/// INTERNAL ONLY
template<bool is_defined>
struct unscaled_get_default_conversion_impl;

/// INTERNAL ONLY
template<>
struct unscaled_get_default_conversion_impl<true>
{
    template<class T>
    struct apply
    {
        typedef typename unscaled_get_default_conversion<typename unscale<T>::type>::type type;
    };
};

/// INTERNAL ONLY
template<>
struct unscaled_get_default_conversion_impl<false>
{
    template<class T>
    struct apply
    {
        typedef typename T::unit_type type;
    };
};

/// INTERNAL ONLY
template<class BaseUnit>
struct get_default_conversion
{
    typedef typename unscaled_get_default_conversion_impl<
        unscaled_get_default_conversion<typename unscale<BaseUnit>::type>::is_defined
    >::template apply<BaseUnit>::type type;
};

/// INTERNAL ONLY
template<class Source, class Destination>
struct select_base_unit_converter
{
    typedef Source source_type;
    typedef Destination destination_type;
};

/// INTERNAL ONLY
template<class Source, class Dest>
struct base_unit_converter_base : undefined_base_unit_converter_base {
};

/// INTERNAL ONLY
template<class Source>
struct base_unit_converter_base<Source, BOOST_UNITS_MAKE_HETEROGENEOUS_UNIT(Source, typename Source::dimension_type)>
{
    static const bool is_defined = true;
    typedef one type;
    static type value() {
        one result;
        return(result);
    }
};

/// INTERNAL ONLY
template<class Source, class Dest>
struct base_unit_converter : base_unit_converter_base<Source, Dest> { };

namespace detail {

template<class Source, class Dest>
struct do_call_base_unit_converter {
    typedef select_base_unit_converter<typename unscale<Source>::type, typename unscale<Dest>::type> selector;
    typedef typename selector::source_type source_type;
    typedef typename selector::destination_type destination_type;
    typedef base_unit_converter<source_type, destination_type> converter;
    typedef typename mpl::divides<typename get_scale_list<Source>::type, typename get_scale_list<source_type>::type>::type source_factor;
    typedef typename mpl::divides<typename get_scale_list<Dest>::type, typename get_scale_list<destination_type>::type>::type destination_factor;
    typedef typename mpl::divides<source_factor, destination_factor>::type factor;
    typedef eval_scale_list<factor> eval_factor;
    typedef typename multiply_typeof_helper<typename converter::type, typename eval_factor::type>::type type;
    static type value()
    {
        return(converter::value() * eval_factor::value());
    }
};

template<bool forward_is_defined, bool reverse_is_defined>
struct call_base_unit_converter_base_unit_impl;

template<>
struct call_base_unit_converter_base_unit_impl<true, true>
{
    template<class Source, class Dest>
    struct apply
        : do_call_base_unit_converter<Source, typename Dest::unit_type>
    {
    };
};

template<>
struct call_base_unit_converter_base_unit_impl<true, false>
{
    template<class Source, class Dest>
    struct apply
        : do_call_base_unit_converter<Source, typename Dest::unit_type>
    {
    };
};

template<>
struct call_base_unit_converter_base_unit_impl<false, true>
{
    template<class Source, class Dest>
    struct apply
    {
        typedef do_call_base_unit_converter<Dest, typename Source::unit_type> converter;
        typedef typename divide_typeof_helper<one, typename converter::type>::type type;
        static type value() {
            one numerator;
            return(numerator / converter::value());
        }
    };
};

template<>
struct call_base_unit_converter_base_unit_impl<false, false>
{
    template<class Source, class Dest>
    struct apply
    {
        typedef typename reduce_unit<typename get_default_conversion<Source>::type>::type new_source;
        typedef typename reduce_unit<typename get_default_conversion<Dest>::type>::type new_dest;
        typedef call_base_unit_converter<Source, new_source> start;
        typedef detail::conversion_factor_helper<
            new_source,
            new_dest
        > conversion;
        typedef call_base_unit_converter<Dest, new_dest> end;
        typedef typename divide_typeof_helper<
            typename multiply_typeof_helper<
                typename start::type,
                typename conversion::type
            >::type,
            typename end::type
        >::type type;
        static type value() {
            return(start::value() * conversion::value() / end::value());
        }
    };
};

template<int N>
struct get_default_conversion_impl
{
    template<class Begin>
    struct apply
    {
        typedef typename Begin::item source_pair;
        typedef typename source_pair::value_type exponent;
        typedef typename source_pair::tag_type source;
        typedef typename get_default_conversion<source>::type new_source;
        typedef typename get_default_conversion_impl<N-1>::template apply<typename Begin::next> next_iteration;
        typedef typename multiply_typeof_helper<typename power_typeof_helper<new_source, exponent>::type, typename next_iteration::unit_type>::type unit_type;
        typedef call_base_unit_converter<source, new_source> conversion;
        typedef typename multiply_typeof_helper<typename conversion::type, typename next_iteration::type>::type type;
        static type value() {
            return(static_rational_power<exponent>(conversion::value()) * next_iteration::value());
        }
    };
};

template<>
struct get_default_conversion_impl<0>
{
    template<class Begin>
    struct apply
    {
        typedef unit<dimensionless_type, heterogeneous_system<heterogeneous_system_impl<dimensionless_type, dimensionless_type, no_scale> > > unit_type;
        typedef one type;
        static one value() {
            one result;
            return(result);
        }
    };
};

template<bool is_defined>
struct call_base_unit_converter_impl;

template<>
struct call_base_unit_converter_impl<true>
{
    template<class Source, class Dest>
    struct apply
        : do_call_base_unit_converter<Source, Dest>
    {
    };
};

template<>
struct call_base_unit_converter_impl<false>
{
    template<class Source, class Dest>
    struct apply {
        typedef typename reduce_unit<typename get_default_conversion<Source>::type>::type new_source;
        typedef typename Dest::system_type::type system_list;
        typedef typename get_default_conversion_impl<system_list::size::value>::template apply<system_list> impl;
        typedef typename impl::unit_type new_dest;
        typedef call_base_unit_converter<Source, new_source> start;
        typedef conversion_factor_helper<new_source, new_dest> conversion;
        typedef typename divide_typeof_helper<
            typename multiply_typeof_helper<
                typename start::type,
                typename conversion::type
            >::type,
            typename impl::type
        >::type type;
        static type value() {
            return(start::value() * conversion::value() / impl::value());
        }
    };
};

#define BOOST_UNITS_DETAIL_BASE_UNIT_CONVERTER_IS_DEFINED(Source, Dest)\
    base_unit_converter<\
        typename select_base_unit_converter<typename unscale<Source>::type, typename unscale<Dest>::type>::source_type,\
        typename select_base_unit_converter<typename unscale<Source>::type, typename unscale<Dest>::type>::destination_type\
    >::is_defined

template<class Source, class Dest>
struct call_base_unit_converter : call_base_unit_converter_impl<BOOST_UNITS_DETAIL_BASE_UNIT_CONVERTER_IS_DEFINED(Source, Dest)>::template apply<Source, Dest>
{
};

template<class Source, class Dest>
struct call_base_unit_converter<Source, BOOST_UNITS_MAKE_HETEROGENEOUS_UNIT(Dest, typename Source::dimension_type)> :
    call_base_unit_converter_base_unit_impl<
        BOOST_UNITS_DETAIL_BASE_UNIT_CONVERTER_IS_DEFINED(Source, typename Dest::unit_type),
        BOOST_UNITS_DETAIL_BASE_UNIT_CONVERTER_IS_DEFINED(Dest, typename Source::unit_type)
    >::template apply<Source, Dest>
{
};

template<int N>
struct conversion_impl
{
    template<class Begin, class DestinationSystem>
    struct apply
    {
        typedef typename conversion_impl<N-1>::template apply<
            typename Begin::next,
            DestinationSystem
        > next_iteration;
        typedef typename Begin::item unit_pair;
        typedef typename unit_pair::tag_type unit;
        typedef typename unit::dimension_type dimensions;
        typedef typename reduce_unit<units::unit<dimensions, DestinationSystem> >::type reduced_unit;
        typedef detail::call_base_unit_converter<unit, reduced_unit> converter;
        typedef typename multiply_typeof_helper<typename converter::type, typename next_iteration::type>::type type;
        static type value() { return(static_rational_power<typename unit_pair::value_type>(converter::value()) * next_iteration::value()); }
    };
};

template<>
struct conversion_impl<0>
{
    template<class Begin, class DestinationSystem>
    struct apply
    {
        typedef one type;
        static type value() { one result; return(result); }
    };
};

} // namespace detail

/// conversions between homogeneous systems are defined
/// INTERNAL ONLY
template<class D, class L1, class T1, class L2, class T2>
struct conversion_helper<quantity<unit<D, homogeneous_system<L1> >, T1>, quantity<unit<D, homogeneous_system<L2> >, T2> >
{
    /// INTERNAL ONLY
    typedef quantity<unit<D, homogeneous_system<L2> >, T2> destination_type;
    /// INTERNAL ONLY
    typedef typename reduce_unit<unit<D, homogeneous_system<L1> > >::type source_unit;
    /// INTERNAL ONLY
    typedef typename source_unit::system_type::type unit_list;
    static destination_type convert(const quantity<unit<D, homogeneous_system<L1> >, T1>& source)
    {
        return(destination_type::from_value(source.value() *
            detail::conversion_impl<unit_list::size::value>::template apply<
                unit_list,
                homogeneous_system<L2>
            >::value()
            ));
    }
};

/// conversions between heterogeneous systems and homogeneous systems are defined
/// INTERNAL ONLY
template<class D, class L1, class T1, class L2, class T2>
struct conversion_helper<quantity<unit<D, heterogeneous_system<L1> >, T1>, quantity<unit<D, homogeneous_system<L2> >, T2> >
{
    /// INTERNAL ONLY
    typedef quantity<unit<D, homogeneous_system<L2> >, T2> destination_type;
    static destination_type convert(const quantity<unit<D, heterogeneous_system<L1> >, T1>& source)
    {
        return(destination_type::from_value(source.value() *
            detail::conversion_impl<L1::type::size::value>::template apply<
                typename L1::type,
                homogeneous_system<L2>
            >::value() *
            eval_scale_list<typename L1::scale>::value()
            ));
    }
};

// There is no simple algorithm for doing this conversion
// other than just defining it as the reverse of the
// heterogeneous->homogeneous case
/// conversions between heterogeneous systems and homogeneous systems are defined
/// INTERNAL ONLY
template<class D, class L1, class T1, class L2, class T2>
struct conversion_helper<quantity<unit<D, homogeneous_system<L1> >, T1>, quantity<unit<D, heterogeneous_system<L2> >, T2> >
{
    /// INTERNAL ONLY
    typedef quantity<unit<D, heterogeneous_system<L2> >, T2> destination_type;
    static destination_type convert(const quantity<unit<D, homogeneous_system<L1> >, T1>& source)
    {
        return(destination_type::from_value(source.value() /
            (detail::conversion_impl<L2::type::size::value>::template apply<
                typename L2::type,
                homogeneous_system<L1>
            >::value() *
            eval_scale_list<typename L2::scale>::value()
            )
            ));
    }
};

/// Requires that all possible conversions
/// between base units are defined.
/// INTERNAL ONLY
template<class D, class S1, class T1, class S2, class T2>
struct conversion_helper<quantity<unit<D, heterogeneous_system<S1> >, T1>, quantity<unit<D, heterogeneous_system<S2> >, T2> >
{
    /// INTERNAL ONLY
    typedef quantity<unit<D, heterogeneous_system<S1> >, T1> source_type;
    /// INTERNAL ONLY
    typedef quantity<unit<D, heterogeneous_system<S2> >, T2> destination_type;
    /// INTERNAL ONLY
    typedef typename detail::extract_base_units<S1::type::size::value>::template apply<
        typename S1::type,
        dimensionless_type
    >::type from_base_units;
    /// INTERNAL ONLY
    typedef typename detail::extract_base_units<S2::type::size::value>::template apply<
        typename S2::type,
        from_base_units
    >::type all_base_units;
    /// INTERNAL ONLY
    typedef typename detail::make_homogeneous_system<all_base_units>::type system;
    /// INTERNAL ONLY
    typedef typename mpl::divides<typename S1::scale, typename S2::scale>::type result_scale;
    static destination_type convert(const source_type& source)
    {
        return(destination_type::from_value(source.value() *
            (detail::conversion_impl<S1::type::size::value>::template apply<
                typename S1::type,
                system
            >::value() * eval_scale_list<result_scale>::value() /
            detail::conversion_impl<S2::type::size::value>::template apply<
                typename S2::type,
                system
            >::value()
            )
            ));
    }
};

namespace detail {

template<class Source, class Dest>
struct conversion_factor_helper;

template<class D, class L1, class L2>
struct conversion_factor_helper<unit<D, homogeneous_system<L1> >, unit<D, homogeneous_system<L2> > >
{
    typedef typename reduce_unit<unit<D, homogeneous_system<L1> > >::type source_unit;
    typedef typename source_unit::system_type::type unit_list;
    typedef typename detail::conversion_impl<unit_list::size::value>::template apply<
        unit_list,
        homogeneous_system<L2>
    > impl;
    typedef typename impl::type type;
    static type value()
    {
        return(impl::value());
    }
};

template<class D, class L1, class L2>
struct conversion_factor_helper<unit<D, heterogeneous_system<L1> >, unit<D, homogeneous_system<L2> > >
{
    typedef typename detail::conversion_impl<L1::type::size::value>::template apply<
        typename L1::type,
        homogeneous_system<L2>
    > impl;
    typedef eval_scale_list<typename L1::scale> scale;
    typedef typename multiply_typeof_helper<typename impl::type, typename scale::type>::type type;
    static type value()
    {
        return(impl::value() * scale::value());
    }
};

// There is no simple algorithm for doing this conversion
// other than just defining it as the reverse of the
// heterogeneous->homogeneous case
template<class D, class L1, class L2>
struct conversion_factor_helper<unit<D, homogeneous_system<L1> >, unit<D, heterogeneous_system<L2> > >
{
    typedef typename detail::conversion_impl<L2::type::size::value>::template apply<
        typename L2::type,
        homogeneous_system<L1>
    > impl;
    typedef eval_scale_list<typename L2::scale> scale;
    typedef typename multiply_typeof_helper<typename impl::type, typename scale::type>::type type;
    static type value()
    {
        one numerator;
        return(numerator / (impl::value() * scale::value()));
    }
};

/// Requires that all possible conversions
/// between base units are defined.
template<class D, class S1, class S2>
struct conversion_factor_helper<unit<D, heterogeneous_system<S1> >, unit<D, heterogeneous_system<S2> > >
{
    /// INTERNAL ONLY
    typedef typename detail::extract_base_units<S1::type::size::value>::template apply<
        typename S1::type,
        dimensionless_type
    >::type from_base_units;
    /// INTERNAL ONLY
    typedef typename detail::extract_base_units<S2::type::size::value>::template apply<
        typename S2::type,
        from_base_units
    >::type all_base_units;
    /// INTERNAL ONLY
    typedef typename detail::make_homogeneous_system<all_base_units>::type system;
    typedef typename detail::conversion_impl<S1::type::size::value>::template apply<
        typename S1::type,
        system
    > conversion1;
    typedef typename detail::conversion_impl<S2::type::size::value>::template apply<
        typename S2::type,
        system
    > conversion2;
    typedef eval_scale_list<typename mpl::divides<typename S1::scale, typename S2::scale>::type> scale;
    typedef typename multiply_typeof_helper<
        typename conversion1::type,
        typename divide_typeof_helper<typename scale::type, typename conversion2::type>::type
    >::type type;
    static type value()
    {
        return(conversion1::value() * (scale::value() / conversion2::value()));
    }
};

} // namespace detail

} // namespace units

} // namespace boost

#endif // BOOST_UNITS_CONVERSION_IMPL_HPP