/*
 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 portfolio/legdata.hpp
    \brief leg data model and serialization
    \ingroup tradedata
*/

#pragma once

#include <boost/make_shared.hpp>
#include <ored/portfolio/enginefactory.hpp>
#include <ored/portfolio/schedule.hpp>
#include <ored/utilities/parsers.hpp>

#include <ql/cashflow.hpp>
#include <ql/experimental/coupons/swapspreadindex.hpp>
#include <ql/indexes/iborindex.hpp>
#include <qle/indexes/bmaindexwrapper.hpp>
#include <qle/indexes/equityindex.hpp>

#include <vector>

namespace ore {
namespace data {
using namespace QuantLib;
using std::string;

//! Serializable Additional Leg Data
/*!
\ingroup tradedata
*/

// Really bad name....
class LegAdditionalData : public XMLSerializable {
public:
    LegAdditionalData(const string& legType, const string& legNodeName)
        : legType_(legType), legNodeName_(legNodeName) {}
    LegAdditionalData(const string& legType) : legType_(legType), legNodeName_(legType + "LegData") {}

    const string& legType() const { return legType_; }
    const string& legNodeName() const { return legNodeName_; }

private:
    string legType_;
    string legNodeName_; // the XML node name
};

//! Serializable Cashflow Leg Data
/*!
  \ingroup tradedata
*/

class CashflowData : public LegAdditionalData {
public:
    //! Default constructor
    CashflowData() : LegAdditionalData("Cashflow", "CashflowData") {}
    //! Constructor
    CashflowData(const vector<double>& amounts, const vector<string>& dates)
        : LegAdditionalData("Cashflow", "CashflowData"), amounts_(amounts), dates_(dates) {}

    //! \name Inspectors
    //@{
    const vector<double>& amounts() const { return amounts_; }
    const vector<string>& dates() const { return dates_; }
    //@}

    //! \name Serialisation
    //@{
    void fromXML(XMLNode* node);
    XMLNode* toXML(XMLDocument& doc);
    //@}
private:
    vector<double> amounts_;
    vector<string> dates_;
};

//! Serializable Fixed Leg Data
/*!
  \ingroup tradedata
*/
class FixedLegData : public LegAdditionalData {
public:
    //! Default constructor
    FixedLegData() : LegAdditionalData("Fixed") {}
    //! Constructor
    FixedLegData(const vector<double>& rates, const vector<string>& rateDates = vector<string>())
        : LegAdditionalData("Fixed"), rates_(rates), rateDates_(rateDates) {}

    //! \name Inspectors
    //@{
    const vector<double>& rates() const { return rates_; }
    const vector<string>& rateDates() const { return rateDates_; }
    //@}

    //! \name Serialisation
    //@{
    virtual void fromXML(XMLNode* node);
    virtual XMLNode* toXML(XMLDocument& doc);
    //@}
private:
    vector<double> rates_;
    vector<string> rateDates_;
};

//! Serializable Fixed Leg Data
/*!
  \ingroup tradedata
*/
class ZeroCouponFixedLegData : public LegAdditionalData {
public:
    //! Default constructor
    ZeroCouponFixedLegData() : LegAdditionalData("ZeroCouponFixed") {}
    //! Constructor
    ZeroCouponFixedLegData(const Rate& rate, const int& years)
        : LegAdditionalData("ZeroCouponFixed"), rate_(rate), years_(years) {}

    //! \name Inspectors
    //@{
    const Rate& rate() const { return rate_; }
    const int& years() const { return years_; }
    //@}

    //! \name Serialisation
    //@{
    virtual void fromXML(XMLNode* node);
    virtual XMLNode* toXML(XMLDocument& doc);
    //@}
private:
    Rate rate_;
    int years_;
};

//! Serializable Floating Leg Data
/*!
  \ingroup tradedata
*/
class FloatingLegData : public LegAdditionalData {
public:
    //! Default constructor
    FloatingLegData() : LegAdditionalData("Floating"), fixingDays_(0), isInArrears_(true), nakedOption_(false) {}
    //! Constructor
    FloatingLegData(const string& index, QuantLib::Natural fixingDays, bool isInArrears, const vector<double>& spreads,
                    const vector<string>& spreadDates = vector<string>(), const vector<double>& caps = vector<double>(),
                    const vector<string>& capDates = vector<string>(), const vector<double>& floors = vector<double>(),
                    const vector<string>& floorDates = vector<string>(),
                    const vector<double>& gearings = vector<double>(),
                    const vector<string>& gearingDates = vector<string>(), bool isAveraged = false,
                    bool nakedOption = false)
        : LegAdditionalData("Floating"), index_(index), fixingDays_(fixingDays), isInArrears_(isInArrears),
          isAveraged_(isAveraged), spreads_(spreads), spreadDates_(spreadDates), caps_(caps), capDates_(capDates),
          floors_(floors), floorDates_(floorDates), gearings_(gearings), gearingDates_(gearingDates),
          nakedOption_(nakedOption) {}

    //! \name Inspectors
    //@{
    const string& index() const { return index_; }
    QuantLib::Natural fixingDays() const { return fixingDays_; }
    bool isInArrears() const { return isInArrears_; }
    bool isAveraged() const { return isAveraged_; }
    const vector<double>& spreads() const { return spreads_; }
    const vector<string>& spreadDates() const { return spreadDates_; }
    const vector<double>& caps() const { return caps_; }
    const vector<string>& capDates() const { return capDates_; }
    const vector<double>& floors() const { return floors_; }
    const vector<string>& floorDates() const { return floorDates_; }
    const vector<double>& gearings() const { return gearings_; }
    const vector<string>& gearingDates() const { return gearingDates_; }
    bool nakedOption() const { return nakedOption_; }
    //@}

    //! \name Serialisation
    //@{
    virtual void fromXML(XMLNode* node);
    virtual XMLNode* toXML(XMLDocument& doc);
    //@}
private:
    string index_;
    QuantLib::Natural fixingDays_;
    bool isInArrears_;
    bool isAveraged_;
    vector<double> spreads_;
    vector<string> spreadDates_;
    vector<double> caps_;
    vector<string> capDates_;
    vector<double> floors_;
    vector<string> floorDates_;
    vector<double> gearings_;
    vector<string> gearingDates_;
    bool nakedOption_;
};

//! Serializable CPI Leg Data
/*!
\ingroup tradedata
*/

class CPILegData : public LegAdditionalData {
public:
    //! Default constructor
    CPILegData() : LegAdditionalData("CPI") {}
    //! Constructor
    CPILegData(string index, double baseCPI, string observationLag, bool interpolated, const vector<double>& rates,
               const vector<string>& rateDates = std::vector<string>(), bool subtractInflationNominal = true)
        : LegAdditionalData("CPI"), index_(index), baseCPI_(baseCPI), observationLag_(observationLag),
          interpolated_(interpolated), rates_(rates), rateDates_(rateDates),
          subtractInflationNominal_(subtractInflationNominal) {}

    //! \name Inspectors
    //@{
    const string index() const { return index_; }
    double baseCPI() const { return baseCPI_; }
    const string observationLag() const { return observationLag_; }
    bool interpolated() const { return interpolated_; }
    const std::vector<double>& rates() const { return rates_; }
    const std::vector<string>& rateDates() const { return rateDates_; }
    bool subtractInflationNominal() const { return subtractInflationNominal_; }
    //@}

    //! \name Serialisation
    //@{
    virtual void fromXML(XMLNode* node);
    virtual XMLNode* toXML(XMLDocument& doc);
    //@}
private:
    string index_;
    double baseCPI_;
    string observationLag_;
    bool interpolated_;
    vector<double> rates_;
    vector<string> rateDates_;
    bool subtractInflationNominal_;
};

//! Serializable YoY Leg Data
/*!
\ingroup tradedata
*/
class YoYLegData : public LegAdditionalData {
public:
    //! Default constructor
    YoYLegData() : LegAdditionalData("YY") {}
    //! Constructor
    YoYLegData(string index, string observationLag, bool interpolated, Size fixingDays,
               const vector<double>& gearings = std::vector<double>(),
               const vector<string>& gearingDates = std::vector<string>(),
               const vector<double>& spreads = std::vector<double>(),
               const vector<string>& spreadDates = std::vector<string>())
        : LegAdditionalData("YY"), index_(index), observationLag_(observationLag), interpolated_(interpolated),
          fixingDays_(fixingDays), gearings_(gearings), gearingDates_(gearingDates), spreads_(spreads),
          spreadDates_(spreadDates) {}

    //! \name Inspectors
    //@{
    const string index() const { return index_; }
    const string observationLag() const { return observationLag_; }
    bool interpolated() const { return interpolated_; }
    Size fixingDays() const { return fixingDays_; }
    const std::vector<double>& gearings() const { return gearings_; }
    const std::vector<string>& gearingDates() const { return gearingDates_; }
    const std::vector<double>& spreads() const { return spreads_; }
    const std::vector<string>& spreadDates() const { return spreadDates_; }
    //@}

    //! \name Serialisation
    //@{
    virtual void fromXML(XMLNode* node);
    virtual XMLNode* toXML(XMLDocument& doc);
    //@}

private:
    string index_;
    string observationLag_;
    bool interpolated_;
    Size fixingDays_;
    vector<double> gearings_;
    vector<string> gearingDates_;
    vector<double> spreads_;
    vector<string> spreadDates_;
};

//! Serializable CMS Leg Data
/*!
\ingroup tradedata
*/
class CMSLegData : public LegAdditionalData {
public:
    //! Default constructor
    CMSLegData() : LegAdditionalData("CMS"), fixingDays_(0), isInArrears_(true), nakedOption_(false) {}
    //! Constructor
    CMSLegData(const string& swapIndex, int fixingDays, bool isInArrears, const vector<double>& spreads,
               const vector<string>& spreadDates = vector<string>(), const vector<double>& caps = vector<double>(),
               const vector<string>& capDates = vector<string>(), const vector<double>& floors = vector<double>(),
               const vector<string>& floorDates = vector<string>(), const vector<double>& gearings = vector<double>(),
               const vector<string>& gearingDates = vector<string>(), bool nakedOption = false)
        : LegAdditionalData("CMS"), swapIndex_(swapIndex), fixingDays_(fixingDays), isInArrears_(isInArrears),
          spreads_(spreads), spreadDates_(spreadDates), caps_(caps), capDates_(capDates), floors_(floors),
          floorDates_(floorDates), gearings_(gearings), gearingDates_(gearingDates), nakedOption_(nakedOption) {}

    //! \name Inspectors
    //@{
    const string& swapIndex() const { return swapIndex_; }
    int fixingDays() const { return fixingDays_; }
    bool isInArrears() const { return isInArrears_; }
    const vector<double>& spreads() const { return spreads_; }
    const vector<string>& spreadDates() const { return spreadDates_; }
    const vector<double>& caps() const { return caps_; }
    const vector<string>& capDates() const { return capDates_; }
    const vector<double>& floors() const { return floors_; }
    const vector<string>& floorDates() const { return floorDates_; }
    const vector<double>& gearings() const { return gearings_; }
    const vector<string>& gearingDates() const { return gearingDates_; }
    bool nakedOption() const { return nakedOption_; }
    //@}

    //! \name Serialisation
    //@{
    virtual void fromXML(XMLNode* node);
    virtual XMLNode* toXML(XMLDocument& doc);
    //@}
private:
    string swapIndex_;
    int fixingDays_;
    bool isInArrears_;
    vector<double> spreads_;
    vector<string> spreadDates_;
    vector<double> caps_;
    vector<string> capDates_;
    vector<double> floors_;
    vector<string> floorDates_;
    vector<double> gearings_;
    vector<string> gearingDates_;
    bool nakedOption_;
};

//! Serializable CMS Spread Leg Data
/*!
\ingroup tradedata
*/
class CMSSpreadLegData : public LegAdditionalData {
public:
    //! Default constructor
    CMSSpreadLegData() : LegAdditionalData("CMSSpread"), fixingDays_(0), isInArrears_(true), nakedOption_(false) {}
    //! Constructor
    CMSSpreadLegData(const string& swapIndex1, const string& swapIndex2, int fixingDays, bool isInArrears,
                     const vector<double>& spreads, const vector<string>& spreadDates = vector<string>(),
                     const vector<double>& caps = vector<double>(), const vector<string>& capDates = vector<string>(),
                     const vector<double>& floors = vector<double>(),
                     const vector<string>& floorDates = vector<string>(),
                     const vector<double>& gearings = vector<double>(),
                     const vector<string>& gearingDates = vector<string>(), bool nakedOption = false)
        : LegAdditionalData("CMSSpread"), swapIndex1_(swapIndex1), swapIndex2_(swapIndex2), fixingDays_(fixingDays),
          isInArrears_(isInArrears), spreads_(spreads), spreadDates_(spreadDates), caps_(caps), capDates_(capDates),
          floors_(floors), floorDates_(floorDates), gearings_(gearings), gearingDates_(gearingDates),
          nakedOption_(nakedOption) {}

    //! \name Inspectors
    //@{
    const string& swapIndex1() const { return swapIndex1_; }
    const string& swapIndex2() const { return swapIndex2_; }
    int fixingDays() const { return fixingDays_; }
    bool isInArrears() const { return isInArrears_; }
    const vector<double>& spreads() const { return spreads_; }
    const vector<string>& spreadDates() const { return spreadDates_; }
    const vector<double>& caps() const { return caps_; }
    const vector<string>& capDates() const { return capDates_; }
    const vector<double>& floors() const { return floors_; }
    const vector<string>& floorDates() const { return floorDates_; }
    const vector<double>& gearings() const { return gearings_; }
    const vector<string>& gearingDates() const { return gearingDates_; }
    bool nakedOption() const { return nakedOption_; }
    //@}

    //! \name Serialisation
    //@{
    virtual void fromXML(XMLNode* node);
    virtual XMLNode* toXML(XMLDocument& doc);
    //@}
private:
    string swapIndex1_;
    string swapIndex2_;
    int fixingDays_;
    bool isInArrears_;
    vector<double> spreads_;
    vector<string> spreadDates_;
    vector<double> caps_;
    vector<string> capDates_;
    vector<double> floors_;
    vector<string> floorDates_;
    vector<double> gearings_;
    vector<string> gearingDates_;
    bool nakedOption_;
};

//! Serializable CMS Spread Leg Data
/*!
\ingroup tradedata
*/
class DigitalCMSSpreadLegData : public LegAdditionalData {
public:
    //! Default constructor
    DigitalCMSSpreadLegData() : LegAdditionalData("DigitalCMSSpread") {}
    //! Constructor
    DigitalCMSSpreadLegData(
        const boost::shared_ptr<CMSSpreadLegData>& underlying, Position::Type callPosition = Position::Long,
        bool isCallATMIncluded = false, const vector<double> callStrikes = vector<double>(),
        const vector<string> callStrikeDates = vector<string>(), const vector<double> callPayoffs = vector<double>(),
        const vector<string> callPayoffDates = vector<string>(), Position::Type putPosition = Position::Long,
        bool isPutATMIncluded = false, const vector<double> putStrikes = vector<double>(),
        const vector<string> putStrikeDates = vector<string>(), const vector<double> putPayoffs = vector<double>(),
        const vector<string> putPayoffDates = vector<string>())
        : LegAdditionalData("DigitalCMSSpread"), underlying_(underlying), callPosition_(callPosition),
          isCallATMIncluded_(isCallATMIncluded), callStrikes_(callStrikes), callStrikeDates_(callStrikeDates),
          callPayoffs_(callPayoffs), callPayoffDates_(callPayoffDates), putPosition_(putPosition),
          isPutATMIncluded_(isPutATMIncluded), putStrikes_(putStrikes), putStrikeDates_(putStrikeDates),
          putPayoffs_(putPayoffs), putPayoffDates_(putPayoffDates) {}

    //! \name Inspectors
    //@{
    const boost::shared_ptr<CMSSpreadLegData>& underlying() const { return underlying_; }

    const Position::Type callPosition() const { return callPosition_; }
    const bool isCallATMIncluded() const { return isCallATMIncluded_; }
    const vector<double> callStrikes() const { return callStrikes_; }
    const vector<double> callPayoffs() const { return callPayoffs_; }
    const vector<string> callStrikeDates() const { return callStrikeDates_; }
    const vector<string> callPayoffDates() const { return callPayoffDates_; }

    const Position::Type putPosition() const { return putPosition_; }
    const bool isPutATMIncluded() const { return isPutATMIncluded_; }
    const vector<double> putStrikes() const { return putStrikes_; }
    const vector<double> putPayoffs() const { return putPayoffs_; }
    const vector<string> putStrikeDates() const { return putStrikeDates_; }
    const vector<string> putPayoffDates() const { return putPayoffDates_; }
    //@}

    //! \name Serialisation
    //@{
    virtual void fromXML(XMLNode* node);
    virtual XMLNode* toXML(XMLDocument& doc);
    //@}
private:
    boost::shared_ptr<CMSSpreadLegData> underlying_;

    Position::Type callPosition_;
    bool isCallATMIncluded_;
    vector<double> callStrikes_;
    vector<string> callStrikeDates_;
    vector<double> callPayoffs_;
    vector<string> callPayoffDates_;

    Position::Type putPosition_;
    bool isPutATMIncluded_;
    vector<double> putStrikes_;
    vector<string> putStrikeDates_;
    vector<double> putPayoffs_;
    vector<string> putPayoffDates_;
};

//! Serializable Fixed Leg Data
/*!
\ingroup tradedata
*/
class EquityLegData : public LegAdditionalData {
public:
    //! Default constructor
    EquityLegData() : LegAdditionalData("Equity") {}
    //! Constructor
    EquityLegData(string returnType, Real dividendFactor, string eqName, Natural fixingDays)
        : LegAdditionalData("Equity"), returnType_(returnType), dividendFactor_(dividendFactor), eqName_(eqName),
          fixingDays_(fixingDays) {}

    //! \name Inspectors
    //@{
    const string& returnType() const { return returnType_; }
    const string& eqName() const { return eqName_; }
    Real dividendFactor() const { return dividendFactor_; }
    Natural fixingDays() const { return fixingDays_; }
    //@}

    //! \name Serialisation
    //@{
    virtual void fromXML(XMLNode* node);
    virtual XMLNode* toXML(XMLDocument& doc);
    //@}
private:
    string returnType_;
    Real dividendFactor_ = 1.0;
    string eqName_;
    Natural fixingDays_ = 0;
};

//! Serializable object holding amortization rules
class AmortizationData : public XMLSerializable {
public:
    AmortizationData() : initialized_(false) {}

    AmortizationData(string type, double value, string startDate, string endDate, string frequency, bool underflow)
        : type_(type), value_(value), startDate_(startDate), endDate_(endDate), frequency_(frequency),
          underflow_(underflow), initialized_(true) {}

    virtual void fromXML(XMLNode* node);
    virtual XMLNode* toXML(XMLDocument& doc);

    //! FixedAmount, RelativeToInitialNotional, RelativeToPreviousNotional, Annuity
    const string& type() const { return type_; }
    //! Interpretation depending on type()
    double value() const { return value_; }
    //! Amortization start date
    const string& startDate() const { return startDate_; }
    //! Amortization end date
    const string& endDate() const { return endDate_; }
    //! Amortization frequency
    const string& frequency() const { return frequency_; }
    //! Allow amortization below zero notional if true
    bool underflow() const { return underflow_; }
    bool initialized() const { return initialized_; }

private:
    string type_;
    double value_;
    string startDate_;
    string endDate_;
    string frequency_;
    bool underflow_;
    bool initialized_;
};

//! Serializable object holding leg data
class LegData : public XMLSerializable {
public:
    //! Default constructor
    LegData()
        : isPayer_(true), notionalInitialExchange_(false), notionalFinalExchange_(false),
          notionalAmortizingExchange_(false), isNotResetXCCY_(true), foreignAmount_(0.0), fixingDays_(0) {}

    //! Constructor with concrete leg data
    LegData(const boost::shared_ptr<LegAdditionalData>& innerLegData, bool isPayer, const string& currency,
            const ScheduleData& scheduleData = ScheduleData(), const string& dayCounter = "",
            const std::vector<double>& notionals = std::vector<double>(),
            const std::vector<string>& notionalDates = std::vector<string>(), const string& paymentConvention = "F",
            const bool notionalInitialExchange = false, const bool notionalFinalExchange = false,
            const bool notionalAmortizingExchange = false, const bool isNotResetXCCY = true,
            const string& foreignCurrency = "", const double foreignAmount = 0, const string& fxIndex = "",
            int fixingDays = 0, const string& fixingCalendar = "",
            const std::vector<AmortizationData>& amortizationData = std::vector<AmortizationData>(),
            const int paymentLag = 0);

    //! \name Serialisation
    //@{
    virtual void fromXML(XMLNode* node);
    virtual XMLNode* toXML(XMLDocument& doc);
    //@}

    //! \name Inspectors
    //@{
    bool isPayer() const { return isPayer_; }
    const string& currency() const { return currency_; }
    const ScheduleData& schedule() const { return schedule_; }
    const vector<double>& notionals() const { return notionals_; }
    const vector<string>& notionalDates() const { return notionalDates_; }
    const string& dayCounter() const { return dayCounter_; }
    const string& paymentConvention() const { return paymentConvention_; }
    bool notionalInitialExchange() const { return notionalInitialExchange_; }
    bool notionalFinalExchange() const { return notionalFinalExchange_; }
    bool notionalAmortizingExchange() const { return notionalAmortizingExchange_; }
    bool isNotResetXCCY() const { return isNotResetXCCY_; }
    const string& foreignCurrency() const { return foreignCurrency_; }
    double foreignAmount() const { return foreignAmount_; }
    const string& fxIndex() const { return fxIndex_; }
    int fixingDays() const { return fixingDays_; }
    const string& fixingCalendar() const { return fixingCalendar_; }
    const int paymentLag() const { return paymentLag_; }
    const std::vector<AmortizationData>& amortizationData() const { return amortizationData_; }
    //
    const string& legType() const { return concreteLegData_->legType(); }
    boost::shared_ptr<LegAdditionalData> concreteLegData() const { return concreteLegData_; }
    //@}

protected:
    virtual boost::shared_ptr<LegAdditionalData> initialiseConcreteLegData(const string&);

private:
    boost::shared_ptr<LegAdditionalData> concreteLegData_;
    bool isPayer_;
    string currency_;
    string legType_;
    ScheduleData schedule_;
    string dayCounter_;
    vector<double> notionals_;
    vector<string> notionalDates_;
    string paymentConvention_;
    bool notionalInitialExchange_;
    bool notionalFinalExchange_;
    bool notionalAmortizingExchange_;
    bool isNotResetXCCY_;
    string foreignCurrency_;
    double foreignAmount_;
    string fxIndex_;
    int fixingDays_;
    string fixingCalendar_;
    std::vector<AmortizationData> amortizationData_;
    int paymentLag_;
};

//! \name Utilities for building QuantLib Legs
//@{
Leg makeFixedLeg(const LegData& data);
Leg makeZCFixedLeg(const LegData& data);
Leg makeIborLeg(const LegData& data, const boost::shared_ptr<IborIndex>& index,
                const boost::shared_ptr<EngineFactory>& engineFactory, const bool attachPricer = true);
Leg makeOISLeg(const LegData& data, const boost::shared_ptr<OvernightIndex>& index);
Leg makeBMALeg(const LegData& data, const boost::shared_ptr<QuantExt::BMAIndexWrapper>& indexWrapper);
Leg makeSimpleLeg(const LegData& data);
Leg makeNotionalLeg(const Leg& refLeg, const bool initNomFlow, const bool finalNomFlow, const bool amortNomFlow = true);
Leg makeCPILeg(const LegData& data, const boost::shared_ptr<ZeroInflationIndex>& index);
Leg makeYoYLeg(const LegData& data, const boost::shared_ptr<YoYInflationIndex>& index);
Leg makeCMSLeg(const LegData& data, const boost::shared_ptr<QuantLib::SwapIndex>& swapindex,
               const boost::shared_ptr<EngineFactory>& engineFactory, const vector<double>& caps = vector<double>(),
               const vector<double>& floors = vector<double>(), const bool attachPricer = true);
Leg makeCMSSpreadLeg(const LegData& data, const boost::shared_ptr<QuantLib::SwapSpreadIndex>& swapSpreadIndex,
                     const boost::shared_ptr<EngineFactory>& engineFactory, const bool attachPricer = true);
Leg makeDigitalCMSSpreadLeg(const LegData& data, const boost::shared_ptr<QuantLib::SwapSpreadIndex>& swapSpreadIndex,
                            const boost::shared_ptr<EngineFactory>& engineFactory);
Leg makeEquityLeg(const LegData& data, const boost::shared_ptr<QuantExt::EquityIndex>& equityCurve);
Real currentNotional(const Leg& leg);

//@}

//! Build a full vector of values from the given node.
//  For use with Notionals, Rates, Spreads, Gearing, Caps and Floor rates.
//  In all cases we can expand the vector to take the given schedule into account
vector<double> buildScheduledVector(const vector<double>& values, const vector<string>& dates,
                                    const Schedule& schedule);

// extend values to schedule size (if values is empty, the default value is used)
vector<double> normaliseToSchedule(const vector<double>& values, const Schedule& schedule,
                                   const Real defaultValue = Null<Real>());

// normaliseToSchedule concat buildScheduledVector
vector<double> buildScheduledVectorNormalised(const vector<double>& values, const vector<string>& dates,
                                              const Schedule& schedule, const Real defaultValue = Null<Real>());

// notional vector derived from a fixed amortisation amount
vector<double> buildAmortizationScheduleFixedAmount(const vector<double>& notionals, const Schedule& schedule,
                                                    const AmortizationData& data);

// notional vector with amortizations expressed as a percentage of initial notional
vector<double> buildAmortizationScheduleRelativeToInitialNotional(const vector<double>& notionals,
                                                                  const Schedule& schedule,
                                                                  const AmortizationData& data);

// notional vector with amortizations expressed as a percentage of the respective previous notional
vector<double> buildAmortizationScheduleRelativeToPreviousNotional(const vector<double>& notionals,
                                                                   const Schedule& schedule,
                                                                   const AmortizationData& data);

// notional vector derived from a fixed annuity amount
vector<double> buildAmortizationScheduleFixedAnnuity(const vector<double>& notionals, const vector<double>& rates,
                                                     const Schedule& schedule, const AmortizationData& data,
                                                     const DayCounter& dc);
} // namespace data
} // namespace ore


#include <boost/program_options.hpp>
#include <string>
#include <d3d_base/grid.h>
#include <iostream>
#include <d3d_base/configFile.h>
#include <d3d_fusion/volumetricFusionTools.h>
#include <algorithm>

struct Hist {
    float counts[8];
};

D3D::Grid<float> runTVHist(D3D::Grid<Hist> dataCost, Eigen::Vector3f& minCorner, Eigen::Vector3f& size, Eigen::Matrix4f boxToGlobal, Eigen::Vector3f color, std::string vrmlOutputFile,
                                const int numIter,
                                const float lambda, const float theta)
{
    D3D::Grid<float> u(dataCost.getWidth(), dataCost.getHeight(), dataCost.getDepth());
    for (unsigned z = 0; z < u.getDepth(); z++)
        for (unsigned y = 0; y < u.getHeight(); y++)
            for (unsigned x = 0; x < u.getWidth(); x++)
            {
                u(x,y,z) = 0.f;
            }

    D3D::Grid<float> uBar = u.clone();

    D3D::Grid<float> p1(dataCost.getWidth(), dataCost.getHeight(), dataCost.getDepth(), 0);
    D3D::Grid<float> p2(dataCost.getWidth(), dataCost.getHeight(), dataCost.getDepth(), 0);
    D3D::Grid<float> p3(dataCost.getWidth(), dataCost.getHeight(), dataCost.getDepth(), 0);

    const int xDim = u.getWidth();
    const int yDim = u.getHeight();
    const int zDim = u.getDepth();

    // uses diagonal preconditioned first order pirmal dual

    const float sigma = 0.99f/2.f;
    const float tau = 0.99f/6.f;
    const float centers[8] = {-1.f, -5.f/7.f, -3.f/7.f, -1.f/7.f, 1.f/7.f, 3.f/7.f, 5.f/7.f, 1.f};

    for (int c = 0; c < numIter; c++)
    {
        std::cout << ".";
        std::cout.flush();

        // update dual variables
#pragma omp parallel for
        for (int z = 0; z < zDim; z++)
        {
            int const Z1 = (z < zDim-1) ? (z+1) : z;
            for (int y = 0; y < yDim; y++)
            {
                int const Y1 = (y < yDim-1) ? (y+1) : y;
                for (int x = 0; x < xDim; x++)
                {
                    float const u0 = uBar(x,y,z);

                    int const X1 = (x < xDim -1) ? (x+1) : x;

                    float const u_x = (uBar(X1, y, z) - u0);
                    float const u_y = (uBar(x, Y1, z) - u0);
                    float const u_z = (uBar(x, y, Z1) - u0);

                    float const d_x = p1(x,y,z) + sigma*u_x;
                    float const d_y = p2(x,y,z) + sigma*u_y;
                    float const d_z = p3(x,y,z) + sigma*u_z;

                    float const tv = sqrtf(d_x*d_x + d_y*d_y + d_z*d_z);

                    float const denom = std::max(1.f, tv);

                    p1(x,y,z) = d_x/denom;
                    p2(x,y,z) = d_y/denom;
                    p3(x,y,z) = d_z/denom;
                }
            }
        }

        // update primal
#pragma omp parallel for
        for (int z = 0; z < zDim; z++)
        {
            int const Z0 = z-1;
            for (int y = 0; y < yDim; y++)
            {
                int const Y0 = y-1;
                for (int x = 0; x < xDim; x++)
                {
                    int const X0 = x-1;

                    float const div = (X0 < xDim -1 ? p1(x,y,z) : 0.0f) - (X0 >= 0 ? p1(X0, y, z) : 0.0f) +
                                      (Y0 < yDim -1 ? p2(x,y,z) : 0.0f) - (Y0 >= 0 ? p2(x, Y0, z) : 0.0f) +
                                      (Z0 < zDim -1 ? p3(x,y,z) : 0.0f) - (Z0 >= 0 ? p3(x, y, Z0) : 0.0f);

                    float const u0 = u(x,y,z);
                    float U = u(x,y,z) + tau*div;

//                    // check most likely bin, is in [1, 8] as u0 in [-1, 1]
//                    int mlBin = int(7.f * (u0 + 1.f) / 2.f + 1.f);

//                    float step = 0.f;
//                    for (int b = 0; b <= mlBin; ++b) {
//                        step -= float(dataCost(x, y, z).counts[b]);
//                    }
//                    for (int b = mlBin+1; b < 10; ++b) {
//                        step += float(dataCost(x, y, z).counts[b]);
//                    }

//                    float proxMin = u0 + tau*lambda*step;

                    float proxMin, step;

//                    if (proxMin <= centers[mlBin] || centers[mlBin+1] <= proxMin) {
                        // outside of range
                        bool found = false;
                        for (int bin = 0; bin < 7; bin++) {
//                            if (bin == mlBin)
//                                continue;
                            step = 0.f;
                            for (int b = 0; b <= bin; ++b) {
                                step -= dataCost(x, y, z).counts[b];
                            }
                            for (int b = bin+1; b < 8; ++b) {
                                step += dataCost(x, y, z).counts[b];
                            }

                            proxMin = U + tau*lambda*step;

                            if (centers[bin] < proxMin && proxMin < centers[bin+1]) {
                                // inside of range, val is the minimum
                                found = true;
                                break;
                            }
                        }
                        if (!found) {
                            // minimum must be one of the bin centers
                            float proxMinEnergy = std::numeric_limits<float>::max();
                            for (int bin = 0; bin < 8; bin++) {
                                float dataterm = 0.f;
                                for (int i = 0; i < 8; ++i) {
                                    dataterm += dataCost(x, y, z).counts[i] * fabs(centers[bin]-centers[i]);
                                }
                                float proxEnergy = (U-centers[bin])*(U-centers[bin])/(2.f*tau) + lambda*dataterm;
                                if (proxEnergy < proxMinEnergy) {
                                    proxMinEnergy = proxEnergy;
                                    proxMin = centers[bin];
                                }
                            }
                        }
//                    }

                    U = proxMin;

                    u(x,y,z) = U;
                    uBar(x,y,z) = U + theta*(U - u0);

                }
            }
        }

        if ((c+1) % 50 == 0) {
            D3D::saveVolumeAsVRMLMesh(u, 0.0f, minCorner, size, boxToGlobal, color, vrmlOutputFile, true);
            std::cout << std::endl;
        }
    }
    return u;
}

int main(int argc, char* argv[])
{
    std::string configFile;
    std::string intDepthGridFile;
    std::string vrmlOutputFile;

    boost::program_options::options_description desc("Allowed options");
    desc.add_options()
            ("help", "Produce help message")
            ("intDepthGridFile", boost::program_options::value<std::string>(&intDepthGridFile)->default_value("intCSDFGrid.dat"), "Depth map list file")
            ("configFile", boost::program_options::value<std::string>(&configFile)->default_value("conf.txt"), "Config file")
            ("vrmlOutputFile", boost::program_options::value<std::string>(&vrmlOutputFile)->default_value("tvHistModel.wrl"), "Output data file with integrated depth data.")
            ;

    boost::program_options::variables_map vm;
    boost::program_options::store(boost::program_options::command_line_parser(argc, argv).options(desc).run(), vm);
    boost::program_options::notify(vm);

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

    // config file
    D3D::ConfigFile conf(configFile);

    if (!conf.isFileRead())
    {
        D3D_THROW_EXCEPTION("Could not open config file.")
    }

    // load the box parameters
    std::string bpMinXStr = conf.get("BP_MIN_X");
    if (bpMinXStr.empty())
    {
        D3D_THROW_EXCEPTION("Parameter BP_MIN_X not specified in the config file.")
    }
    float bpMinX = atof(bpMinXStr.c_str());

    std::string bpSizeXStr = conf.get("BP_SIZE_X");
    if (bpSizeXStr.empty())
    {
        D3D_THROW_EXCEPTION("Parameter BP_SIZE_X not specified in the config file.")
    }
    float bpSizeX = atof(bpSizeXStr.c_str());

    std::string bpMinYStr = conf.get("BP_MIN_Y");
    if (bpMinYStr.empty())
    {
        D3D_THROW_EXCEPTION("Parameter BP_MIN_Y not specified in the config file.")
    }
    float bpMinY = atof(bpMinYStr.c_str());

    std::string bpSizeYStr = conf.get("BP_SIZE_Y");
    if (bpSizeYStr.empty())
    {
        D3D_THROW_EXCEPTION("Parameter BP_SIZE_Y not specified in the config file.")
    }
    float bpSizeY = atof(bpSizeYStr.c_str());

    std::string bpMinZStr = conf.get("BP_MIN_Z");
    if (bpMinZStr.empty())
    {
        D3D_THROW_EXCEPTION("Parameter BP_MIN_Z not specified in the config file.")
    }
    float bpMinZ = atof(bpMinZStr.c_str());

    std::string bpSizeZStr = conf.get("BP_SIZE_Z");
    if (bpSizeZStr.empty())
    {
        D3D_THROW_EXCEPTION("Parameter BP_SIZE_Z not specified in the config file.")
    }
    float bpSizeZ = atof(bpSizeZStr.c_str());

    std::string bpResXStr = conf.get("BP_RES_X");
    if (bpResXStr.empty())
    {
        D3D_THROW_EXCEPTION("Parameter BP_RES_X not specified in the config file.")
    }
    int bpResX = atoi(bpResXStr.c_str());

    std::string bpResYStr = conf.get("BP_RES_Y");
    if (bpResYStr.empty())
    {
        D3D_THROW_EXCEPTION("Parameter BP_RES_Y not specified in the config file.")
    }
    int bpResY = atoi(bpResYStr.c_str());

    std::string bpResZStr = conf.get("BP_RES_Z");
    if (bpResZStr.empty())
    {
        D3D_THROW_EXCEPTION("Parameter BP_RES_Z not specified in the config file.")
    }
    int bpResZ = atoi(bpResZStr.c_str());

    std::string bpOriginStr = conf.get("BP_ORIGIN");
    Eigen::Vector3f bpOrigin;
    if (bpOriginStr.empty())
    {
        bpOrigin = Eigen::Vector3f::Zero();
    }
    else
    {
        std::stringstream bpOriginStream(bpOriginStr);
        bpOriginStream >> bpOrigin(0);
        bpOriginStream >> bpOrigin(1);
        bpOriginStream >> bpOrigin(2);

        if (bpOriginStream.fail())
        {
            D3D_THROW_EXCEPTION("Error reading BP_ORIGIN");
        }
    }

    std::string bpXAxisStr = conf.get("BP_X_AXIS");
    Eigen::Vector3f bpXAxis;
    bool xAxisSet = false;
    if (!bpXAxisStr.empty())
    {
        xAxisSet = true;

        std::stringstream bpXAxisStream(bpXAxisStr);
        bpXAxisStream >> bpXAxis(0);
        bpXAxisStream >> bpXAxis(1);
        bpXAxisStream >> bpXAxis(2);

        if (bpXAxisStream.fail())
        {
            D3D_THROW_EXCEPTION("Error reading BP_X_AXIS");
        }
    }

    std::string bpYAxisStr = conf.get("BP_Y_AXIS");
    Eigen::Vector3f bpYAxis;
    bool yAxisSet = false;
    if (!bpYAxisStr.empty())
    {
        yAxisSet = true;

        std::stringstream bpYAxisStream(bpYAxisStr);
        bpYAxisStream >> bpYAxis(0);
        bpYAxisStream >> bpYAxis(1);
        bpYAxisStream >> bpYAxis(2);

        if (bpYAxisStream.fail())
        {
            D3D_THROW_EXCEPTION("Error reading BP_Y_AXIS");
        }
    }

    std::string bpZAxisStr = conf.get("BP_Z_AXIS");
    Eigen::Vector3f bpZAxis;
    bool zAxisSet = false;
    if (!bpZAxisStr.empty())
    {
        zAxisSet = true;

        std::stringstream bpZAxisStream(bpZAxisStr);
        bpZAxisStream >> bpZAxis(0);
        bpZAxisStream >> bpZAxis(1);
        bpZAxisStream >> bpZAxis(2);

        if (bpZAxisStream.fail())
        {
            D3D_THROW_EXCEPTION("Error reading BP_Z_AXIS");
        }
    }

    if (xAxisSet == false && yAxisSet == false && zAxisSet == false)
    {
        bpXAxis(0) = 1; bpXAxis(1) = 0; bpXAxis(2) = 0;
        bpYAxis(0) = 0; bpYAxis(1) = 1; bpYAxis(2) = 0;
        bpZAxis(0) = 0; bpZAxis(1) = 0; bpZAxis(2) = 1;
    }
    else if (!(xAxisSet && yAxisSet && zAxisSet))
    {
        D3D_THROW_EXCEPTION("Either all or none out of BP_X_AXIS, BP_Y_AXIS and BP_Z_AXIS have to be specified.")
    }

    // rectify the axes
    // normlaize
    bpXAxis /= bpXAxis.norm();
    bpYAxis /= bpYAxis.norm();
    bpZAxis /= bpZAxis.norm();

    Eigen::Vector3f rightAngleZAxis = bpXAxis.cross(bpYAxis);
    if (rightAngleZAxis.dot(bpZAxis) < 0.95)
    {
        D3D_THROW_EXCEPTION("The given axes are not orthogonal to each other.")
    }

    bpZAxis = rightAngleZAxis;

    Eigen::Vector3f rightAngleYAxis = bpZAxis.cross(bpXAxis);
    if (rightAngleYAxis.dot(bpYAxis) < 0.95)
    {
        D3D_THROW_EXCEPTION("The given axes are not orthogonal to each other.");
    }

    bpYAxis = rightAngleYAxis;

    Eigen::Matrix4f boxToGlobal = Eigen::Matrix4f::Identity();
    boxToGlobal.topLeftCorner(3,3).col(0) = bpXAxis;
    boxToGlobal.topLeftCorner(3,3).col(1) = bpYAxis;
    boxToGlobal.topLeftCorner(3,3).col(2) = bpZAxis;
    boxToGlobal.topRightCorner(3,1).col(0) = bpOrigin;

    std::string hfNumIterStr = conf.get("HF_NUM_ITER");
    if (hfNumIterStr.empty())
    {
        D3D_THROW_EXCEPTION("Paramter HF_NUM_ITER is not specified in the config file.")
    }
    int ffNumIter = atoi(hfNumIterStr.c_str());

    std::string hfLambdaStr = conf.get("HF_LAMBDA");
    if (hfLambdaStr.empty())
    {
        D3D_THROW_EXCEPTION("Parameter HF_LAMBDA is not specified in the config file.")
    }
    float hfLambda = atof(hfLambdaStr.c_str());

    // load grid
    D3D::Grid<Hist> dataCost;
    dataCost.loadFromDataFile(intDepthGridFile);

    std::cout << "Data Cost Loaded: width = " << dataCost.getWidth() << ", height = " << dataCost.getHeight() << ", depth = " << dataCost.getDepth() << std::endl;

    if (bpResX != (int) dataCost.getWidth() || bpResY != (int) dataCost.getHeight() || bpResZ != (int) dataCost.getDepth())
    {
        D3D_THROW_EXCEPTION("Resolution specified in the config file does not match the dimension of the grid with integrated depth maps.")
    }

    Eigen::Vector3f minCorner;
    minCorner(0) = bpMinX;
    minCorner(1) = bpMinY;
    minCorner(2) = bpMinZ;
    Eigen::Vector3f size;
    size(0) = bpSizeX;
    size(1) = bpSizeY;
    size(2) = bpSizeZ;
    Eigen::Vector3f color;
    color(0) = 0.5;
    color(1) = 0.5;
    color(2) = 0.5;

    runTVHist(dataCost, minCorner, size, boxToGlobal, color, vrmlOutputFile, ffNumIter, hfLambda, 1);
}


/*===================================================================

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center.

All rights reserved.

This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.

See LICENSE.txt or http://www.mitk.org for details.

===================================================================*/

#ifndef __itkEvaluateDirectionImagesFilter_cpp
#define __itkEvaluateDirectionImagesFilter_cpp

#include "itkEvaluateDirectionImagesFilter.h"
#include <itkImageRegionConstIterator.h>
#include <itkImageRegionIterator.h>
#include <itkImageDuplicator.h>
#include <boost/progress.hpp>

namespace itk {

template< class PixelType >
ComparePeakImagesFilter< PixelType >
::ComparePeakImagesFilter():
  m_IgnoreMissingRefDirections(false),
  m_IgnoreMissingTestDirections(false),
  m_Eps(0.0001)
{
  this->SetNumberOfIndexedOutputs(2);
}

template< class PixelType >
void ComparePeakImagesFilter< PixelType >::GenerateData()
{
  if (m_TestImage.IsNull() || m_ReferenceImage.IsNull())
    mitkThrow() << "Test or reference image not set!";
//  if (m_TestImage->GetLargestPossibleRegion().GetSize()!=m_ReferenceImage->GetLargestPossibleRegion().GetSize())
//    mitkThrow() << "Test and reference image need to have same sizes!";

  Vector<float, 4> spacing4 = m_TestImage->GetSpacing();
  Point<float, 4> origin4 = m_TestImage->GetOrigin();
  Matrix<double, 4, 4> direction4 = m_TestImage->GetDirection();
  ImageRegion<4> imageRegion4 = m_TestImage->GetLargestPossibleRegion();

  Vector<double, 3> spacing3;
  Point<float, 3> origin3;
  Matrix<double, 3, 3> direction3;
  ImageRegion<3> imageRegion3;

  spacing3[0] = spacing4[0]; spacing3[1] = spacing4[1]; spacing3[2] = spacing4[2];
  origin3[0] = origin4[0]; origin3[1] = origin4[1]; origin3[2] = origin4[2];
  for (int r=0; r<3; r++)
    for (int c=0; c<3; c++)
      direction3[r][c] = direction4[r][c];
  imageRegion3.SetSize(0, imageRegion4.GetSize()[0]);
  imageRegion3.SetSize(1, imageRegion4.GetSize()[1]);
  imageRegion3.SetSize(2, imageRegion4.GetSize()[2]);

  // angular error image
  typename OutputImageType::Pointer outputImage = OutputImageType::New();
  outputImage->SetOrigin( origin3 );
  outputImage->SetRegions( imageRegion3 );
  outputImage->SetSpacing( spacing3 );
  outputImage->SetDirection( direction3 );
  outputImage->Allocate();
  outputImage->FillBuffer(0.0);
  this->SetNthOutput(0, outputImage);

  // length error image
  outputImage = OutputImageType::New();
  outputImage->SetOrigin( origin3 );
  outputImage->SetRegions( imageRegion3 );
  outputImage->SetSpacing( spacing3 );
  outputImage->SetDirection( direction3 );
  outputImage->Allocate();
  outputImage->FillBuffer(0.0);
  this->SetNthOutput(1, outputImage);

  if (m_MaskImage.IsNull())
  {
    m_MaskImage = UCharImageType::New();
    m_MaskImage->SetOrigin( origin3 );
    m_MaskImage->SetRegions( imageRegion3 );
    m_MaskImage->SetSpacing( spacing3 );
    m_MaskImage->SetDirection( direction3 );
    m_MaskImage->Allocate();
    m_MaskImage->FillBuffer(1);
  }

  m_MeanAngularError = 0.0;
  m_MedianAngularError = 0;
  m_MaxAngularError = 0.0;
  m_MinAngularError = itk::NumericTraits<float>::max();
  m_VarAngularError = 0.0;
  m_AngularErrorVector.clear();

  m_MeanLengthError = 0.0;
  m_MedianLengthError = 0;
  m_MaxLengthError = 0.0;
  m_MinLengthError = itk::NumericTraits<float>::max();
  m_VarLengthError = 0.0;
  m_LengthErrorVector.clear();

  outputImage = static_cast< OutputImageType* >(this->ProcessObject::GetOutput(0));
  typename OutputImageType::Pointer outputImage2 = static_cast< OutputImageType* >(this->ProcessObject::GetOutput(1));

  ImageRegionIterator< OutputImageType > oit(outputImage, outputImage->GetLargestPossibleRegion());
  ImageRegionIterator< OutputImageType > oit2(outputImage2, outputImage2->GetLargestPossibleRegion());
  ImageRegionIterator< UCharImageType > mit(m_MaskImage, m_MaskImage->GetLargestPossibleRegion());

  boost::progress_display disp(outputImage->GetLargestPossibleRegion().GetSize()[0]*outputImage->GetLargestPossibleRegion().GetSize()[1]*outputImage->GetLargestPossibleRegion().GetSize()[2]);
  while( !oit.IsAtEnd() )
  {
    ++disp;
    if( mit.Get()!=1 )
    {
      ++oit;
      ++oit2;
      ++mit;
      continue;
    }
    typename OutputImageType::IndexType index = oit.GetIndex();
    itk::Index<4> idx4;
    idx4[0] = index[0];
    idx4[1] = index[1];
    idx4[2] = index[2];

    std::vector< PixelType > ref_magnitudes;
    std::vector< PeakType > ref_peaks;
    for (unsigned int i=0; i<m_ReferenceImage->GetLargestPossibleRegion().GetSize()[3]/3; i++)
    {
      PeakType ref_peak;
      idx4[3] = i*3;
      ref_peak[0] = m_ReferenceImage->GetPixel(idx4);
      idx4[3] = i*3 + 1;
      ref_peak[1] = m_ReferenceImage->GetPixel(idx4);
      idx4[3] = i*3 + 2;
      ref_peak[2] = m_ReferenceImage->GetPixel(idx4);

      PixelType ref_mag = ref_peak.magnitude();
      if (ref_mag > m_Eps )
      {
        ref_peak.normalize();
        ref_peaks.push_back(ref_peak);
        ref_magnitudes.push_back(ref_mag);
      }
    }

    std::vector< PixelType > test_magnitudes;
    std::vector< PeakType > test_peaks;
    for (unsigned int j=0; j<m_TestImage->GetLargestPossibleRegion().GetSize()[3]/3; j++)
    {
      PeakType test_peak;
      idx4[3] = j*3;
      test_peak[0] = m_TestImage->GetPixel(idx4);
      idx4[3] = j*3 + 1;
      test_peak[1] = m_TestImage->GetPixel(idx4);
      idx4[3] = j*3 + 2;
      test_peak[2] = m_TestImage->GetPixel(idx4);

      PixelType test_mag = test_peak.magnitude();
      if (test_mag > m_Eps )
      {
        test_peak.normalize();
        test_peaks.push_back(test_peak);
        test_magnitudes.push_back(test_mag);
      }
    }

    if (test_peaks.empty() && ref_peaks.empty())
    {
      ++oit;
      ++oit2;
      ++mit;
      continue;
    }
    else if (test_peaks.empty())
    {
      if (!m_IgnoreMissingTestDirections)
      {
        PixelType length_sum = 0;
        for (auto l : ref_magnitudes)
        {
          length_sum += l;
          m_AngularErrorVector.push_back(90);
          m_LengthErrorVector.push_back(l);
          m_MeanAngularError += m_AngularErrorVector.back();
          m_MeanLengthError += m_LengthErrorVector.back();
        }
        oit2.Set(length_sum/ref_magnitudes.size());
        oit.Set(90);
      }
    }
    else if (ref_peaks.empty())
    {
      if (!m_IgnoreMissingRefDirections)
      {
        PixelType length_sum = 0;
        for (auto l : test_magnitudes)
        {
          length_sum += l;
          m_AngularErrorVector.push_back(90);
          m_LengthErrorVector.push_back(length_sum);
          m_MeanAngularError += m_AngularErrorVector.back();
          m_MeanLengthError += m_LengthErrorVector.back();
        }
        oit2.Set(length_sum/test_magnitudes.size());
        oit.Set(90);
      }
    }
    else
    {
      PixelType error_a = 0;
      PixelType error_l = 0;
      for (unsigned int i=0; i<ref_peaks.size(); ++i)
      {
        PixelType max_a = 0;
        PixelType max_l = 0;
        for (unsigned int j=0; j<test_peaks.size(); ++j)
        {
          auto a = fabs(dot_product(test_peaks[j], ref_peaks[i]));
          if (a>1.0)
            a = 1.0;
          if (a>max_a)
          {
            max_a = a;
            max_l = fabs(ref_magnitudes[i]-test_magnitudes[j]);
          }
        }
        m_LengthErrorVector.push_back( max_l );
        m_AngularErrorVector.push_back( acos(max_a)*180.0/itk::Math::pi );
        m_MeanAngularError += m_AngularErrorVector.back();
        m_MeanLengthError += m_LengthErrorVector.back();
        error_a += m_AngularErrorVector.back();
        error_l += m_LengthErrorVector.back();
      }

      for (unsigned int i=0; i<test_peaks.size(); ++i)
      {
        PixelType max_a = 0;
        PixelType max_l = 0;
        for (unsigned int j=0; j<ref_peaks.size(); ++j)
        {
          auto a = fabs(dot_product(test_peaks[i], ref_peaks[j]));
          if (a>1.0)
            a = 1.0;
          if (a>max_a)
          {
            max_a = a;
            max_l = fabs(ref_magnitudes[j]-test_magnitudes[i]);
          }
        }
        m_LengthErrorVector.push_back( max_l );
        m_AngularErrorVector.push_back( acos(max_a)*180.0/itk::Math::pi );
        m_MeanAngularError += m_AngularErrorVector.back();
        m_MeanLengthError += m_LengthErrorVector.back();
        error_a += m_AngularErrorVector.back();
        error_l += m_LengthErrorVector.back();
      }

      error_a /= (test_peaks.size() + ref_peaks.size());
      error_l /= (test_peaks.size() + ref_peaks.size());

      oit2.Set(error_l);
      oit.Set(error_a);
    }

    ++oit;
    ++oit2;
    ++mit;
  }

  std::sort( m_AngularErrorVector.begin(), m_AngularErrorVector.end() );
  if (!m_AngularErrorVector.empty())
    m_MeanAngularError /= m_AngularErrorVector.size();      // mean

  for (unsigned int i=0; i<m_AngularErrorVector.size(); i++)
  {
    float temp = m_AngularErrorVector.at(i) - m_MeanAngularError;
    m_VarAngularError += temp*temp;

    if ( m_AngularErrorVector.at(i)>m_MaxAngularError )
      m_MaxAngularError = m_AngularErrorVector.at(i);
    if ( m_AngularErrorVector.at(i)<m_MinAngularError )
      m_MinAngularError = m_AngularErrorVector.at(i);
  }
  if (m_AngularErrorVector.size()>1)
  {
    m_VarAngularError /= (m_AngularErrorVector.size()-1);   // variance

    // median
    if (m_AngularErrorVector.size()%2 == 0)
      m_MedianAngularError = 0.5*( m_AngularErrorVector.at( m_AngularErrorVector.size()/2 ) +  m_AngularErrorVector.at( m_AngularErrorVector.size()/2+1 ) );
    else
      m_MedianAngularError = m_AngularErrorVector.at( (m_AngularErrorVector.size()+1)/2 ) ;
  }

  std::sort( m_LengthErrorVector.begin(), m_LengthErrorVector.end() );
  m_MeanLengthError /= m_LengthErrorVector.size();      // mean
  for (unsigned int i=0; i<m_LengthErrorVector.size(); i++)
  {
    float temp = m_LengthErrorVector.at(i) - m_MeanLengthError;
    m_VarLengthError += temp*temp;

    if ( m_LengthErrorVector.at(i)>m_MaxLengthError )
      m_MaxLengthError = m_LengthErrorVector.at(i);
    if ( m_LengthErrorVector.at(i)<m_MinLengthError )
      m_MinLengthError = m_LengthErrorVector.at(i);
  }
  if (m_LengthErrorVector.size()>1)
  {
    m_VarLengthError /= (m_LengthErrorVector.size()-1);   // variance

    // median
    if (m_LengthErrorVector.size()%2 == 0)
      m_MedianLengthError = 0.5*( m_LengthErrorVector.at( m_LengthErrorVector.size()/2 ) +  m_LengthErrorVector.at( m_LengthErrorVector.size()/2+1 ) );
    else
      m_MedianLengthError = m_LengthErrorVector.at( (m_LengthErrorVector.size()+1)/2 ) ;
  }
}

}

#endif // __itkEvaluateDirectionImagesFilter_cpp


// ====================================================================
// This file is part of FlexibleSUSY.
//
// FlexibleSUSY is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published
// by the Free Software Foundation, either version 3 of the License,
// or (at your option) any later version.
//
// FlexibleSUSY is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with FlexibleSUSY.  If not, see
// <http://www.gnu.org/licenses/>.
// ====================================================================

// File generated at Thu 10 May 2018 14:37:40

#ifndef MSSMatMSUSYEFTHiggs_mAmu_UTILITIES_H
#define MSSMatMSUSYEFTHiggs_mAmu_UTILITIES_H

#include "MSSMatMSUSYEFTHiggs_mAmu_mass_eigenstates.hpp"
#include "MSSMatMSUSYEFTHiggs_mAmu_info.hpp"
#include "wrappers.hpp"

#include <Eigen/Core>
#include <array>
#include <string>
#include <vector>
#include <valarray>
#include <utility>

namespace softsusy {
class QedQcd;
}

namespace flexiblesusy {

struct MSSMatMSUSYEFTHiggs_mAmu_observables;
class Physical_input;

class MSSMatMSUSYEFTHiggs_mAmu_parameter_getter {
private:
   std::vector<std::string> get_mass_names(const std::string& head = "") const {
      using namespace MSSMatMSUSYEFTHiggs_mAmu_info;
      std::vector<std::string> masses;
      for (int i = 0; i < NUMBER_OF_PARTICLES; i++) {
         for (int m = 0; m < particle_multiplicities[i]; m++) {
            masses.push_back(
               head + "M" + particle_names[i] +
               (particle_multiplicities[i] == 1 ? "" : "("
                + std::to_string(static_cast<long long>(m)) + ")"));
         }
      }
      return masses;
   }

   std::array<std::string, MSSMatMSUSYEFTHiggs_mAmu_info::NUMBER_OF_MIXINGS> get_mixing_names() const {
      return MSSMatMSUSYEFTHiggs_mAmu_info::particle_mixing_names;
   }

public:
   /// returns DR-bar parameters
   Eigen::ArrayXd get_parameters(const MSSMatMSUSYEFTHiggs_mAmu_mass_eigenstates& model) {
      return model.get();
   }

   /// returns names of DR-bar parameters
   std::array<std::string, MSSMatMSUSYEFTHiggs_mAmu_info::NUMBER_OF_PARAMETERS> get_parameter_names() const {
      return MSSMatMSUSYEFTHiggs_mAmu_info::parameter_names;
   }

   /// returns names of particles
   std::array<std::string, MSSMatMSUSYEFTHiggs_mAmu_info::NUMBER_OF_PARTICLES> get_particle_names() const {
      return MSSMatMSUSYEFTHiggs_mAmu_info::particle_names;
   }

   /// returns names of DR-bar masses
   std::vector<std::string> get_DRbar_mass_names() const { return get_mass_names(); }

   /// returns names of pole masses
   std::vector<std::string> get_pole_mass_names() const { return get_mass_names("Pole"); }

   /// returns names of DR-bar mixing matrices
   std::array<std::string, MSSMatMSUSYEFTHiggs_mAmu_info::NUMBER_OF_MIXINGS> get_DRbar_mixing_names() const {
      return get_mixing_names();
   }

   /// returns names of pole mixing matrices
   std::array<std::string, MSSMatMSUSYEFTHiggs_mAmu_info::NUMBER_OF_MIXINGS> get_pole_mixing_names() const {
      auto mixing_names = get_mixing_names();
      for (auto& n: mixing_names)
         n = std::string("Pole") + n;
      return mixing_names;
   }

   /// returns names of input parameters
   std::array<std::string, MSSMatMSUSYEFTHiggs_mAmu_info::NUMBER_OF_INPUT_PARAMETERS> get_input_parameter_names() const {
      return MSSMatMSUSYEFTHiggs_mAmu_info::input_parameter_names;
   }

   /// returns names of input parameters
   std::array<std::string, MSSMatMSUSYEFTHiggs_mAmu_info::NUMBER_OF_EXTRA_PARAMETERS> get_extra_parameter_names() const {
      return MSSMatMSUSYEFTHiggs_mAmu_info::extra_parameter_names;
   }

   /// returns number of particle masses
   decltype(MSSMatMSUSYEFTHiggs_mAmu_info::NUMBER_OF_MASSES) get_number_of_masses() const {
      return MSSMatMSUSYEFTHiggs_mAmu_info::NUMBER_OF_MASSES;
   }
};

class MSSMatMSUSYEFTHiggs_mAmu_spectrum_plotter {
public:
   MSSMatMSUSYEFTHiggs_mAmu_spectrum_plotter() = default;
   explicit MSSMatMSUSYEFTHiggs_mAmu_spectrum_plotter(const MSSMatMSUSYEFTHiggs_mAmu_mass_eigenstates&);
   void extract_spectrum(const MSSMatMSUSYEFTHiggs_mAmu_mass_eigenstates&);
   void write_to_file(const std::string&) const;

private:
   struct TParticle {
      std::string name;
      std::string latex_name;
      std::valarray<double> masses;
      TParticle(const std::string& name_, const std::string& latex_name_,
                const std::valarray<double>& masses_)
         : name(name_)
         , latex_name(latex_name_)
         , masses(masses_)
         {}
   };
   using TSpectrum = std::vector<TParticle>;
   TSpectrum spectrum{};
   double scale{0.};
   int width{16};

   void write_spectrum(const TSpectrum&, std::ofstream&) const;
};

namespace MSSMatMSUSYEFTHiggs_mAmu_database {

/// append parameter point to database
void to_database(
   const std::string&,
   const MSSMatMSUSYEFTHiggs_mAmu_mass_eigenstates&,
   const softsusy::QedQcd* qedqcd = nullptr,
   const Physical_input* physical_input = nullptr,
   const MSSMatMSUSYEFTHiggs_mAmu_observables* observables = nullptr);

/// fill model from an entry of the database
MSSMatMSUSYEFTHiggs_mAmu_mass_eigenstates from_database(
   const std::string&,
   long long,
   softsusy::QedQcd* qedqcd = nullptr,
   Physical_input* physical_input = nullptr,
   MSSMatMSUSYEFTHiggs_mAmu_observables* observables = nullptr);

} // namespace MSSMatMSUSYEFTHiggs_mAmu_database

} // namespace flexiblesusy

#endif


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SIMD_SSE_AVX_TOFLOAT_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SIMD_SSE_AVX_TOFLOAT_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_AVX_SUPPORT
#include <boost/simd/toolbox/arithmetic/functions/tofloat.hpp>
#include <boost/simd/include/functions/simd/bitwise_andnot.hpp>
#include <boost/simd/include/functions/simd/if_else.hpp>
#include <boost/simd/include/functions/simd/plus.hpp>
#include <boost/simd/include/functions/simd/is_gez.hpp>
#include <boost/simd/include/functions/simd/make.hpp>
#include <boost/simd/include/constants/int_splat.hpp>
#include <boost/simd/sdk/meta/scalar_of.hpp>
#include <boost/dispatch/meta/as_floating.hpp>
#include <boost/dispatch/meta/as_integer.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::tofloat_, boost::simd::tag::avx_,
                          (A0),
                          ((simd_<int32_<A0>,boost::simd::tag::avx_>))
                         )
  {
    typedef typename dispatch::meta::as_floating<A0>::type  result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return _mm256_cvtepi32_ps(a0);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::tofloat_, boost::simd::tag::avx_,
                          (A0),
                          ((simd_<uint32_<A0>,boost::simd::tag::avx_>))
                         )
  {
    typedef typename dispatch::meta::as_floating<A0>::type  result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(1)
    {
      typedef typename dispatch::meta::scalar_of<result_type>::type stype;
      typedef native < stype, boost::simd::tag::sse_ > htype;

      htype h1 = make<htype>(a0[0],a0[1],a0[2],a0[3]);
      htype h2 = make<htype>(a0[4],a0[5],a0[6],a0[7]);
      result_type r = _mm256_castps128_ps256(h1);
      r =  _mm256_insertf128_ps(r, h2, 1);
      return r;
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::tofloat_, boost::simd::tag::avx_,
                          (A0),
                          ((simd_<int64_<A0>,boost::simd::tag::avx_>))
                         )
  {
    typedef typename dispatch::meta::as_floating<A0>::type  result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return make<result_type>(a0[0], a0[1], a0[2], a0[3]);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::tofloat_, boost::simd::tag::avx_,
                          (A0),
                          ((simd_<uint64_<A0>,boost::simd::tag::avx_>))
                         )
  {
    typedef typename dispatch::meta::as_floating<A0>::type  result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return make<result_type>(a0[0], a0[1], a0[2], a0[3]);
    }
  };
} } }

#endif
#endif


// Copyright (c) 2012-2019 The GalaxyCash developers
// Distributed under the MIT/X11 software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include <bignum.h>
#include <chainparams.h>
#include <consensus/validation.h>
#include <kernel.h>
#include <random.h>
#include <script/interpreter.h>
#include <streams.h>
#include <timedata.h>
#include <txdb.h>
#include <util.h>
#include <utilstrencodings.h>
#include <validation.h>

#include <boost/assign/list_of.hpp>
#include <boost/foreach.hpp>

using namespace std;

// Hard checkpoints of stake modifiers to ensure they are deterministic
static std::map<int, unsigned int> mapStakeModifierCheckpoints;
static std::map<int, unsigned int> mapStakeModifierTestnetCheckpoints;

// Get time weight
int64_t GetWeight(int64_t nIntervalBeginning, int64_t nIntervalEnd)
{
    // Kernel hash weight starts from 0 at the min age
    // this change increases active coins participating the hash and helps
    // to secure the network when proof-of-stake difficulty is low

    return nIntervalEnd - nIntervalBeginning - Params().GetConsensus().nStakeMinAge;
}

// Get selection interval section (in seconds)
static int64_t GetStakeModifierSelectionIntervalSection(int nSection)
{
    assert(nSection >= 0 && nSection < 64);
    return (Params().GetConsensus().nModifierInterval * 63 / (63 + ((63 - nSection) * (MODIFIER_INTERVAL_RATIO - 1))));
}

// Get stake modifier selection interval (in seconds)
static int64_t GetStakeModifierSelectionInterval()
{
    int64_t nSelectionInterval = 0;
    for (int nSection = 0; nSection < 64; nSection++)
        nSelectionInterval += GetStakeModifierSelectionIntervalSection(nSection);
    return nSelectionInterval;
}

uint256 ComputeStakeModifier(const CBlockIndex* pindexPrev, const uint256& kernel)
{
    if (!pindexPrev)
        return 0; // genesis block's modifier is 0

    CDataStream ss(SER_GETHASH, 0);
    ss << kernel << pindexPrev->bnStakeModifier;
    return HashX12(ss.begin(), ss.end());
}

bool CheckStakeKernelHash(const CBlockIndex* pindexPrev, unsigned int nBits, const CBlockHeader& blockFrom, const CTransaction& tx, const CTransaction& txPrev, const COutPoint& prevout, uint256& hashProofOfStake, bool fPrintProofOfStake)
{
    if (tx.nTime < txPrev.nTime) // Transaction timestamp violation
        return error("CheckStakeKernelHash() : nTime violation");

    // Base target
    arith_uint256 bnTarget;
    bnTarget.SetCompact(nBits);

    // Weighted target
    int64_t nValueIn = txPrev.vout[prevout.n].nValue;
    arith_uint256 bnWeight = arith_uint256(nValueIn);
    bnTarget *= bnWeight;

    // Calculate hash
    CDataStream ss(SER_GETHASH, 0);
    ss << pindexPrev->bnStakeModifier;
    ss << txPrev.nTime << prevout.hash << prevout.n << tx.nTime;
    hashProofOfStake = HashX12(ss.begin(), ss.end());

    // Now check if proof-of-stake hash meets target protocol
    if (arith_uint256(hashProofOfStake) > bnTarget)
        return false;

    return true;
}

// Check kernel hash target and coinstake signature
bool CheckProofOfStake(const CBlockIndex* pindexPrev, unsigned int nBits, const CTransaction& tx, uint256& hashProofOfStake)
{
    if (!tx.IsCoinStake())
        return true;

    // Kernel (input 0) must match the stake hash target per coin age (nBits)
    const CTxIn& txin = tx.vin[0];

    // Transaction index is required to get to block header
    if (!fTxIndex)
        return error("CheckProofOfStake() : transaction index not available");

    // Get transaction index for the previous transaction
    CDiskTxPos postx;
    if (!pblocktree->ReadTxIndex(txin.prevout.hash, postx))
        return error("CheckProofOfStake() : tx index not found"); // tx index not found

    // Read txPrev and header of its block
    CBlockHeader header;
    CTransactionRef txPrev;
    {
        CAutoFile file(OpenBlockFile(postx, true), SER_DISK, CLIENT_VERSION);
        try {
            file >> header;
            fseek(file.Get(), postx.nTxOffset, SEEK_CUR);
            file >> txPrev;
        } catch (std::exception& e) {
            return error("%s() : deserialize or I/O error in CheckProofOfStake()", __PRETTY_FUNCTION__);
        }
        if (txPrev->GetHash() != txin.prevout.hash)
            return error("%s() : txid mismatch in CheckProofOfStake()", __PRETTY_FUNCTION__);
    }

    int nDepth = 0;
    CBlockIndex* ptxPrevBlock = mapBlockIndex.find(header.GetHash()) != mapBlockIndex.end() ? mapBlockIndex[header.GetHash()] : NULL;
    if (ptxPrevBlock && pindexPrev)
        nDepth = (pindexPrev->nHeight + 1) - ptxPrevBlock->nHeight;

    if (nDepth < Params().GetConsensus().nStakeMinConfirmations - 1)
        return error("CheckProofOfStake() : tried to stake at depth %d", nDepth + 1);

    // Verify signature
    {
        int nIn = 0;
        const CTxOut& prevOut = txPrev->vout[tx.vin[nIn].prevout.n];
        TransactionSignatureChecker checker(&tx, nIn, prevOut.nValue, PrecomputedTransactionData(tx));

        if (!VerifyScript(tx.vin[nIn].scriptSig, prevOut.scriptPubKey, SCRIPT_VERIFY_P2SH, checker, nullptr))
            return error("%s: VerifyScript failed on coinstake %s", __func__, tx.GetHash().ToString());
    }

    if (!CheckStakeKernelHash(pindexPrev, nBits, header, tx, *txPrev, txin.prevout, hashProofOfStake, gArgs.GetBoolArg("-debug", false)))
        return error("CheckProofOfStake() : INFO: check kernel failed on coinstake %s, hashProof=%s", tx.GetHash().ToString(), hashProofOfStake.ToString()); // may occur during initial download or if behind on block chain sync

    return true;
}

// Check whether the coinstake timestamp meets protocol
bool CheckCoinStakeTimestamp(int64_t nTimeBlock, int64_t nTimeTx)
{
    if (true)
        return (nTimeBlock == nTimeTx) && ((nTimeTx & STAKE_TIMESTAMP_MASK) == 0);
    else
        return (nTimeBlock == nTimeTx);
}

// Get stake modifier checksum
unsigned int GetStakeModifierChecksum(const CBlockIndex* pindex)
{
    assert(pindex->pprev || pindex->GetBlockHash() == Params().GetConsensus().hashGenesisBlock);
    // Hash previous checksum with flags, hashProofOfStake and nStakeModifier
    CDataStream ss(SER_GETHASH, 0);
    if (pindex->pprev)
        ss << pindex->pprev->nStakeModifierChecksum;
    ss << pindex->nFlags << pindex->hashProofOfStake << pindex->bnStakeModifier;
    arith_uint256 hashChecksum = UintToArith256(HashX12(ss.begin(), ss.end()));
    hashChecksum >>= (256 - 32);
    return hashChecksum.GetLow64();
}

// Check stake modifier hard checkpoints
bool CheckStakeModifierCheckpoints(int nHeight, unsigned int nStakeModifierChecksum)
{
    if (nHeight = 0)
        return true;

    bool fTestNet = Params().NetworkIDString() == CBaseChainParams::TESTNET;
    if (fTestNet && mapStakeModifierTestnetCheckpoints.count(nHeight))
        return nStakeModifierChecksum == mapStakeModifierTestnetCheckpoints[nHeight];

    if (!fTestNet && mapStakeModifierCheckpoints.count(nHeight))
        return nStakeModifierChecksum == mapStakeModifierCheckpoints[nHeight];

    return true;
}

bool IsSuperMajority(int minVersion, const CBlockIndex* pstart, unsigned int nRequired, unsigned int nToCheck)
{
    unsigned int nFound = 0;
    for (unsigned int i = 0; i < nToCheck && nFound < nRequired && pstart != NULL; pstart = pstart->pprev) {
        if (!pstart->IsProofOfStake())
            continue;

        if (pstart->nVersion >= minVersion)
            ++nFound;

        i++;
    }
    return (nFound >= nRequired);
}

// galaxycash: entropy bit for stake modifier if chosen by modifier
unsigned int GetStakeEntropyBit(const CBlockHeader& block)
{
    unsigned int nEntropyBit = (block.GetHash().GetLow64() & 1llu); // last bit of block hash
    if (gArgs.GetBoolArg("-printstakemodifier", false))
        LogPrintf("GetStakeEntropyBit(v0.4+): nTime=%u hashBlock=%s entropybit=%d\n", block.nTime, block.GetHash().ToString(), nEntropyBit);
    return nEntropyBit;
}

bool CheckKernel(CBlockIndex* pindexPrev, unsigned int nBits, const CTransaction& tx, uint256* hashProof)
{
    uint256 hashProofOfStake;

    if (!tx.IsCoinStake())
        return error("CheckKernel() : called on non-coinstake %s", tx.GetHash().ToString());

    // Kernel (input 0) must match the stake hash target per coin age (nBits)
    const CTxIn& txin = tx.vin[0];

    // Transaction index is required to get to block header
    if (!fTxIndex)
        return error("CheckKernel() : transaction index not available");

    // Get transaction index for the previous transaction
    CDiskTxPos postx;
    if (!pblocktree->ReadTxIndex(txin.prevout.hash, postx))
        return error("CheckProofOfStake() : tx index not found"); // tx index not found

    // Read txPrev and header of its block
    CBlockHeader header;
    CTransactionRef txPrev;
    {
        CAutoFile file(OpenBlockFile(postx, true), SER_DISK, CLIENT_VERSION);
        try {
            file >> header;
            fseek(file.Get(), postx.nTxOffset, SEEK_CUR);
            file >> txPrev;
        } catch (std::exception& e) {
            return error("%s() : deserialize or I/O error in CheckProofOfStake()", __PRETTY_FUNCTION__);
        }
        if (txPrev->GetHash() != txin.prevout.hash)
            return error("%s() : txid mismatch in CheckProofOfStake()", __PRETTY_FUNCTION__);
    }

    int nDepth = 0;
    CBlockIndex* ptxPrevBlock = mapBlockIndex.find(header.GetHash()) != mapBlockIndex.end() ? mapBlockIndex[header.GetHash()] : NULL;
    if (ptxPrevBlock && pindexPrev)
        nDepth = (pindexPrev->nHeight + 1) - ptxPrevBlock->nHeight;

    if (nDepth < Params().GetConsensus().nStakeMinConfirmations - 1)
        return error("CheckProofOfStake() : tried to stake at depth %d", nDepth + 1);

    bool fOk = CheckStakeKernelHash(pindexPrev, nBits, header, tx, *txPrev, txin.prevout, hashProofOfStake, gArgs.GetBoolArg("-debug", false));
    if (hashProof)
        *hashProof = hashProofOfStake;
    return fOk;
}


#include <ql/quantlib.hpp>
//#include <ql/userconfig.hpp>
#include <iostream>
#include <vector>
#include <boost/foreach.hpp>

using namespace std;
using namespace QuantLib;

int main(void) {

    MCDiscreteArithmeticAPEngine<> mcd;
    return 0;
}


/** tsne.cc
    Jeremy Barnes, 16 December 2014
    Copyright (c) 2014 mldb.ai inc.  All rights reserved.

    This file is part of MLDB. Copyright 2015 mldb.ai inc. All rights reserved.

    Implementation of an TSNE algorithm for embedding of a dataset.
*/

#include "tsne_interface.h"
#include "mldb/builtin/matrix.h"
#include "mldb/core/mldb_engine.h"
#include "mldb/core/dataset.h"
#include "mldb/utils/distribution.h"
#include <boost/multi_array.hpp>
#include "mldb/base/scope.h"
#include "mldb/base/parallel.h"
#include "mldb/utils/pair_utils.h"
#include "mldb/arch/timers.h"
#include "mldb/arch/simd_vector.h"
#include "mldb/utils/vector_utils.h"
#include "mldb/types/basic_value_descriptions.h"
#include "mldb/types/any_impl.h"
#include "mldb/builtin/sql_config_validator.h"
#include "mldb/utils/vantage_point_tree.h"
#include "mldb/plugins/jml/tsne/tsne.h"
#include "mldb/sql/sql_expression.h"
#include "mldb/core/analytics.h"
#include "mldb/vfs/fs_utils.h"
#include "mldb/vfs/filter_streams.h"
#include "mldb/types/jml_serialization.h"
#include "mldb/utils/log.h"

using namespace std;

namespace MLDB {

DEFINE_STRUCTURE_DESCRIPTION(TsneConfig);

TsneConfigDescription::
TsneConfigDescription()
{
    addField("trainingData", &TsneConfig::trainingData,
             "Specification of the data for input to the TSNE procedure.  This should be "
             "organized as an embedding, with each selected row containing the same "
             "set of columns with numeric values to be used as coordinates.  The select statement "
             "does not support groupby and having clauses.");
    addField("rowOutputDataset", &TsneConfig::output,
             "Dataset for TSNE output, with embeddings of training data. "
             "One row will be added for each row in the input dataset, "
             "with a list of coordinates.",
             PolyConfigT<Dataset>().withType("embedding"));
    addField("numInputDimensions", &TsneConfig::numInputDimensions,
             "Number of dimensions from the input to use.  This will limit "
             "the columns to the n first columns in the alphabetical "
             "sorting of the columns (-1 = all).",
             -1);
    addField("numOutputDimensions", &TsneConfig::numOutputDimensions,
             "Number of dimensions to produce in t-SNE space.  Normally "
             "this will be 2 or 3, depending upon the number of dimensions "
             "in the visualization.",
             2);
    addField("tolerance", &TsneConfig::tolerance,
             "Tolerance of perplexity calculation.  This is an internal "
             "parameter that only needs to be changed in rare circumstances.");
    addField("perplexity", &TsneConfig::perplexity,
             "Perplexity to aim for; higher means more spread out.  This "
             "controls how hard t-SNE tries to spread the points out.  If "
             "the resulting output looks more like a ball or a sphere than "
             "individual clusters, you should reduce this number.  If it "
             "looks like a dot or star, you should increase it.",
             30.0);
    addField("learningRate", &TsneConfig::learningRate,
             "The learning rate specifies the gradient descent step size during "
             "optimization of the cost function.  A learning rate that is too small "
             "may hold optimization in a local minimum.  A learning rate that is too high "
             "may jump over the best optimal point. In general, the learning rate "
             "should be between 100 and 1000.",
             500.0);
    addField("modelFileUrl", &TsneConfig::modelFileUrl,
             "URL where the model file (with extension '.tsn') should be saved. "
             "This file can be loaded by the ![](%%doclink tsne.embedRow function). "
             "This parameter is optional unless the `functionName` parameter is used.");
    addField("functionName", &TsneConfig::functionName,
             "If specified, an instance of the ![](%%doclink tsne.embedRow function) of this name will be created using "
             "the trained model. Note that to use this parameter, the `modelFileUrl` must "
             "also be provided.");
    addAuto("minIterations", &TsneConfig::minIterations,
            "Minimum number of t-SNE iterations to run.  Making this too "
            "small may lead to spurious early stopping.");
    addAuto("maxIterations", &TsneConfig::maxIterations,
            "Maximum number of t-SNE iterations to run (it will stop if "
            "convergance is obtained before this number of iterations). ");
    addParent<ProcedureConfig>();

    onPostValidate = chain(validateQuery(&TsneConfig::trainingData,
                                         MustContainFrom(),
                                         NoGroupByHaving()),
                           validateFunction<TsneConfig>());
}

/*****************************************************************************/
/* TSNE PROCEDURE                                                             */
/*****************************************************************************/

struct TsneItl {
    TsneItl()
    {
    }

    TsneItl(const Url & filename)
    {
        filter_istream stream(filename);
        MLDB::DB::Store_Reader store(stream);

        reconstitute(store);

        stream.close();
    }

    ML::TSNE_Params params;
    boost::multi_array<float, 2> inputPath;
    boost::multi_array<float, 2> outputPath;
    std::unique_ptr<MLDB::VantagePointTree> vpTree;
    std::unique_ptr<MLDB::Quadtree> qtree;
    std::vector<Utf8String> inputColumnNames;
    std::vector<Utf8String> outputColumnNames;
    std::shared_ptr<const std::vector<ColumnPath> > outputColumnNamesShared;

    size_t numOutputDimensions() const { return outputPath.shape()[1]; }

    int64_t memusage() const
    {
        int64_t result = sizeof(*this);
        result += sizeof(float) * inputPath.shape()[0] * inputPath.shape()[1];
        result += sizeof(float) * outputPath.shape()[0] * outputPath.shape()[1];
        result += vpTree->memusage();
        result += qtree->root->memusage();
        return result;
    }

    void save(const std::string & filename) const
    {
        filter_ostream stream(filename);
        MLDB::DB::Store_Writer store(stream);
        serialize(store);
    }

    void serialize(MLDB::DB::Store_Writer & store) const
    {
        using namespace MLDB::DB;

        store << string("TSNE") << compact_size_t(2);

        size_t rows = inputPath.shape()[0];
        size_t dimsIn = inputPath.shape()[1];
        size_t dimsOut = outputPath.shape()[1];

        store << compact_size_t(rows)
              << compact_size_t(dimsIn)
              << compact_size_t(dimsOut);

        store << inputPath << outputPath;

        store << inputColumnNames << outputColumnNames;

        MLDB::VantagePointTree::serializePtr(store, vpTree.get());
        qtree->serialize(store);
    }

    void reconstitute(MLDB::DB::Store_Reader & store)
    {
        using namespace MLDB::DB;

        string tag;
        store >> tag;
        if (tag != "TSNE")
            throw MLDB::Exception("Expected TSNE tag");
        compact_size_t version(store);
        if (version != 2)
            throw MLDB::Exception("Unknown version for t-SNE");

        compact_size_t rows(store), dimsIn(store), dimsOut(store);

        store >> inputPath >> outputPath;

        store >> inputColumnNames >> outputColumnNames;

        outputColumnNamesShared.reset(new vector<ColumnPath>(outputColumnNames.begin(), outputColumnNames.end()));

        vpTree.reset(MLDB::VantagePointTree::reconstitutePtr(store));
        qtree.reset(new MLDB::Quadtree(store));
    }

    distribution<float> reembed(const distribution<float> & v) const
    {
        return retsneApproxFromCoords(v, inputPath, outputPath,
                                      *qtree, *vpTree, params);
    }

};

TsneProcedure::
TsneProcedure(MldbEngine * owner,
            PolyConfig config,
            const std::function<bool (const Json::Value &)> & onProgress)
    : Procedure(owner)
{
    tsneConfig = config.params.convert<TsneConfig>();
}

Any
TsneProcedure::
getStatus() const
{
    return Any();
}

RunOutput
TsneProcedure::
run(const ProcedureRunConfig & run,
      const std::function<bool (const Json::Value &)> & onProgress) const
{
    auto runProcConf = applyRunConfOverProcConf(tsneConfig, run);

    if (!runProcConf.modelFileUrl.empty()) {
        checkWritability(runProcConf.modelFileUrl.toString(), "modelFileUrl");
    }

    auto onProgress2 = [&] (const Json::Value & progress)
        {
            Json::Value value;
            value["dataset"] = progress;
            return onProgress(value);
        };

    // 1.  Get a numsubjects by numdimensions input matrix to train the
    //     t-SNE algorithm on
    auto itl = std::make_shared<TsneItl>();

    itl->params.perplexity = runProcConf.perplexity;
    itl->params.tolerance = runProcConf.tolerance;
    itl->params.eta = runProcConf.learningRate;
    itl->params.min_iter = runProcConf.minIterations;
    itl->params.max_iter = runProcConf.maxIterations;

    DEBUG_MSG(logger) << "perplexity = " << itl->params.perplexity;
    DEBUG_MSG(logger) << "tolerance = " << itl->params.tolerance;
    DEBUG_MSG(logger) << "learningRate = " << itl->params.eta;
    DEBUG_MSG(logger) << "minIterations = " << itl->params.min_iter;
    DEBUG_MSG(logger) << "maxIterations = " << itl->params.max_iter;
    DEBUG_MSG(logger) << "doing t-SNE";

    SqlExpressionMldbScope context(engine);

    ConvertProgressToJson convertProgressToJson(onProgress);
    auto embeddingOutput = getEmbedding(*runProcConf.trainingData.stm,
                                        context,
                                        runProcConf.numInputDimensions,
                                        convertProgressToJson);

    std::vector<std::tuple<RowHash, RowPath, std::vector<double>,
                           std::vector<ExpressionValue> > > & rows
        = embeddingOutput.first;
    std::vector<KnownColumn> & vars = embeddingOutput.second;

    size_t numDims = vars.size();

    DEBUG_MSG(logger) << "numDims = " << numDims;

    boost::multi_array<float, 2> coords
        (boost::extents[rows.size()][numDims]);

    for (unsigned i = 0;  i < rows.size();  ++i) {
        for (auto & e: std::get<2>(rows[i]))
            ExcAssert(isfinite(e));
        std::copy(std::get<2>(rows[i]).begin(), std::get<2>(rows[i]).end(),
                  &coords[i][0]);
    }

    if (coords.size() == 0)
        throw AnnotatedException(400, "t-sne training requires at least 1 datapoint. "
                                  "Make sure your dataset is not empty and that your WHERE, offset "
                                  "and limit expressions do not filter all the rows");

    DEBUG_MSG(logger) << "copied into matrix";

//     DEBUG_MSG(logger) << "rows[0] = " << rows[0].second;
//     DEBUG_MSG(logger) << "rows[1] = " << rows[1].second;
//     DEBUG_MSG(logger) << "rows[2] = " << rows[1].second;
//     DEBUG_MSG(logger) << "rows[0] dot rows[1] = " << rows[0].second.dotprod(rows[1].second);
//     DEBUG_MSG(logger) << "rows[0] dot rows[2] = " << rows[0].second.dotprod(rows[2].second);
//     DEBUG_MSG(logger) << "rows[0] dist rows[1] = " << (rows[0].second - rows[1].second).two_norm();
//     DEBUG_MSG(logger) << "rows[0] dist rows[2] = " << (rows[0].second - rows[2].second).two_norm();
//     DEBUG_MSG(logger) << "rows[1] dist rows[2] = " << (rows[1].second - rows[2].second).two_norm();

    itl->inputPath.resize(boost::extents[rows.size()][numDims]);
    itl->inputPath = coords;

    ML::TSNE_Callback callback = [&] (int iter, float cost,
                                      std::string phase)
        {
            if (iter == 1 || iter % 10 == 0)
                INFO_MSG(logger) << "phase " << phase << " iter " << iter
                     << " cost " << cost;
            return true;
        };

    ExcAssertGreaterEqual(runProcConf.numOutputDimensions, 1);

    itl->outputPath.resize(boost::extents[rows.size()][runProcConf.numOutputDimensions]);
    itl->outputPath
        = ML::tsneApproxFromCoords(coords, runProcConf.numOutputDimensions,
                                   itl->params, callback, &itl->vpTree,
                                   &itl->qtree);

    ExcAssert(itl->qtree);
    ExcAssert(itl->vpTree);

    vector<ColumnPath> names = { ColumnPath("x"), ColumnPath("y"), ColumnPath("z") };
    if (runProcConf.numOutputDimensions <= 3)
        names.resize(runProcConf.numOutputDimensions);
    else {
        names.clear();
        for (unsigned i = 0; i < runProcConf.numOutputDimensions;  ++i)
            names.push_back(ColumnPath(MLDB::format("dim%04d", i)));
    }

    // Record the column names for later
    for (auto & c: vars) {
        itl->inputColumnNames.emplace_back(c.columnName.toUtf8String());
    }
    for (auto & c: names) {
        itl->outputColumnNames.emplace_back(c.toUtf8String());
    }

    itl->outputColumnNamesShared
        .reset(new vector<ColumnPath>(itl->outputColumnNames.begin(),
                                      itl->outputColumnNames.end()));

    if (!runProcConf.modelFileUrl.empty()) {
        makeUriDirectory(runProcConf.modelFileUrl.toString());
        itl->save(runProcConf.modelFileUrl.toString());
    }

    // Create a dataset to contain the output embedding if we ask for it
    if (!runProcConf.output.type.empty()) {
        auto output = createDataset(engine, runProcConf.output, onProgress2, true /*overwrite*/);

        for (unsigned i = 0;  i < rows.size();  ++i) {
            TRACE_MSG(logger) << "row " << i << " had coords " << itl->outputPath[i][0] << ","
                 << itl->outputPath[i][1];
            std::vector<std::tuple<ColumnPath, CellValue, Date> > cols;
            for (unsigned j = 0;  j < runProcConf.numOutputDimensions;  ++j) {
                ExcAssert(isfinite(itl->outputPath[i][j]));
                cols.emplace_back(names[j], itl->outputPath[i][j], Date());
            }

            output->recordRow(std::get<1>(rows[i]), cols);
        }

        output->commit();
    }

    if(!runProcConf.functionName.empty()) {
        PolyConfig tsneFuncPC;
        tsneFuncPC.type = "tsne.embedRow";
        tsneFuncPC.id = runProcConf.functionName;
        tsneFuncPC.params = TsneEmbedConfig(runProcConf.modelFileUrl);

        createFunction(engine, tsneFuncPC, onProgress, true);
    }

    return Any();
}

DEFINE_STRUCTURE_DESCRIPTION(TsneEmbedConfig);

TsneEmbedConfigDescription::
TsneEmbedConfigDescription()
{
    addField("modelFileUrl", &TsneEmbedConfig::modelFileUrl,
             "URL of the model file (with extension '.tns') to load. "
             "This file is created by the ![](%%doclink tsne.train procedure).");
}

/*****************************************************************************/
/* TSNE EMBED ROW                                                            */
/*****************************************************************************/

DEFINE_STRUCTURE_DESCRIPTION(TsneInput);

TsneInputDescription::TsneInputDescription()
{
    addField("embedding", &TsneInput::embedding,
             "Undocumented.");
}

DEFINE_STRUCTURE_DESCRIPTION(TsneOutput);

TsneOutputDescription::TsneOutputDescription()
{
    addField("cluster", &TsneOutput::tsne,
             "Undocumented.");
}

TsneEmbed::
TsneEmbed(MldbEngine * owner,
          PolyConfig config,
          const std::function<bool (const Json::Value &)> & onProgress)
    : BaseT(owner, config)
{
    functionConfig = config.params.convert<TsneEmbedConfig>();
    itl = std::make_shared<TsneItl>(functionConfig.modelFileUrl);
}

TsneOutput
TsneEmbed::
call(TsneInput input) const
{
    throw AnnotatedException(500, "t-SNE Embed apply function is not yet implemented");
#if 0
    ExpressionValue result;

    ExpressionValue storage;
    const ExpressionValue & inputVal = context.get("embedding", storage);

    distribution<float> input = inputVal.getEmbedding(itl->inputColumnNames.size());

    Date ts = Date::negativeInfinity();
    auto embedding = itl->reembed(input);

    result.set("tsne", ExpressionValue(embedding, ts));

    return result;
#endif
}

namespace {

RegisterProcedureType<TsneProcedure, TsneConfig>
regTsne(builtinPackage(),
        "Project a high dimensional space into a low-dimensional space suitable for visualization",
        "procedures/TsneProcedure.md.html");

RegisterFunctionType<TsneEmbed, TsneEmbedConfig>
regTsneEmbed(builtinPackage(),
             "tsne.embedRow",
             "Embed a pre-trained t-SNE algorithm to new data points",
             "functions/TsneEmbed.md.html",
             nullptr,
             {MldbEntity::INTERNAL_ENTITY});

} // file scope

} // namespace MLDB


// Lane keep and automatic cruise control system being tested
// against the monoDrive simulator in closed loop mode

#include <iostream>		  // std::cout
#include <chrono>		  // std::chrono
#include <thread>         // std::thread
#include <memory>
#include <vector>
#include <future>

// monoDrive includes
#include "Simulator.h"
#include "Configuration.h"
#include "Sensor.h"
#include "sensor_config.h"
#include "DataFrame.h"
#include "Stopwatch.h"
#include "Jobs.h"

#include "opencv2/core.hpp"
#include "opencv2/core/utility.hpp"
#include "opencv2/highgui.hpp"
#include "opencv2/imgproc.hpp"
#include <Eigen/Eigen>

#include "pid.h"

// test critera
const unsigned int DURATION = 10000;  // ms
const float MAX_LANE_DEVIATION = 25;  // cm

// speed control
const float DESIRED_SPEED = 1000.0;
const float LOOK_AHEAD = 200;

PID pid(0.01f, 0.0005f, 0.0002f, -1.0f, 1.0f);
float last_time = 0;
float last_throttle = 0;

// simulator server
std::string server_ip = "127.0.0.1";
int server_port = 8999;

// results
float deviation = 0;

EgoControlConfig planning(DataFrame* stateFrame, DataFrame* waypointFrame) {
    StateFrame* state = static_cast<StateFrame*>(stateFrame);
    WaypointFrame* waypoints = static_cast<WaypointFrame*>(waypointFrame);

    // find ego
    VehicleState* ego = nullptr;
    for (auto& vehicle : state->vehicles){
        for(auto& tag : vehicle.state.tags){
            if(tag == "ego"){
                ego = &vehicle;
                break;
            }
        }
    }
    if (ego == nullptr) {
        std::cout << "No ego vehicle in frame." << std::endl;
        return EgoControlConfig();
    }

    // find lane waypoints
    Eigen::VectorXd curr(3);
    Eigen::VectorXd next(3);
    curr.setZero();
    for (const auto& actor : waypoints->actor_waypoints) {
        if (actor.actor_id == ego->state.name) {
            for(const auto& lane : actor.lanes) {
                if(lane.waypoints.size() >= 2) {
                    curr << lane.waypoints[0].location.x,
                        lane.waypoints[0].location.y,
                        lane.waypoints[0].location.z;
                    next << lane.waypoints[1].location.x,
                        lane.waypoints[1].location.y,
                        lane.waypoints[1].location.z;
                    break;
                }
            }
        }
    }
    if (curr.isZero()) {
        std::cout << "Unable to get ego lane waypoints."<< std::endl;
        return EgoControlConfig();
    }

    // use lane follower for steering
    Eigen::VectorXd position(3);
    position << ego->state.odometry.pose.position.x,
        ego->state.odometry.pose.position.y,
        ego->state.odometry.pose.position.z;
    Eigen::Quaternion<double> orientation(
        ego->state.odometry.pose.orientation.w,
        ego->state.odometry.pose.orientation.x,
        ego->state.odometry.pose.orientation.y,
        ego->state.odometry.pose.orientation.z
    );
    Eigen::VectorXd forwardVector(3);
    forwardVector << 1, 0, 0;
    forwardVector = orientation * forwardVector;
    Eigen::VectorXd dirToNextPoint = next - position;
    dirToNextPoint.normalize();

    double angle = -dirToNextPoint.head<3>().cross(forwardVector.head<3>())[2];

    // use PID speed controller for throttle
    Eigen::VectorXd velocity(3);
    velocity << ego->state.odometry.linear_velocity.x,
        ego->state.odometry.linear_velocity.y,
        ego->state.odometry.linear_velocity.z;
    double speed = velocity.norm();

    double dt = last_time > 0 ? state->game_time - last_time : 0.1;
    last_time = state->game_time;

    float throttle = last_throttle;
    if (dt) {
        throttle = pid.pid(DESIRED_SPEED - speed, dt);
        last_throttle = throttle;
    }

    // get lane deviation
    double dev = (curr - position).norm();
    if (dev > deviation) {
        deviation = dev;
    }

    // form controls response
    EgoControlConfig egoControl;
    egoControl.forward_amount = std::max(0.0f, throttle);
    egoControl.brake_amount = std::min(0.0f, throttle);
    egoControl.drive_mode = 1;
    egoControl.right_amount = (float)angle;
    return egoControl;
}

int uut(int argc, char** argv, Job* job)
{
    // reset any jobs globals
    pid.reset();
    last_time = 0;
    last_throttle = 0;
    deviation = 0;

    // get configuration from job
    Configuration config = job->getConfig();

    // set to closed loop mode
    config.simulator["simulation_mode"] = 0;

    // setup simulator
    Simulator& sim = Simulator::getInstance(config, server_ip, server_port);

    if(!sim.configure()){
        return -1;
    }

    // configure sensors
    std::vector<std::shared_ptr<Sensor>> sensors;

    ViewportCameraConfig vp_config;
    vp_config.server_ip = sim.getServerIp();
    vp_config.server_port = sim.getServerPort();
    vp_config.location.z = 400;
    vp_config.location.x = -800;
    vp_config.rotation.pitch = -15;
    Sensor(std::make_unique<ViewportCameraConfig>(vp_config)).configure();

    StateConfig state_config;
    state_config.desired_tags = {"ego", "vehicle"};
    state_config.undesired_tags = {"static"};
    state_config.server_ip = sim.getServerIp();
    state_config.server_port = sim.getServerPort();
    state_config.listen_port = 8101;
    state_config.debug_drawing = true;
    sensors.push_back(std::make_shared<Sensor>(std::make_unique<StateConfig>(state_config)));

    WaypointConfig wp_config;
    wp_config.server_ip = sim.getServerIp();
    wp_config.server_port = sim.getServerPort();
    wp_config.listen_port = 8102;
    wp_config.distance = 2000;
    wp_config.frequency = LOOK_AHEAD;
    wp_config.draw_debug = true;
    wp_config.debug_tags = {"ego"};
    sensors.push_back(std::make_shared<Sensor>(std::make_unique<WaypointConfig>(wp_config)));

    std::cout << "Configuring sensors..." << std::endl;
    for (auto& sensor : sensors)
    {
        sensor->configure();
    }

    // execute test
    mono::precise_stopwatch watch;
    int count = 0;
    while (watch.elapsed_time<unsigned int, std::chrono::milliseconds>() < DURATION)
    {
        // sampling/perception
        sim.sampleAll(sensors);

        // planning
        EgoControlConfig egoControl = planning(sensors[0]->frame, sensors[1]->frame);

        // control
        sim.sendCommand(ApiMessage(777, EgoControl_ID, true, egoControl.dump()));

        if (deviation > MAX_LANE_DEVIATION) {
            std::cout << "Lane deviation exceeded limit: "
                << deviation <<" cm. Exiting early." << std::endl;
            sim.sendControl(0.f, 0.f, 1.f, 1);
            break;
        }
        count++;
    }
    double fps = (double)count * 1000.0 / watch.elapsed_time<double, std::chrono::milliseconds>();

    // write test result
    ResultMetric metricDeviation;
    metricDeviation.name = "lane_deviation";
    metricDeviation.score = deviation;

    ResultMetric metricFPS;
    metricFPS.name = "fps";
    metricFPS.score = fps;

    Result res;
    res.pass = deviation < MAX_LANE_DEVIATION;
    res.message = "this job passed";
    res.metrics.push_back(metricDeviation);
    res.metrics.push_back(metricFPS);

    job->setResult(res);

    // cleanup
    Simulator::clearInstances();

    return 0;
}

int main(int argc, char** argv)
{
    Job job(argc, argv);
    return job.run(uut);
}