/*******************************************************************************
*
*  Filename    : PlotLimit.cc
*  Description : Macros For plotting confidence level limits
*  Author      : Yi-Mu "Enoch" Chen [ ensc@hep1.phys.ntu.edu.tw ]
*
*******************************************************************************/
#include "ManagerUtils/Common/interface/ConfigReader.hpp"
#include "ManagerUtils/Common/interface/STLUtils.hpp"
#include "ManagerUtils/Maths/interface/Intersect.hpp"
#include "ManagerUtils/Maths/interface/Parameter.hpp"
#include "ManagerUtils/PlotUtils/interface/Common.hpp"
#include "ManagerUtils/SampleMgr/interface/SampleMgrLoader.hpp"

#include "TstarAnalysis/Common/interface/PlotStyle.hpp"
#include "TstarAnalysis/LimitCalc/interface/Common.hpp"
#include "TstarAnalysis/LimitCalc/interface/Limit.hpp"

#include <algorithm>
#include <boost/algorithm/string.hpp>
#include <boost/filesystem.hpp>
#include <boost/format.hpp>
#include <fstream>
#include <map>
#include <sstream>
#include <string>
#include <vector>

#include "TFile.h"
#include "TTree.h"

using namespace std;
using namespace mgr;

/*******************************************************************************
*   Defining plotting control flow
*******************************************************************************/
pair<Parameter, Parameter>
MakeLimitPlot( const std::string& additionaltag )
{
  const map<double, double> xsec = GetXsectionMap();

  TGraph* onesiggraph = MakeCalcGraph( xsec, additionaltag, explim, onesig_up, onesig_down );
  TGraph* twosiggraph = MakeCalcGraph( xsec, additionaltag, explim, twosig_up, twosig_down );
  TGraph* expgraph    = MakeCalcGraph( xsec, additionaltag, explim, skip, skip );
  TGraph* obsgraph    = MakeCalcGraph( xsec, additionaltag, obslim, skip, skip );
  TGraph* theorygraph = MakeTheoryGraph( xsec );

  const double y_max = mgr::GetYmax( twosiggraph, obsgraph );
  const double y_min = mgr::GetYmin( twosiggraph, obsgraph );
  const double x_max = theorygraph->GetX()[theorygraph->GetN()-1];
  const double x_min = theorygraph->GetX()[0];

  // ----- Setting Styles  --------------------------------------------------------
  tstar::SetOneSigmaStyle( onesiggraph );
  tstar::SetTwoSigmaStyle( twosiggraph );
  tstar::SetExpLimitStyle( expgraph );
  tstar::SetObsLimitStyle( obsgraph );
  tstar::SetTheoryStyle( theorygraph );

  // Making object for plotting
  TCanvas* c1 = mgr::NewCanvas();
  mgr::SetSinglePad( c1 );

  TMultiGraph* mg = new TMultiGraph();

  mg->Add( twosiggraph );
  mg->Add( onesiggraph );
  mg->Add( theorygraph );
  mg->Add( expgraph );
  if( limnamer.CheckInput( "drawdata" ) ){
    mg->Add( obsgraph );
  }

  mg->Draw( "AL3" );
  mgr::SetAxis( mg );
  mg->GetXaxis()->SetTitle( "t* Mass (GeV/c^{2})" );// MUST Be after draw!!
  mg->GetYaxis()->SetTitle( "#sigma(pp#rightarrow t*#bar{t}*) (pb)" );// https://root.cern.ch/root/roottalk/roottalk09/0078.html
  mg->GetXaxis()->SetLimits( x_min, x_max );
  mg->SetMaximum( y_max*300. );
  mg->SetMinimum( y_min*0.3 );

  // Making legend
  const double legend_x_min = 0.55;
  const double legend_y_min = 0.50;
  TLegend* l                = mgr::NewLegend( legend_x_min, legend_y_min );
  l->AddEntry( theorygraph,    "R-S Model",              "lf" );
  l->AddEntry( (TObject*)NULL, "",                       "" );
  l->AddEntry( (TObject*)NULL, "95% CL_{s} upper limit", "" );
  l->AddEntry( obsgraph,       "Observed",               "l" );
  l->AddEntry( expgraph,       "Medium expected",        "l" );
  l->AddEntry( onesiggraph,    "68% expected",           "f" );
  l->AddEntry( twosiggraph,    "95% expected",           "f" );
  l->Draw();

  // Additional titles settings
  mgr::DrawCMSLabel();
  mgr::DrawLuminosity( limnamer.GetExt<double>( "era", "Lumi" ) );

  // Writing additional text
  const string rootlabel = limnamer.GetChannelEXT( "Root Name" );
  // const string fitmethod = limnamer.GetExt<string>( "fitmethod", "Full Name" );
  // const string funcname  = limnamer.GetExt<string>( "fitfunc", "Full Name" );
  boost::format explimfmt( "Expected: %s GeV/c^{2}" );
  boost::format obslimfmt( "Observed: %s GeV/c^{2}" );

  mgr::LatexMgr latex;
  latex.SetOrigin( PLOT_X_MIN, PLOT_Y_MAX + TEXT_MARGIN/2, BOTTOM_LEFT )
  .WriteLine( rootlabel );
  // .WriteLine( fitmethod )
  // .WriteLine( funcname );

  // Cannot have Latex style spacing '\,'
  const Parameter explim_err = GetInterSect( theorygraph, onesiggraph );
  const Parameter obslim_err = GetInterSect( theorygraph, obsgraph );
  const string explim_str    = FloatingPoint( explim_err.CentralValue(), 0, false );
  const string obslim_str    = FloatingPoint( obslim_err.CentralValue(), 0, false );

  latex.SetOrigin( PLOT_X_TEXT_MAX, legend_y_min-TEXT_MARGIN, TOP_RIGHT );
  latex.WriteLine( boost::str( explimfmt % explim_str ) );
  if( limnamer.CheckInput( "drawdata" ) ){
    latex.WriteLine( boost::str( obslimfmt % obslim_str ) );
  }

  // ----- Saving and cleaning up  ------------------------------------------------
  c1->SetLogy( kTRUE );
  c1->Update();

  if( limnamer.CheckInput( "seed" ) ){
    limnamer.AddCutOptions( "seed" );
  }

  if( limnamer.CheckInput( "rMin" ) ){
    limnamer.AddCutOptions( "rMin" );
  }

  if( limnamer.CheckInput( "rMax" ) ){
    limnamer.AddCutOptions( "rMax" );
  }

  mgr::SaveToROOT( c1, limnamer.PlotRootFile(), limnamer.OptFileName( "", "limit", additionaltag ) );
  mgr::SaveToPDF( c1, limnamer.PlotFileName( "limit", additionaltag ) );
  delete onesiggraph;
  delete twosiggraph;
  delete obsgraph;
  delete expgraph;
  delete theorygraph;
  delete mg;
  delete l;
  delete c1;

  return make_pair( explim_err, obslim_err );
}

/*******************************************************************************
*   Filling out map from master file
*******************************************************************************/
map<double, double>
GetXsectionMap()
{
  const string theoryfile = limnamer.SubPackageDir() / "data/excitedtoppair13TeV.dat";
  if( !boost::filesystem::exists( theoryfile ) ){
    cerr << "Error! File containing theoretical cross section doesn't exists!" << endl;
    cerr << "Check file: " << theoryfile << endl;
    throw std::runtime_error( "File not found" );
  }

  ifstream xsec_file( theoryfile );
  string line;
  map<double, double> ans;
  double energy, mass, xsec_value;

  while( getline( xsec_file, line ) ){
    istringstream linestream( line );
    linestream >> energy >> mass >> xsec_value;
    ans[mass] = xsec_value * 1000.;
  }

  xsec_file.close();
  return ans;
}

/*******************************************************************************
*   Object generation functions
*******************************************************************************/
TGraphAsymmErrors*
MakeTheoryGraph( const map<double, double>& xsec )
{
  // Making the PDF/QCD scale error Graph objects for linear interpolation
  const vector<string> siglist = limnamer.MasterConfig().GetStaticStringList( "Signal List" );
  TGraph* errorupgraph         = new TGraph( siglist.size() );
  TGraph* errordowngraph       = new TGraph( siglist.size() );

  for( size_t i = 0; i < siglist.size(); ++i ){
    const auto& sig   = siglist.at( i );
    const double mass = GetInt( sig );

    mgr::SampleGroup sigmgr( sig, limnamer.MasterConfig() );

    const mgr::Parameter pdferr   = sigmgr.Sample().PDFUncertainty();
    const mgr::Parameter scaleerr = sigmgr.Sample().QCDScaleUncertainty();
    const mgr::Parameter toterr   = pdferr * scaleerr;

    errorupgraph->SetPoint( i, mass, toterr.RelUpperError() );
    errordowngraph->SetPoint( i, mass, toterr.RelLowerError() );
  }

  // Making the main theoretical curve line
  TGraphAsymmErrors* graph = new TGraphAsymmErrors();

  for( const auto& point : xsec ){
    const double mass = point.first;
    if( mass < 650 || mass > 1650 ){ continue; }
    const double thisxsec = point.second;
    const double xsecup   = thisxsec * errorupgraph->Eval( mass );
    const double xsecdown = thisxsec * errordowngraph->Eval( mass );

    const int newbin = graph->GetN();
    graph->SetPoint( newbin, mass, thisxsec );
    graph->SetPointError(
      newbin,
      0, 0,// No X error for now
      xsecdown,
      xsecup
      );
  }

  delete errorupgraph;
  delete errordowngraph;

  return graph;
}

/******************************************************************************/
extern const int explim      = 2;
extern const int obslim      = 5;
extern const int onesig_up   = 3;
extern const int onesig_down = 1;
extern const int twosig_up   = 4;
extern const int twosig_down = 0;
extern const int skip        = -1;

extern TGraph*
MakeCalcGraph(
  const std::map<double, double>& xsec,
  const string& additionaltag,
  const int centralentry,
  const int uperrorentry,
  const int downerrorentry
  )
{
  const vector<string> siglist = limnamer.MasterConfig().GetStaticStringList( "Signal List" );
  TGraphAsymmErrors* graph     = new TGraphAsymmErrors( siglist.size() );

  unsigned bin = 0;

  for( const auto& sig : siglist ){
    const double mass     = GetInt( sig );
    const double expxsec  = xsec.at( mass );
    const string filename = limnamer.RootFileName( "combine", sig, additionaltag );

    // Geting contents of higgs combine output file
    TFile* file = TFile::Open( filename.c_str() );
    if( !file ){
      fprintf( stderr, "Cannot open file (%s), skipping sample for %s\n",
        filename.c_str(),
        sig.c_str() );
      continue;
    }

    double temp;
    TTree* tree = ( (TTree*)file->Get( "limit" ) );
    tree->SetBranchAddress( "limit", &temp );

    // Getting the entries
    tree->GetEntry( centralentry );
    const double central = temp * expxsec;

    if( uperrorentry != skip ){ tree->GetEntry( uperrorentry ); }
    const double errorup = ( uperrorentry != skip ) ?
                           temp*expxsec - central :  0;

    if( downerrorentry != skip ){ tree->GetEntry( downerrorentry ); }
    const double errordown = ( downerrorentry != skip ) ?
                             central - temp*expxsec :  0;

    // closing file
    file->Close();

    graph->SetPoint( bin, mass, central );
    graph->SetPointError( bin, 50, 50, errordown, errorup );
    cout << boost::format( "%u %4.0lf  %8.6lf[+%8.6lf/-%8.6lf]" )
      % bin % mass % central % errorup % errordown << endl;
    ++bin;

    delete file;
  }

  return graph;
}

/*******************************************************************************
*   Function for calculating intersect points
*******************************************************************************/

Parameter
GetInterSect( const TGraph* graph, const TGraph* errgraph )
{
  double y             = 0;
  double explimit      = errgraph->GetX()[0];
  double explimit_up   = errgraph->GetX()[0];
  double explimit_down = errgraph->GetX()[0];

  for( int i = 0; i < graph->GetN()-1; ++i ){
    for( int j = 0; j < errgraph->GetN()-1; ++j ){

      const double graphmass        = graph->GetX()[i];
      const double graphmassnext    = graph->GetX()[i+1];
      const double graphval         = graph->GetY()[i];
      const double graphvalnext     = graph->GetY()[i+1];
      const double graphvalup       = graph->GetErrorYhigh( i ) + graphval;
      const double graphvalupnext   = graph->GetErrorYhigh( i+1 ) + graphvalnext;
      const double graphvaldown     = graphval - graph->GetErrorYlow( i );
      const double graphvaldownnext = graphvalnext - graph->GetErrorYlow( i+1 );

      const double errgraphmass     = errgraph->GetX()[j];
      const double errgraphmassnext = errgraph->GetX()[j+1];
      const double errgraphval      = errgraph->GetY()[j];
      const double errgraphvalnext  = errgraph->GetY()[j+1];
      const double hierr            = errgraph->GetErrorYhigh( j ) + errgraphval;
      const double hierrnext        = errgraph->GetErrorYhigh( j+1 ) + errgraphvalnext;
      const double loerr            = errgraphval - errgraph->GetErrorYlow( j );
      const double loerrnext        = errgraphvalnext - errgraph->GetErrorYlow( j+1 );

      // Central value intersect
      Intersect(
        graphmass, graphval,
        graphmassnext, graphvalnext,
        errgraphmass, errgraphval,
        errgraphmassnext, errgraphvalnext,
        explimit, y
        );

      // Lower theretical band intersect with upper limit band
      Intersect(
        graphmass, graphvaldown,
        graphmassnext, graphvaldownnext,
        errgraphmass, hierr,
        errgraphmassnext, hierrnext,
        explimit_down, y
        );

      // Upper theoretical band intersec with lower limit band
      Intersect(
        graphmass, graphvalup,
        graphmassnext, graphvalupnext,
        errgraphmass, loerr,
        errgraphmassnext, loerrnext,
        explimit_up, y
        );
    }
  }

  return Parameter( explimit, explimit_up - explimit, explimit - explimit_down );
}


/*
 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/builders/vanillaoption.hpp
    \brief Abstract engine builders for European and American Options
    \ingroup builders
*/

#pragma once

#include <boost/make_shared.hpp>
#include <ored/portfolio/builders/cachingenginebuilder.hpp>
#include <ored/portfolio/enginefactory.hpp>
#include <ored/utilities/indexparser.hpp>
#include <ored/utilities/log.hpp>
#include <ored/utilities/to_string.hpp>
#include <ql/pricingengines/vanilla/analyticeuropeanengine.hpp>
#include <ql/pricingengines/vanilla/fdblackscholesvanillaengine.hpp>
#include <ql/processes/blackscholesprocess.hpp>
#include <ql/version.hpp>
#include <qle/pricingengines/analyticcashsettledeuropeanengine.hpp>
#include <qle/pricingengines/baroneadesiwhaleyengine.hpp>
#include <qle/termstructures/blackmonotonevarvoltermstructure.hpp>
#include <qle/termstructures/pricetermstructureadapter.hpp>

namespace ore {
namespace data {

template <class T, typename... Args> class CachingOptionEngineBuilder : public CachingPricingEngineBuilder<T, Args...> {
public:
    CachingOptionEngineBuilder(const string& model, const string& engine, const set<string>& tradeTypes,
                               const AssetClass& assetClass)
        : CachingPricingEngineBuilder<T, Args...>(model, engine, tradeTypes), assetClass_(assetClass) {}

protected:
    boost::shared_ptr<GeneralizedBlackScholesProcess> getBlackScholesProcess(const string& assetName,
                                                                             const Currency& ccy,
                                                                             const AssetClass& assetClassUnderlying,
                                                                             const std::vector<Time>& timePoints = {}) {

        using VVTS = QuantExt::BlackMonotoneVarVolTermStructure;
        string config = this->configuration(ore::data::MarketContext::pricing);

        if (assetClassUnderlying == AssetClass::EQ) {
            Handle<BlackVolTermStructure> vol = this->market_->equityVol(assetName, config);
            if (!timePoints.empty()) {
                vol = Handle<BlackVolTermStructure>(boost::make_shared<VVTS>(vol, timePoints));
                vol->enableExtrapolation();
            }
            return boost::make_shared<GeneralizedBlackScholesProcess>(
                this->market_->equitySpot(assetName, config), this->market_->equityDividendCurve(assetName, config),
                this->market_->equityForecastCurve(assetName, config), vol);

        } else if (assetClassUnderlying == AssetClass::FX) {
            const string& ccyPairCode = assetName + ccy.code();
            Handle<BlackVolTermStructure> vol = this->market_->fxVol(ccyPairCode, config);
            if (!timePoints.empty()) {
                vol = Handle<BlackVolTermStructure>(boost::make_shared<VVTS>(vol, timePoints));
                vol->enableExtrapolation();
            }
            return boost::make_shared<GeneralizedBlackScholesProcess>(
                this->market_->fxSpot(ccyPairCode, config), this->market_->discountCurve(assetName, config),
                this->market_->discountCurve(ccy.code(), config), vol);

        } else if (assetClassUnderlying == AssetClass::COM) {

            Handle<BlackVolTermStructure> vol = this->market_->commodityVolatility(assetName, config);
            if (!timePoints.empty()) {
                vol = Handle<BlackVolTermStructure>(boost::make_shared<VVTS>(vol, timePoints));
                vol->enableExtrapolation();
            }

            // Create the commodity convenience yield curve for the process
            Handle<QuantExt::PriceTermStructure> priceCurve = this->market_->commodityPriceCurve(assetName, config);
            Handle<Quote> commoditySpot(boost::make_shared<QuantExt::DerivedPriceQuote>(priceCurve));
            Handle<YieldTermStructure> discount = this->market_->discountCurve(ccy.code(), config);
            Handle<YieldTermStructure> yield(
                boost::make_shared<QuantExt::PriceTermStructureAdapter>(*priceCurve, *discount));
            yield->enableExtrapolation();

            return boost::make_shared<GeneralizedBlackScholesProcess>(commoditySpot, yield, discount, vol);

        } else {
            QL_FAIL("Asset class of " << (int)assetClassUnderlying << " not recognized.");
        }
    }
    AssetClass assetClass_;
};

//! Abstract Engine Builder for Vanilla Options
/*! Pricing engines are cached by asset/currency

    \ingroup builders
 */
class VanillaOptionEngineBuilder
    : public CachingOptionEngineBuilder<string, const string&, const Currency&, const AssetClass&, const Date&> {
public:
    VanillaOptionEngineBuilder(const string& model, const string& engine, const set<string>& tradeTypes,
                               const AssetClass& assetClass, const Date& expiryDate)
        : CachingOptionEngineBuilder(model, engine, tradeTypes, assetClass), expiryDate_(expiryDate) {}

    boost::shared_ptr<PricingEngine> engine(const string& assetName, const Currency& ccy, const Date& expiryDate) {
        return CachingPricingEngineBuilder<string, const string&, const Currency&, const AssetClass&,
                                           const Date&>::engine(assetName, ccy, assetClass_, expiryDate);
    }

    boost::shared_ptr<PricingEngine> engine(const Currency& ccy1, const Currency& ccy2, const Date& expiryDate) {
        return CachingPricingEngineBuilder<string, const string&, const Currency&, const AssetClass&,
                                           const Date&>::engine(ccy1.code(), ccy2, assetClass_, expiryDate);
    }

protected:
    virtual string keyImpl(const string& assetName, const Currency& ccy, const AssetClass& assetClassUnderlying,
                           const Date& expiryDate) override {
        return assetName + "/" + ccy.code() + "/" + to_string(expiryDate);
    }

    Date expiryDate_;
};

//! Abstract Engine Builder for European Vanilla Options
/*! Pricing engines are cached by asset/currency

    \ingroup builders
 */
class EuropeanOptionEngineBuilder : public VanillaOptionEngineBuilder {
public:
    EuropeanOptionEngineBuilder(const string& model, const set<string>& tradeTypes, const AssetClass& assetClass)
        : VanillaOptionEngineBuilder(model, "AnalyticEuropeanEngine", tradeTypes, assetClass, Date()) {}

protected:
    virtual boost::shared_ptr<PricingEngine> engineImpl(const string& assetName, const Currency& ccy,
                                                        const AssetClass& assetClassUnderlying,
                                                        const Date& expiryDate) override {
        string key = keyImpl(assetName, ccy, assetClassUnderlying, expiryDate);
        boost::shared_ptr<GeneralizedBlackScholesProcess> gbsp =
            getBlackScholesProcess(assetName, ccy, assetClassUnderlying);
        Handle<YieldTermStructure> discountCurve =
            market_->discountCurve(ccy.code(), configuration(MarketContext::pricing));
        return boost::make_shared<QuantLib::AnalyticEuropeanEngine>(gbsp, discountCurve);
    }
};

/*! European cash-settled option engine builder
    \ingroup builders
 */
class EuropeanCSOptionEngineBuilder : public VanillaOptionEngineBuilder {
public:
    EuropeanCSOptionEngineBuilder(const string& model, const set<string>& tradeTypes, const AssetClass& assetClass)
        : VanillaOptionEngineBuilder(model, "AnalyticCashSettledEuropeanEngine", tradeTypes, assetClass, Date()) {}

protected:
    virtual boost::shared_ptr<PricingEngine> engineImpl(const string& assetName, const Currency& ccy,
                                                        const AssetClass& assetClassUnderlying,
                                                        const Date& expiryDate) override {
        string key = keyImpl(assetName, ccy, assetClassUnderlying, expiryDate);
        boost::shared_ptr<GeneralizedBlackScholesProcess> gbsp =
            getBlackScholesProcess(assetName, ccy, assetClassUnderlying);
        Handle<YieldTermStructure> discountCurve =
            market_->discountCurve(ccy.code(), configuration(MarketContext::pricing));
        return boost::make_shared<QuantExt::AnalyticCashSettledEuropeanEngine>(gbsp, discountCurve);
    }
};

//! Abstract Engine Builder for American Vanilla Options
/*! Pricing engines are cached by asset/currency

    \ingroup builders
 */
class AmericanOptionEngineBuilder : public VanillaOptionEngineBuilder {
public:
    AmericanOptionEngineBuilder(const string& model, const string& engine, const set<string>& tradeTypes,
                                const AssetClass& assetClass, const Date& expiryDate)
        : VanillaOptionEngineBuilder(model, engine, tradeTypes, assetClass, expiryDate) {}
};

//! Abstract Engine Builder for American Vanilla Options using Finite Difference Method
/*! Pricing engines are cached by asset/currency

    \ingroup builders
 */
class AmericanOptionFDEngineBuilder : public AmericanOptionEngineBuilder {
public:
    AmericanOptionFDEngineBuilder(const string& model, const set<string>& tradeTypes, const AssetClass& assetClass,
                                  const Date& expiryDate)
        : AmericanOptionEngineBuilder(model, "FdBlackScholesVanillaEngine", tradeTypes, assetClass, expiryDate) {}

protected:
    virtual boost::shared_ptr<PricingEngine> engineImpl(const string& assetName, const Currency& ccy,
                                                        const AssetClass& assetClass, const Date& expiryDate) override {
        // We follow the way FdBlackScholesBarrierEngine determines maturity for time grid generation
        Handle<YieldTermStructure> riskFreeRate =
            market_->discountCurve(ccy.code(), configuration(ore::data::MarketContext::pricing));
        Time expiry = riskFreeRate->dayCounter().yearFraction(riskFreeRate->referenceDate(), expiryDate);
        QL_REQUIRE(expiry > 0.0, "FdBlackScholesVanillaEngine expects a positive time to expiry but got "
                                     << expiry << " for an expiry date " << io::iso_date(expiryDate) << ".");

        FdmSchemeDesc scheme = parseFdmSchemeDesc(engineParameter("Scheme"));
        Size tGrid = (Size)(ore::data::parseInteger(engineParameter("TimeGridPerYear")) * expiry);
        Size xGrid = ore::data::parseInteger(engineParameter("XGrid"));
        Size dampingSteps = ore::data::parseInteger(engineParameter("DampingSteps"));
        bool monotoneVar = ore::data::parseBool(engineParameter("EnforceMonotoneVariance", "", false, "true"));

        boost::shared_ptr<GeneralizedBlackScholesProcess> gbsp;

        if (monotoneVar) {
            // Replicate the construction of time grid in FiniteDifferenceModel::rollbackImpl
            // This time grid is required to build a BlackMonotoneVarVolTermStructure which
            // ensures monotonic variance along the time grid
            const Size totalSteps = tGrid + dampingSteps;
            std::vector<Time> timePoints(totalSteps + 1);
            Array timePointsArray(totalSteps, expiry, -expiry / totalSteps);
            timePoints[0] = 0.0;
            for (Size i = 0; i < totalSteps; i++)
                timePoints[timePoints.size() - i - 1] = timePointsArray[i];
            timePoints.insert(std::upper_bound(timePoints.begin(), timePoints.end(), 0.99 / 365), 0.99 / 365);
            gbsp = getBlackScholesProcess(assetName, ccy, assetClass, timePoints);
        } else {
            gbsp = getBlackScholesProcess(assetName, ccy, assetClass);
        }
        return boost::make_shared<FdBlackScholesVanillaEngine>(gbsp, tGrid, xGrid, dampingSteps, scheme);
    }
};

//! Abstract Engine Builder for American Vanilla Options using Barone Adesi Whaley Approximation
/*! Pricing engines are cached by asset/currency

    \ingroup builders
 */
class AmericanOptionBAWEngineBuilder : public AmericanOptionEngineBuilder {
public:
    AmericanOptionBAWEngineBuilder(const string& model, const set<string>& tradeTypes, const AssetClass& assetClass)
        : AmericanOptionEngineBuilder(model, "BaroneAdesiWhaleyApproximationEngine", tradeTypes, assetClass, Date()) {}

protected:
    virtual boost::shared_ptr<PricingEngine> engineImpl(const string& assetName, const Currency& ccy,
                                                        const AssetClass& assetClass, const Date& expiryDate) override {
        boost::shared_ptr<GeneralizedBlackScholesProcess> gbsp = getBlackScholesProcess(assetName, ccy, assetClass);
        return boost::make_shared<QuantExt::BaroneAdesiWhaleyApproximationEngine>(gbsp);
    }
};

} // namespace data
} // namespace ore


/******************************************************************************
Copyright (c) 2017, Farbod Farshidian. All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

 * Redistributions of source code must retain the above copyright notice, this
  list of conditions and the following disclaimer.

 * Redistributions in binary form must reproduce the above copyright notice,
  this list of conditions and the following disclaimer in the documentation
  and/or other materials provided with the distribution.

 * Neither the name of the copyright holder nor the names of its
  contributors may be used to endorse or promote products derived from
  this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 ******************************************************************************/

//
// Created by rgrandia on 26.02.20.
//

#include "ocs2_qp_solver/QpSolver.h"

#include <Eigen/LU>
#include <numeric>
#include <tuple>

namespace ocs2 {
namespace qp_solver {

/**
 * Extracts the problem state and inputs dimensions as well as number of constraints from a linear quadratic approximation
 * Looks at the size of the flowmap derivatives of the dynamics.
 * @return { numStatesPerStage, numInputsPerStage, numConstraintsPerStage}
 */
static std::tuple<std::vector<int>, std::vector<int>, std::vector<int>> getNumStatesInputsConstraints(
    const std::vector<LinearQuadraticStage>& linearQuadraticApproximation) {
  if (linearQuadraticApproximation.empty()) {
    return {std::vector<int>(0), std::vector<int>(0), std::vector<int>(0)};
  }

  const int N = linearQuadraticApproximation.size() - 1;
  std::vector<int> numStates;
  std::vector<int> numInputs;
  std::vector<int> numConstraints;
  numStates.reserve(N + 1);
  numInputs.reserve(N);
  numConstraints.reserve(N + 1);

  for (int k = 0; k < N; ++k) {
    numStates.push_back(linearQuadraticApproximation[k].dynamics.dfdx.cols());
    numInputs.push_back(linearQuadraticApproximation[k].dynamics.dfdu.cols());
    numConstraints.push_back(linearQuadraticApproximation[k].constraints.f.size());
  }
  numStates.push_back(linearQuadraticApproximation[N - 1].dynamics.dfdx.rows());
  numConstraints.push_back(linearQuadraticApproximation[N].constraints.f.size());

  return {numStates, numInputs, numConstraints};
}

/** Counts the number of decision variables in the QP */
static int getNumDecisionVariables(const std::vector<int>& numStates, const std::vector<int>& numInputs) {
  const auto totalNumberOfStates = std::accumulate(numStates.begin(), numStates.end(), 0);
  const auto totalNumberOfInputs = std::accumulate(numInputs.begin(), numInputs.end(), 0);
  return totalNumberOfStates + totalNumberOfInputs;
}

/** Counts the number of constraints in the QP */
static int getNumConstraints(const std::vector<int>& numStates, const std::vector<int>& numConstraints) {
  // Each stage constrains x_{k+1} states, adding the x_0 constraint, all states are constrained exactly once.
  return std::accumulate(numStates.begin(), numStates.end(), 0) + std::accumulate(numConstraints.begin(), numConstraints.end(), 0);
}

std::pair<vector_array_t, vector_array_t> solveLinearQuadraticProblem(const std::vector<LinearQuadraticStage>& lqApproximation,
                                                                      const vector_t& dx0) {
  // Extract sizes
  std::vector<int> numStates;
  std::vector<int> numInputs;
  std::vector<int> numConstraints;
  std::tie(numStates, numInputs, numConstraints) = getNumStatesInputsConstraints(lqApproximation);
  const auto numDecisionVariables = getNumDecisionVariables(numStates, numInputs);
  const auto numQpConstraints = getNumConstraints(numStates, numConstraints);

  // Construct QP
  const auto qpConstraints = getConstraintMatrices(lqApproximation, dx0, numQpConstraints, numDecisionVariables);
  const auto qpCosts = getCostMatrices(lqApproximation, numDecisionVariables);

  // Solve
  const auto primalDualSolution = solveDenseQp(qpCosts, qpConstraints);

  // Extract solution
  return getStateAndInputTrajectory(numStates, numInputs, primalDualSolution.first);
}

VectorFunctionLinearApproximation getConstraintMatrices(const std::vector<LinearQuadraticStage>& lqp, const vector_t& dx0,
                                                        int numConstraints, int numDecisionVariables) {
  if (lqp.empty()) {
    return VectorFunctionLinearApproximation();
  }

  const int N = lqp.size() - 1;

  // Preallocate full constraint matrix
  VectorFunctionLinearApproximation constraints;
  auto& A = constraints.dfdx;
  auto& b = constraints.f;
  A.setZero(numConstraints, numDecisionVariables);
  b.setZero(numConstraints);

  // Initial state constraint
  const int nx_0 = dx0.size();
  A.topLeftCorner(nx_0, nx_0) = -matrix_t::Identity(nx_0, nx_0);
  b.topRows(nx_0) = dx0;

  int currRow = nx_0;
  int currCol = 0;
  for (int k = 0; k < N; ++k) {
    const auto& dynamics_k = lqp[k].dynamics;
    const auto& constraints_k = lqp[k].constraints;
    const int nu_k = dynamics_k.dfdu.cols();
    const int nx_k = dynamics_k.dfdx.cols();
    const int nx_Next = dynamics_k.dfdx.rows();
    const int nc_k = constraints_k.f.size();

    if (nc_k > 0) {
      // Add [C, D, 0]
      A.block(currRow, currCol, nc_k, nx_k + nu_k) << constraints_k.dfdx, constraints_k.dfdu;
      // Add [e]
      b.segment(currRow, nc_k) = constraints_k.f;

      currRow += nc_k;
    }

    // Add [A, B, -I]
    A.block(currRow, currCol, nx_Next, nx_k + nu_k + nx_Next) << dynamics_k.dfdx, dynamics_k.dfdu, -matrix_t::Identity(nx_Next, nx_Next);
    // Add [b]
    b.segment(currRow, nx_Next) = dynamics_k.f;

    currRow += nx_Next;
    currCol += nx_k + nu_k;
  }

  // Final state constraint
  const auto& constraints_N = lqp[N].constraints;
  const int nc_N = constraints_N.f.size();
  if (nc_N > 0) {
    A.bottomRightCorner(nc_N, constraints_N.dfdx.cols()) = constraints_N.dfdx;
    b.bottomRows(nc_N) = constraints_N.f;
  }

  return constraints;
}

ScalarFunctionQuadraticApproximation getCostMatrices(const std::vector<LinearQuadraticStage>& lqp, int numDecisionVariables) {
  if (lqp.empty()) {
    return ScalarFunctionQuadraticApproximation();
  }

  const int N = lqp.size() - 1;

  // Preallocate full Cost matrices
  ScalarFunctionQuadraticApproximation qpCost;
  auto& H = qpCost.dfdxx;
  auto& g = qpCost.dfdx;
  auto& c = qpCost.f;
  H.setZero(numDecisionVariables, numDecisionVariables);
  g.setZero(numDecisionVariables);
  c = 0.0;

  int currRow = 0;
  for (int k = 0; k < N; ++k) {
    const auto& cost_k = lqp[k].cost;
    const int nx_k = cost_k.dfdux.cols();
    const int nu_k = cost_k.dfdux.rows();

    // Add [ Q, P'
    //       P, R ]
    H.block(currRow, currRow, nx_k + nu_k, nx_k + nu_k) << cost_k.dfdxx, cost_k.dfdux.transpose(), cost_k.dfdux, cost_k.dfduu;
    // Add [ q, r]
    g.segment(currRow, nx_k + nu_k) << cost_k.dfdx, cost_k.dfdu;
    // Add nominal cost
    c += cost_k.f;

    currRow += nx_k + nu_k;
  }

  // Terminal cost
  const auto& cost_N = lqp[N].cost;
  const int nx_N = cost_N.dfdx.size();
  H.block(currRow, currRow, nx_N, nx_N) << cost_N.dfdxx;
  g.segment(currRow, nx_N) << cost_N.dfdx;
  c += cost_N.f;

  return qpCost;
}

std::pair<ScalarFunctionQuadraticApproximation, VectorFunctionLinearApproximation> getDenseQp(const std::vector<LinearQuadraticStage>& lqp,
                                                                                const vector_t& dx0) {
  // Extract sizes
  std::vector<int> numStates;
  std::vector<int> numInputs;
  std::vector<int> numConstraints;
  std::tie(numStates, numInputs, numConstraints) = getNumStatesInputsConstraints(lqp);
  const auto numDecisionVariables = getNumDecisionVariables(numStates, numInputs);
  const auto numQpConstraints = getNumConstraints(numStates, numConstraints);

  // Construct QP
  const auto qpCosts = getCostMatrices(lqp, numDecisionVariables);
  const auto qpConstraints = getConstraintMatrices(lqp, dx0, numQpConstraints, numDecisionVariables);

  return {qpCosts, qpConstraints};
}

std::pair<vector_t, vector_t> solveDenseQp(const ScalarFunctionQuadraticApproximation& cost,
                                           const VectorFunctionLinearApproximation& constraints) {
  const int m = constraints.dfdx.rows();
  const int n = constraints.dfdx.cols();

  // Assemble KKT condition
  matrix_t kktMatrix(n + m, n + m);
  vector_t kktRhs(n + m);
  kktMatrix << cost.dfdxx, constraints.dfdx.transpose(), constraints.dfdx, matrix_t::Zero(m, m);
  kktRhs << -cost.dfdx, -constraints.f;

  // prerequisite for the LU factorization, and the solution would be non-unique.
  if (kktMatrix.fullPivLu().rank() != n + m) {
    throw std::runtime_error("KKT matrix is not full rank");
  }
  vector_t sol = kktMatrix.lu().solve(kktRhs);
  return {sol.head(n), sol.tail(m)};
}

std::pair<vector_array_t, vector_array_t> getStateAndInputTrajectory(const std::vector<int>& numStates, const std::vector<int>& numInputs,
                                                                     const vector_t& w) {
  assert(numStates.size() == numInputs.size() + 1);

  const int N = numInputs.size();

  vector_array_t stateTrajectory;
  vector_array_t inputTrajectory;
  stateTrajectory.reserve(N + 1);
  inputTrajectory.reserve(N);

  int index = 0;
  for (int k = 0; k < N; ++k) {
    // x[k]
    stateTrajectory.emplace_back(w.segment(index, numStates[k]));
    index += numStates[k];

    // u[k]
    inputTrajectory.emplace_back(w.segment(index, numInputs[k]));
    index += numInputs[k];
  }

  // x[N]
  stateTrajectory.emplace_back(w.segment(index, numStates[N]));

  return {stateTrajectory, inputTrajectory};
}

}  // namespace qp_solver
}  // namespace ocs2


#ifndef OSRM_CELLS_CUSTOMIZER_HPP
#define OSRM_CELLS_CUSTOMIZER_HPP

#include "partition/cell_storage.hpp"
#include "partition/multi_level_partition.hpp"
#include "util/binary_heap.hpp"

#include <boost/thread/tss.hpp>
#include <unordered_set>

namespace osrm
{
namespace customizer
{

class CellCustomizer
{

  public:
    CellCustomizer(const partition::MultiLevelPartition &partition) : partition(partition) {}

    template <typename GraphT>
    void Customize(const GraphT &graph, partition::CellStorage &cells, LevelID level, CellID id)
    {
        auto cell = cells.GetCell(level, id);
        auto destinations = cell.GetDestinationNodes();

        // for each source do forward search
        for (auto source : cell.GetSourceNodes())
        {
            std::unordered_set<NodeID> destinations_set(destinations.begin(), destinations.end());
            Heap heap(graph.GetNumberOfNodes());
            heap.Insert(source, 0, {});

            // explore search space
            while (!heap.Empty() && !destinations_set.empty())
            {
                const NodeID node = heap.DeleteMin();
                const EdgeWeight weight = heap.GetKey(node);

                if (level == 1)
                    RelaxNode<true>(graph, cells, heap, level, node, weight);
                else
                    RelaxNode<false>(graph, cells, heap, level, node, weight);

                destinations_set.erase(node);
            }

            // fill a map of destination nodes to placeholder pointers
            auto destination_iter = destinations.begin();
            for (auto &weight : cell.GetOutWeight(source))
            {
                BOOST_ASSERT(destination_iter != destinations.end());
                const auto destination = *destination_iter++;
                weight =
                    heap.WasInserted(destination) ? heap.GetKey(destination) : INVALID_EDGE_WEIGHT;
            }
        }
    }

    template <typename GraphT> void Customize(const GraphT &graph, partition::CellStorage &cells)
    {
        for (std::size_t level = 1; level < partition.GetNumberOfLevels(); ++level)
        {
            tbb::parallel_for(tbb::blocked_range<std::size_t>(0, partition.GetNumberOfCells(level)),
                              [&](const tbb::blocked_range<std::size_t> &range) {
                                  for (auto id = range.begin(), end = range.end(); id != end; ++id)
                                  {
                                      Customize(graph, cells, level, id);
                                  }
                              });
        }
    }

  private:
    struct HeapData
    {
    };
    using Heap = util::
        BinaryHeap<NodeID, NodeID, EdgeWeight, HeapData, util::UnorderedMapStorage<NodeID, int>>;
    using HeapPtr = boost::thread_specific_ptr<Heap>;

    template <bool first_level, typename GraphT>
    void RelaxNode(const GraphT &graph,
                   const partition::CellStorage &cells,
                   Heap &heap,
                   LevelID level,
                   NodeID node,
                   EdgeWeight weight) const
    {
        if (!first_level)
        {
            // Relax sub-cell nodes
            auto subcell_id = partition.GetCell(level - 1, node);
            auto subcell = cells.GetCell(level - 1, subcell_id);
            auto subcell_destination = subcell.GetDestinationNodes().begin();
            for (auto subcell_weight : subcell.GetOutWeight(node))
            {
                if (subcell_weight != INVALID_EDGE_WEIGHT)
                {
                    const NodeID to = *subcell_destination;
                    const EdgeWeight to_weight = subcell_weight + weight;
                    if (!heap.WasInserted(to))
                    {
                        heap.Insert(to, to_weight, {});
                    }
                    else if (to_weight < heap.GetKey(to))
                    {
                        heap.DecreaseKey(to, to_weight);
                    }
                }

                ++subcell_destination;
            }
        }

        // Relax base graph edges if a sub-cell border edge
        for (auto edge : graph.GetInternalEdgeRange(level, node))
        {
            const NodeID to = graph.GetTarget(edge);
            const auto &data = graph.GetEdgeData(edge);
            if (data.forward &&
                (first_level ||
                 partition.GetCell(level - 1, node) != partition.GetCell(level - 1, to)))
            {
                const EdgeWeight to_weight = data.weight + weight;
                if (!heap.WasInserted(to))
                {
                    heap.Insert(to, to_weight, {});
                }
                else if (to_weight < heap.GetKey(to))
                {
                    heap.DecreaseKey(to, to_weight);
                }
            }
        }
    }

    const partition::MultiLevelPartition &partition;
};
}
}

#endif // OSRM_CELLS_CUSTOMIZER_HPP


/*    Copyright (c) 2010-2018, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    References
 *      Klothakis, A. and Nikolos, I., “Modeling of Rarefied Hypersonic Flows Using the Massively
 *        Parallel DSMC Kernel “SPARTA”,” in 8th GRACM International Congress on Computational Mechanics,
 *        Volos, Greece, July 2015.
 *      Dirkx, D. and Mooij, E., Conceptual Shape Optimization of Entry Vehicles. Springer, 2017.
 */

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

#include <Eigen/Geometry>

#include "Tudat/InputOutput/basicInputOutput.h"
#include "Tudat/InputOutput/matrixTextFileReader.h"
#include "Tudat/InputOutput/spartaDataReader.h"
#include "Tudat/InputOutput/spartaInputOutput.h"

#include "Tudat/Astrodynamics/Aerodynamics/rarefiedFlowAnalysis.h"

namespace tudat
{

namespace aerodynamics
{

using namespace unit_conversions;
using namespace input_output;

//! Returns default values of altitude for use in RarefiedFlowAnalysis.
std::vector< double > getDefaultRarefiedFlowAltitudePoints(
        const std::string& targetPlanet )
{
    std::vector< double > altitudePoints;

    // Set default points for Earth.
    if ( targetPlanet == "Earth" )
    {
        altitudePoints.resize( 5 );
        altitudePoints[ 0 ] = 225.0e3;
        altitudePoints[ 1 ] = 250.0e3;
        altitudePoints[ 2 ] = 300.0e3;
        altitudePoints[ 3 ] = 400.0e3;
        altitudePoints[ 4 ] = 600.0;
    }
    // Set default points for Mars.
    else if ( targetPlanet == "Mars" )
    {
        altitudePoints.resize( 5 );
        altitudePoints[ 0 ] = 125.0e3;
        altitudePoints[ 1 ] = 150.0e3;
        altitudePoints[ 2 ] = 200.0e3;
        altitudePoints[ 3 ] = 300.0e3;
        altitudePoints[ 4 ] = 500.0e3;
    }
    // Give error otherwise.
    else
    {
        throw std::runtime_error( "Error in altitude range selection for SPARTA simulation. "
                                  "Planet not supported." );
    }
    return altitudePoints;
}

//! Returns default values of Mach number for use in RarefiedFlowAnalysis.
std::vector< double > getDefaultRarefiedFlowMachPoints(
        const std::string& machRegime )
{
    std::vector< double > machPoints;

    // Set default points for full hypersonic analysis.
    if ( machRegime == "Full" )
    {
        machPoints.resize( 6 );

        machPoints[ 0 ] = 3.0;
        machPoints[ 1 ] = 4.0;
        machPoints[ 2 ] = 5.0;
        machPoints[ 3 ] = 8.0;
        machPoints[ 4 ] = 10.0;
        machPoints[ 5 ] = 20.0;
    }
    // Set default points for low hypersonic analysis.
    else if ( machRegime == "Low" )
    {
        machPoints.resize( 5 );
        machPoints[ 0 ] = 3.0;
        machPoints[ 1 ] = 4.0;
        machPoints[ 2 ] = 5.0;
        machPoints[ 3 ] = 8.0;
        machPoints[ 4 ] = 10.0;
    }
    // Set default points for high hypersonic analysis.
    else if ( machRegime == "High" )
    {
        machPoints.resize( 4 );
        machPoints[ 0 ] = 5.0;
        machPoints[ 1 ] = 8.0;
        machPoints[ 2 ] = 10.0;
        machPoints[ 3 ] = 20.0;
    }
    return machPoints;
}

//! Returns default values of angle of attack for use in RarefiedFlowAnalysis.
std::vector< double > getDefaultRarefiedFlowAngleOfAttackPoints(
        const std::string& angleOfAttackRegime )
{
    std::vector< double > angleOfAttackPoints;

    // Set default angles of attack
    double a = - 35;
    while ( a <= 35 )
    {
        angleOfAttackPoints.push_back( convertDegreesToRadians( a ) );
        a += 5;
    }

    // Add extra points if required
    if ( angleOfAttackRegime == "Full" )
    {
        std::vector< double > frontExtension = { convertDegreesToRadians( -85.0 ),
                                                 convertDegreesToRadians( -70.0 ),
                                                 convertDegreesToRadians( -55.0 ),
                                                 convertDegreesToRadians( -40.0 ) };
        std::vector< double > rearExtension = { convertDegreesToRadians( 40.0 ),
                                                convertDegreesToRadians( 55.0 ),
                                                convertDegreesToRadians( 70.0 ),
                                                convertDegreesToRadians( 85.0 ) };
        angleOfAttackPoints.insert( angleOfAttackPoints.begin( ), frontExtension.begin( ), frontExtension.end( ) );
        angleOfAttackPoints.insert( angleOfAttackPoints.end( ), rearExtension.begin( ), rearExtension.end( ) );
    }
    return angleOfAttackPoints;
}

//! Function to sort the rows of a matrix, based on the specified column and specified order.
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > sortMatrixRows(
        const Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor >& matrixToBeSorted,
        const int referenceColumn, const bool descendingOrder )
{
    // Declare eventual output vector
    Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > sortedMatrix;
    sortedMatrix.resizeLike( matrixToBeSorted );

    // Loop over rows and assign to new matrix
    Eigen::Matrix< double, 1, Eigen::Dynamic, Eigen::RowMajor > currentRow;
    for ( unsigned int i = 0; i < matrixToBeSorted.rows( ); i++ )
    {
        // Retrieve each row in 'scrambled order' and assign it to the new matrix
        currentRow = matrixToBeSorted.row( i );

        // Based on requested order
        if ( descendingOrder )
        {
            sortedMatrix.row( matrixToBeSorted.rows( ) - currentRow[ referenceColumn ] ) = currentRow;
            // no (- 1)'s are needed since both are defined starting from 1
        }
        else
        {
            sortedMatrix.row( currentRow[ referenceColumn ] - 1 ) = currentRow;
        }
    }

    // Give output
    return sortedMatrix;
}

//! Default constructor.
RarefiedFlowAnalysis::RarefiedFlowAnalysis(
        const std::string& SPARTAExecutable,
        const std::vector< std::vector< double > >& dataPointsOfIndependentVariables,
        boost::shared_ptr< TabulatedAtmosphere > atmosphereModel,
        const std::string& simulationGases,
        const std::string& geometryFileUser,
        const double referenceArea,
        const double referenceLength,
        const int referenceAxis,
        const Eigen::Vector3d& momentReferencePoint,
        const double gridSpacing,
        const double simulatedParticlesPerCell,
        const double wallTemperature,
        const double accommodationCoefficient,
        const bool printProgressInCommandWindow,
        const std::string MPIExecutable,
        const unsigned int numberOfCores ) :
    AerodynamicCoefficientGenerator< 3, 6 >(
          dataPointsOfIndependentVariables, referenceLength, referenceArea, referenceLength,
          momentReferencePoint,
          boost::assign::list_of( altitude_dependent )( mach_number_dependent )( angle_of_attack_dependent ),
          true, true ),
      SPARTAExecutable_( SPARTAExecutable ),simulationGases_( simulationGases ), referenceAxis_( referenceAxis ),
      gridSpacing_( gridSpacing ), simulatedParticlesPerCell_( simulatedParticlesPerCell ),
      wallTemperature_( wallTemperature ), accommodationCoefficient_( accommodationCoefficient ),
      printProgressInCommandWindow_( printProgressInCommandWindow ), MPIExecutable_( MPIExecutable ),
      numberOfCores_( numberOfCores ), referenceDimension_( static_cast< unsigned int >( referenceAxis_ ) )
{
    // Check inputs
    if ( referenceDimension_ > 2 )
    {
        throw std::runtime_error( "Error in SPARTA rarefied flow analysis. Reference axis makes "
                                  "no sense for a universe with 3 spacial dimensions (i.e., our universe). "
                                  "Note that the first dimension is identified with 0." );
    }

    // Analyze vehicle geometry
    analyzeGeometryFile( geometryFileUser );

    // Find atmospheric conditions based on altitude
    for ( unsigned int h = 0; h < dataPointsOfIndependentVariables_.at( 0 ).size( ); h++ )
    {
        atmosphericConditions_[ density_index ].push_back(
                    atmosphereModel->getDensity( dataPointsOfIndependentVariables_.at( 0 ).at( h ) ) );
        atmosphericConditions_[ pressure_index ].push_back(
                    atmosphereModel->getPressure( dataPointsOfIndependentVariables_.at( 0 ).at( h ) ) );
        atmosphericConditions_[ temperature_index ].push_back(
                    atmosphereModel->getTemperature( dataPointsOfIndependentVariables_.at( 0 ).at( h ) ) );
        atmosphericConditions_[ speed_of_sound_index ].push_back(
                    atmosphereModel->getSpeedOfSound( dataPointsOfIndependentVariables_.at( 0 ).at( h ) ) );
        atmosphericConditions_[ number_density_index ].push_back(
                    tudat::physical_constants::AVOGADRO_CONSTANT * atmosphericConditions_[ density_index ].at( h ) /
                    atmosphereModel->getMolarMass( dataPointsOfIndependentVariables_.at( 0 ).at( h ) ) );
    }

    // Get simulation conditions
    getSimulationConditions( );

    // Read SPARTA input template
    inputTemplate_ = readSpartaInputFileTemplate( );

    // Copy input shape file to default name
    std::string commandString = "cp " + geometryFileUser + " " + getSpartaInternalGeometryFile( );
    std::system( commandString.c_str( ) );

    // Run SPARTA simulation
    generateCoefficients( );

    // Create interpolator object
    createInterpolator( );
}

//! Open and read geometry file for SPARTA simulation.
void RarefiedFlowAnalysis::analyzeGeometryFile( const std::string& geometryFileUser )
{
    // Extract information on vehicle geometry
    std::pair< Eigen::Matrix< double, Eigen::Dynamic, 3 >, Eigen::Matrix< int, Eigen::Dynamic, 3 > >
            geometryData = readSpartaGeometryFile( geometryFileUser );
    shapePoints_ = geometryData.first;
    shapeTriangles_ = geometryData.second;
    numberOfPoints_ = shapePoints_.rows( );
    numberOfTriangles_ = shapeTriangles_.rows( );

    // Get maximum and minimum values in each dimension
    maximumDimensions_ = shapePoints_.colwise( ).maxCoeff( );
    minimumDimensions_ = shapePoints_.colwise( ).minCoeff( );

    // Compute normal to surface elements, area of surface elements and moment arm values
    Eigen::Matrix3d currentVertices;
    Eigen::Vector3d currentNormal;
    Eigen::Vector3d currentCentroid;
    double currentNormalNorm;
    elementSurfaceNormal_.resize( 3, numberOfTriangles_ );
    elementSurfaceArea_.resize( 1, numberOfTriangles_ );
    elementMomentArm_.resize( 3, numberOfTriangles_ );
    for ( int i = 0; i < numberOfTriangles_; i++ )
    {
        // Compute properties of current surface element
        for ( unsigned int j = 0; j < 3; j++ )
        {
            currentVertices.row( j ) = shapePoints_.row( shapeTriangles_( i, j ) - 1 );
        }
        currentNormal = ( currentVertices.row( 1 ) - currentVertices.row( 0 ) ).cross(
                    currentVertices.row( 2 ) - currentVertices.row( 0 ) );
        currentNormalNorm = currentNormal.norm( );
        currentCentroid = currentVertices.colwise( ).sum( ) / 3.0;

        // Find normal, area and distance to reference point
        elementSurfaceNormal_.col( i ) = currentNormal / currentNormalNorm;
        elementSurfaceArea_( i ) = 0.5 * currentNormalNorm;
        elementMomentArm_.col( i ) = currentCentroid - momentReferencePoint_;
    }

    // Compute cross-sectional area
    for ( unsigned int i = 0; i < 3; i++ )
    {
        shapeCrossSectionalArea_( i ) =
                0.5 * elementSurfaceNormal_.row( i ).cwiseAbs( ).dot( elementSurfaceArea_ );
    }

    // Check consistency with input dimensions
    const double tolerance = 1e-5;
    if ( std::fabs( shapeCrossSectionalArea_( referenceDimension_ ) - referenceArea_ ) > tolerance )
    {
        throw std::runtime_error( "Error in SPARTA geometry file. Input reference area does not match the "
                                  "combination of reference axis and geometry. Note that the first dimension "
                                  "is identified with 0. Tolerance set to: " + std::to_string( tolerance ) );
    }
}

//! Retrieve simulation conditions based on input and geometry.
void RarefiedFlowAnalysis::getSimulationConditions( )
{
    // Simulation boundary and grid
    for ( unsigned int i = 0; i < 3; i++ )
    {
        simulationBoundaries_( 2 * i ) = minimumDimensions_( i ) + 0.5 * minimumDimensions_.minCoeff( ); // add extra space around shape
        simulationBoundaries_( 2 * i + 1 ) = maximumDimensions_( i ) + 0.5 * maximumDimensions_.maxCoeff( ); // add extra space around shape
        if ( i == referenceDimension_ )
        {
            simulationBoundaries_( 2 * i ) -= 1.0; // add extra space along axis of velocity
            simulationBoundaries_( 2 * i + 1 ) += 1.0; // add extra space along axis of velocity
        }
        simulationGrid_( i ) = simulationBoundaries_( 2 * i + 1 ) - simulationBoundaries_( 2 * i );
    }
    simulationGrid_ /= gridSpacing_;

    // Convert molecular speed ratio to stream velocity and compute simulation time step and ratio of real to simulated variables
    freeStreamVelocities_.resize( dataPointsOfIndependentVariables_.at( 0 ).size( ), dataPointsOfIndependentVariables_.at( 1 ).size( ) );
    simulationTimeStep_.resize( dataPointsOfIndependentVariables_.at( 0 ).size( ), dataPointsOfIndependentVariables_.at( 1 ).size( ) );
    ratioOfRealToSimulatedParticles_.resize( dataPointsOfIndependentVariables_.at( 0 ).size( ), 1 );
    double simulationBoxLengthAlongReferenceAxis = ( simulationBoundaries_( 2 * referenceDimension_ + 1 ) -
                                                     simulationBoundaries_( 2 * referenceDimension_ ) );
    for ( unsigned int h = 0; h < dataPointsOfIndependentVariables_.at( 0 ).size( ); h++ )
    {
        for ( unsigned int m = 0; m < dataPointsOfIndependentVariables_.at( 1 ).size( ); m++ )
        {
            freeStreamVelocities_( h, m ) = dataPointsOfIndependentVariables_.at( 1 ).at( m ) *
                    atmosphericConditions_[ speed_of_sound_index ].at( h );
            simulationTimeStep_( h, m ) = 0.1 * simulationBoxLengthAlongReferenceAxis /
                    freeStreamVelocities_( h, m );
            // time step is taken as time it takes for a particle to travel for 10 % of the box
        }
        ratioOfRealToSimulatedParticles_( h ) = atmosphericConditions_[ number_density_index ].at( h ) *
                std::pow( gridSpacing_, 3 ) / simulatedParticlesPerCell_;
    }
}

//! Generate aerodynamic database.
void RarefiedFlowAnalysis::generateCoefficients( )
{
    // Inform user on progress
    std::cout << "Initiating SPARTA simulation. This may take a while." << std::endl;

    // Generate command string for SPARTA
    std::string runSPARTACommandString = "cd " + getSpartaDataPath( ) + "; ";
    if ( MPIExecutable_ != "" )
    {
        if ( numberOfCores_ < 1 )
        {
            throw std::runtime_error( "Error in SPARTA rarefied flow analysis. Number of cores needs to be "
                                      "an integer value larger or equal to one." );
        }
        runSPARTACommandString = runSPARTACommandString + MPIExecutable_ +
                " -np " + std::to_string( numberOfCores_ ) + " ";
    }
    runSPARTACommandString = runSPARTACommandString + SPARTAExecutable_ + " -echo log ";
    if ( !printProgressInCommandWindow_ )
    {
        runSPARTACommandString = runSPARTACommandString + "-screen none ";
    }
    runSPARTACommandString = runSPARTACommandString + "-in " + getSpartaInputFile( );

    // Predefine variables
    Eigen::Vector3d velocityVector;
    std::string temporaryOutputFile = getSpartaOutputPath( ) + "/coeff";
    std::vector< std::string > outputFileExtensions = { ".1000" };//{ ".400", ".600", ".800", ".1000" };
    Eigen::Matrix< double, Eigen::Dynamic, 7, Eigen::RowMajor > outputMatrix;
    Eigen::Matrix< double, 3, Eigen::Dynamic > meanPressureValues;
    Eigen::Matrix< double, 3, Eigen::Dynamic > meanShearValues;

    // Loop over simulation parameters and run SPARTA
    int systemStatus;
    // Loop over altitude
    for ( unsigned int h = 0; h < dataPointsOfIndependentVariables_.at( 0 ).size( ); h++ )
    {
        // Inform user on progress
        std::cout << std::endl << "Altitude: "
                  << dataPointsOfIndependentVariables_.at( 0 ).at( h ) / 1e3
                  << " km" << std::endl;

        // Loop over Mach numbers
        for ( unsigned int m = 0; m < dataPointsOfIndependentVariables_.at( 1 ).size( ); m++ )
        {
            // Inform user on progress
            std::cout << "Mach number: "
                      << dataPointsOfIndependentVariables_.at( 1 ).at( m )
                      << std::endl;

            // Loop over angles of attack
            for ( unsigned int a = 0; a < dataPointsOfIndependentVariables_.at( 2 ).size( ); a++ )
            {
                // Inform user on progress
                std::cout << "Angle of attack: "
                          << convertRadiansToDegrees( dataPointsOfIndependentVariables_.at( 2 ).at( a ) )
                          << " deg" << std::endl;

                // Get velocity vector
                velocityVector = Eigen::Vector3d::Zero( );
                velocityVector( referenceDimension_ ) = ( ( referenceAxis_ >= 0 ) ? - 1.0 : 1.0 ) *
                        freeStreamVelocities_( h, m );

                // Print to file
                FILE * fileIdentifier = std::fopen( getSpartaInputFile( ).c_str( ), "w" );
                std::fprintf( fileIdentifier, inputTemplate_.c_str( ),
                              simulationBoundaries_( 0 ), simulationBoundaries_( 1 ), simulationBoundaries_( 2 ),
                              simulationBoundaries_( 3 ), simulationBoundaries_( 4 ), simulationBoundaries_( 5 ),
                              simulationGrid_( 0 ), simulationGrid_( 1 ), simulationGrid_( 2 ),
                              atmosphericConditions_[ number_density_index ].at( h ), ratioOfRealToSimulatedParticles_( h ),
                              simulationGases_.c_str( ),
                              simulationGases_.c_str( ), velocityVector( 0 ), velocityVector( 1 ), velocityVector( 2 ),
                              simulationGases_.c_str( ), atmosphericConditions_[ temperature_index ].at( h ),
                              convertRadiansToDegrees( dataPointsOfIndependentVariables_.at( 2 ).at( a ) ),
                              wallTemperature_, accommodationCoefficient_,
                              simulationTimeStep_( h, m ),
                              getSpartaOutputDirectory( ).c_str( ) );
                std::fclose( fileIdentifier );

                // Run SPARTA
                systemStatus = std::system( runSPARTACommandString.c_str( ) );
                if ( systemStatus != 0 )
                {
                    throw std::runtime_error( "Error in SPARTA rarefied flow analysis. "
                                              "SPARTA Simulation failed. See the log.sparta file in "
                                              "Tudat/External/SPARTA/ for more details." );
                }

                // Loop over angles of attack
                meanPressureValues.resize( 3, numberOfTriangles_ );
                meanShearValues.resize( 3, numberOfTriangles_ );

                // Read output files and compute mean pressure and shear force values
                meanPressureValues.setZero( );
                meanShearValues.setZero( );
                for ( unsigned int i = 0; i < outputFileExtensions.size( ); i++ )
                {
                    outputMatrix = readMatrixFromFile( temporaryOutputFile + outputFileExtensions.at( i ), "\t ;,", "%", 9 );
                    outputMatrix = sortMatrixRows( outputMatrix, 0 );
                    for ( unsigned int j = 0; j < 3; j++ )
                    {
                        meanPressureValues.row( j ) += outputMatrix.col( j + 1 ).transpose( );
                        meanShearValues.row( j ) += outputMatrix.col( j + 4 ).transpose( );
                    }
                }
                meanPressureValues /= outputFileExtensions.size( );
                meanShearValues /= outputFileExtensions.size( );

                // Convert pressure and shear forces to aerodynamic coefficients
                aerodynamicCoefficients_[ h ][ m ][ a ] = computeAerodynamicCoefficientsFromPressureShearForces(
                            meanPressureValues,
                            meanShearValues,
                            atmosphericConditions_[ density_index ].at( h ),
                            atmosphericConditions_[ pressure_index ].at( h ),
                            freeStreamVelocities_( h, m ),
                            elementSurfaceNormal_,
                            elementSurfaceArea_,
                            elementMomentArm_,
                            referenceArea_,
                            referenceLength_ );

                // Clean up results folder
                std::string commandString = "rm " + getSpartaOutputPath( ) + "/coeff.*";
                std::system( commandString.c_str( ) );
            }
        }
    }

    // Inform user on progress
    std::cout << std::endl << "SPARTA simulation complete." << std::endl << std::endl;
}

//! Get aerodynamic coefficients at specific conditions.
Eigen::Vector6d RarefiedFlowAnalysis::getAerodynamicCoefficientsDataPoint(
        const boost::array< int, 3 > independentVariables )
{
    // Return requested coefficients.
    return aerodynamicCoefficients_( independentVariables );
}

} // namespace aerodynamics

} // namespace tudat


//---------------------------------------------------------------------------//
//!
//! \file   Utility_QuantityTraits.hpp
//! \author Alex Robinson
//! \brief  Quantity traits class specializations
//!
//---------------------------------------------------------------------------//

#ifndef UTILITY_QUANTITY_TRAITS_HPP
#define UTILITY_QUANTITY_TRAITS_HPP

// Boost Includes
#include <boost/units/quantity.hpp>
#include <boost/units/cmath.hpp>
#include <boost/units/static_rational.hpp>
#include <boost/units/limits.hpp>
#include <boost/units/io.hpp>
#include <boost/utility/enable_if.hpp>
#include <boost/type_traits/is_arithmetic.hpp>
#include <boost/type_traits/is_floating_point.hpp>

// Trilinos Includes
#include <Teuchos_ScalarTraits.hpp>

// FRENSIE Includes
#include "Utility_QuantityTraitsDecl.hpp"

namespace Utility{

/*! \brief The QuantityTraitsHelper partial specialization for floating point
 * boost::units::quantity
 * \ingroup quantity_traits
 */
template<typename Unit, typename T>
struct QuantityTraitsHelper<boost::units::quantity<Unit,T>,typename boost::enable_if<boost::is_floating_point<T> >::type>
{
private:
  typedef boost::units::quantity<Unit,T> QuantityType;
  typedef T RawType;

public:
  static inline QuantityType inf()
  { return QuantityType::from_value( std::numeric_limits<RawType>::infinity() ); }

  static inline QuantityType nan()
  { return QuantityType::from_value( Teuchos::ScalarTraits<RawType>::nan() ); }

  static inline bool isnaninf( const QuantityType& a )
  { return Teuchos::ScalarTraits<RawType>::isnaninf( a.value() ); }
};

/*! \brief The partial specialization of QuantityTraits for arithmetic
 * boost::units::quantity
 * \ingroup quantity_traits
 */
template<typename Unit, typename T>
struct QuantityTraits<boost::units::quantity<Unit,T>, typename boost::enable_if<boost::is_arithmetic<T> >::type> : public QuantityTraitsHelper<boost::units::quantity<Unit,T> >
{
  typedef Unit UnitType;
  typedef T RawType;
  typedef boost::units::quantity<Unit,T> QuantityType;
  typedef boost::is_floating_point<T> is_floating_point;

  template<boost::units::integer_type N, boost::units::integer_type D = 1>
  struct GetQuantityToPowerType
  {
    typedef typename boost::units::power_typeof_helper<QuantityType,boost::units::static_rational<N,D> >::type type;
  };

  static inline QuantityType zero()
  { return QuantityType::from_value( RawType(0) ); }

  static inline QuantityType one()
  { return QuantityType::from_value( RawType(1) ); }

  static inline QuantityType conjugate( const QuantityType& a )
  { return QuantityType::from_value( Teuchos::ScalarTraits<RawType>::conjugate( a.value() ) ); }

  static inline QuantityType real( const QuantityType& a )
  { return QuantityType::from_value( Teuchos::ScalarTraits<RawType>::real( a.value() ) ); }

  static inline QuantityType imag( const QuantityType& a )
  { return QuantityType::from_value( Teuchos::ScalarTraits<RawType>::imag( a.value() ) ); }

  //! Possible bug in boost::units::sqrt
  static inline typename GetQuantityToPowerType<1,2>::type sqrt( const QuantityType& quantity )
  {
    return GetQuantityToPowerType<1,2>::type::from_value( std::sqrt( quantity.value() ) );
    // return boost::units::sqrt( quantity )
  }

  template<boost::units::integer_type N, boost::units::integer_type D>
  static inline typename GetQuantityToPowerType<N,D>::type rpow( const QuantityType& quantity )
  { return boost::units::pow<boost::units::static_rational<N,D> >( quantity ); }
  //! Potentially dangerous to initialize quantities in this way!
  static inline QuantityType initializeQuantity( const RawType& raw_quantity )
  { return QuantityType::from_value( raw_quantity ); }

  //! Get the raw value of a quantity
  static inline const RawType& getRawQuantity( const QuantityType& quantity )
  { return quantity.value(); }

  //! Set the value of a quantity (potentially dangerous!)
  static inline void setQuantity( QuantityType& quantity,
				  const RawType& raw_quantity )
  { quantity = QuantityType::from_value( raw_quantity ); }
};

/*! \brief The QuantityTraitsHelper partial specialization for floating point
  types (no units).
 * \ingroup quantity_traits
 */
template<typename T>
struct QuantityTraitsHelper<T,typename boost::enable_if<boost::is_floating_point<T> >::type>
{
private:
  typedef T QuantityType;

public:

  static inline QuantityType inf()
  { return std::numeric_limits<QuantityType>::infinity(); }

  static inline QuantityType nan()
  { return Teuchos::ScalarTraits<QuantityType>::nan(); }

  static inline bool isnaninf( const QuantityType& a )
  { return Teuchos::ScalarTraits<QuantityType>::isnaninf(a); }
};

/*! The specialization of QuantityTraits for all arithmetic types (no units).
 *
 * Note that having no units is different from a dimensionless unit.
 * \ingroup quantity_traits
 */
template<typename T>
struct QuantityTraits<T,typename boost::enable_if<boost::is_arithmetic<T> >::type> : public QuantityTraitsHelper<T>
{
  typedef void Unit;
  typedef T RawType;
  typedef T QuantityType;
  typedef boost::is_floating_point<T> is_floating_point;

  template<boost::units::integer_type N, boost::units::integer_type D = 1>
  struct GetQuantityToPowerType
  { typedef QuantityType type; };

  static inline QuantityType zero()
  { return RawType(0); }

  static inline QuantityType one()
  { return RawType(1); }

  static inline QuantityType conjugate( const QuantityType& a )
  { return Teuchos::ScalarTraits<QuantityType>::conjugate(a); }

  static inline QuantityType real( const QuantityType& a )
  { return Teuchos::ScalarTraits<QuantityType>::real(a); }

  static inline QuantityType imag( const QuantityType& a )
  { return Teuchos::ScalarTraits<QuantityType>::imag(a); }

  static inline QuantityType sqrt( const QuantityType quantity )
  { return std::sqrt( quantity ); }

  template<boost::units::integer_type N, boost::units::integer_type D>
  static inline QuantityType rpow( const QuantityType quantity )
  { return boost::units::pow<boost::units::static_rational<N,D> >( quantity ); }

  static inline QuantityType initializeQuantity( const RawType& raw_quantity )
  { return raw_quantity; }

  static inline const RawType& getRawQuantity( const QuantityType& quantity )
  { return quantity; }

  static inline void setQuantity( QuantityType& quantity,
				  const RawType& raw_quantity )
  { quantity = raw_quantity; }
};

} // end Utility namespace

#endif // end UTILITY_QUANTITY_TRAITS_HPP

//---------------------------------------------------------------------------//
// end Utility_QuantityTraits.hpp
//---------------------------------------------------------------------------//


/*
   Copyright 2018 Simon Vogl <svogl@voxel.at>
                  Angel Merino-Sastre <amerino@voxel.at>

   Licensed under the Apache License, Version 2.0 (the "License");
   you may not use this file except in compliance with the License.
   You may obtain a copy of the License at

       http://www.apache.org/licenses/LICENSE-2.0

   Unless required by applicable law or agreed to in writing, software
   distributed under the License is distributed on an "AS IS" BASIS,
   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   See the License for the specific language governing permissions and
   limitations under the License.
*/
#include <math.h>

#if OCV_VERSION_MAJOR >= 3
#include <opencv2/imgproc.hpp>
#include <opencv2/calib3d.hpp>
#include <opencv2/imgcodecs.hpp>
#else
#  include <opencv2/core/core.hpp>
#  include <opencv2/imgproc/imgproc.hpp>
#  include <opencv2/calib3d/calib3d.hpp>
#endif

#include <boost/log/trivial.hpp>
#include <toffy/common/filenodehelper.hpp>

#include <toffy/detection/mask.hpp>

using namespace toffy;
using namespace toffy::detection;
//using namespace cv;
using namespace std;

#ifdef toffy_DEBUG
const bool dbg=true;
#if OCV_VERSION_MAJOR >= 3
#include <opencv2/highgui.hpp>
#else
#  include <opencv2/highgui/highgui.hpp>
#endif
#else
const bool dbg=false;
#endif

std::size_t Mask::_filter_counter = 1;
const std::string Mask::id_name = "mask";

Mask::Mask(): Filter(Mask::id_name,_filter_counter),
    _in_depth("depth"), _in_ampl("ampl"), _out_mask("mask")
{
    _filter_counter++;

}

boost::property_tree::ptree Mask::getConfig() const {
    BOOST_LOG_TRIVIAL(debug) << __FUNCTION__ <<  " " << id();
    boost::property_tree::ptree pt;

    pt = Filter::getConfig();
    //pt.put("options.max_x", _max_x);

    pt.put("options.maskPath", _maskPath);
    pt.put("options.creation", _creation);
    pt.put("options.dis", _dis);
    pt.put("options.scale", _scale);
    //pt.put("options.cameraMatrix", _cameraMatrix);

    pt.put("inputs.depth", _in_depth);
    pt.put("inputs.ampl", _in_ampl);
    pt.put("outputs.mask", _out_mask);

    return pt;
}

void Mask::updateConfig(const boost::property_tree::ptree &pt) {
    BOOST_LOG_TRIVIAL(debug) << __FUNCTION__ <<  " " << id();

    using namespace boost::property_tree;

    Filter::updateConfig(pt);

    //_max_x = pt.get<float>("options.max_x",_max_x);
    _maskPath = pt.get<string>("options.maskPath",_maskPath);
    _creation = pt.get<bool>("options.creation", _creation);
    _creation = pt.get<bool>("options.creation",false);
    _dis = pt.get<double>("options.dis",_dis);
    _scale = pt.get<int>("options.scale",_scale);

    boost::optional<const boost::property_tree::ptree& > ocvo =
            pt.get_child_optional( "options.cameraMatrix" );
    if (ocvo.is_initialized()) {
        if( toffy::commons::checkOCVNone(*ocvo) ) {
            boost::property_tree::ptree os;
            os.put_child("cameraMatrix",*ocvo);
            cv::FileStorage fs = commons::loadOCVnode(os);
            //cout << fs.getFirstTopLevelNode().name() << endl;
            if (!_cameraMatrix)
                _cameraMatrix.reset(new cv::Mat());
            fs.getFirstTopLevelNode() >> *_cameraMatrix;
            fs.release();
        } else BOOST_LOG_TRIVIAL(debug) << __FUNCTION__;
        BOOST_LOG_TRIVIAL(debug) << "Node cameraMatrix is not opencv.";
    } else
        BOOST_LOG_TRIVIAL(debug) << "Node options.cameraMatrix not found.";

    _in_depth = pt.get<string>("inputs.depth",_in_depth);
    _in_ampl = pt.get<string>("inputs.ampl",_in_ampl);
    _out_mask = pt.get<string>("outputs.mask",_out_mask);
}

bool Mask::filter(const Frame& in, Frame& out) {

    // General input: depth image
    try {
        depth = in.getMatPtr(_in_depth);
    } catch(const boost::bad_any_cast &) {
        BOOST_LOG_TRIVIAL(warning) << "Could not cast input " << _in_depth <<
                                      ", filter  " << id() <<" not applied.";
        return false;
    }

    if (_creation) {

        // Creation input: Ampl image, Camera matrix
        try {
            ampl = in.getMatPtr(_in_ampl);
        } catch(const boost::bad_any_cast &) {
            BOOST_LOG_TRIVIAL(warning) <<
                                          "Could not cast input " << _in_ampl <<
                                          ", filter  " << id() <<" not applied.";
            return false;
        }

        if (!_cameraMatrix || _cameraMatrix->empty()) {
            BOOST_LOG_TRIVIAL(warning) << "No cameraMatrix data, filter "
                                       << id() <<" not applied.";
            return false;
        }

        static double focalLength, aspectRatio;
        static cv::Point2d center;
        _apertureWidth = 6.16;
        _apertureHeight = 4.62;
        cv::calibrationMatrixValues(*_cameraMatrix, depth->size(),
                                    _apertureWidth, _apertureHeight,
                                    fovx, fovy,
                                    focalLength, center, aspectRatio);

        generate3D();

        //groundProjection();
        groundProjection2();
        if (dbg) {
            cv::namedWindow(id_name + "[1] proj2d", cv::WINDOW_NORMAL);
            cv::imshow(id_name + "[1] proj2d", *proj2d);
        }

        if (!fground) {
            fground.reset(new cv::Mat());
        }
        *fground = proj2d->clone();
        BOOST_LOG_TRIVIAL(debug) << "fground->size()" << fground->size();
        //cout << "fground->size()" << fground->size() << endl;
        /*
        for(int i = 0; i < proj2d->rows; i++) {
            const float* ptr = proj2d->ptr<float>(i);
            for(int j = 0; j < proj2d->cols; j++) {
                if(ptr[j] == ptr[j]) {
                    cv::rectangle(*fground, cv::Point (j-maxSizeY/2,i-maxSizeX/2),
                              cv::Point (j+maxSizeY/2,i+maxSizeX/2), ptr[j],
                              cv::FILLED);
                }
            }
        }
        if (dbg) cv::imshow("fground", *fground);
        */
        //Project back
        projectBack();
        if (dbg) {
            cv::namedWindow(id_name + "[2] projectBack", cv::WINDOW_NORMAL);
            imshow(id_name + "[2] projectBack", *new_mask);
        }

        //Add low amp pixel to mask
        /*amplitudeMasking();
        if (dbg) {
            cv::namedWindow("amplitudeMasking", cv::WINDOW_NORMAL);
            imshow("*amplitudeMasking", *new_mask);
        }*/

        morfExMask(*new_mask);
        if (dbg) {
            cv::namedWindow(id_name + "[3] morfExMask", cv::WINDOW_NORMAL);
            imshow(id_name + "[3] morfExMask", *new_mask);
        }

        fillMaskGaps();
        if (dbg) {
            cv::namedWindow(id_name + "[4] new_mask insider", cv::WINDOW_NORMAL);
            imshow(id_name + "[4] new_mask insider", *new_mask);
        }

        *new_mask=~*new_mask;

        cv::Mat add_depth, fdepth;
        depth->copyTo(fdepth, *new_mask);
        add_depth = fdepth > 0;
        bitwise_xor(*new_mask,add_depth,*new_mask);

        boost::shared_ptr<cv::Mat> old_mask;
        try {
            old_mask = in.getMatPtr(_out_mask);
        } catch(const boost::bad_any_cast &) {
            BOOST_LOG_TRIVIAL(debug) << "Could not cast previous mask.";
            out.addData(_out_mask,new_mask);
            old_mask = new_mask;
        }
        *old_mask &= *new_mask;

        if (dbg) {
            cv::namedWindow(id_name + "[5] mask", cv::WINDOW_NORMAL);
            imshow(id_name + "[5] mask", *old_mask);
        }

        boost::shared_ptr<cv::Mat> depth_mask;
        if(!depth_mask) {
            depth_mask.reset(new cv::Mat());
            depth->copyTo(*depth_mask,~*old_mask);
            //cv::subtract(*depth_mask,*depth_mask*(5./100),*depth_mask);
            *depth_mask -= *depth_mask*(5./100);
        }

        cv::Mat temp;
        depth->copyTo(temp,~*old_mask);
        *depth_mask += (temp - temp*(5./100));
        *depth_mask /= 2.f;

        if (dbg) {
            cv::namedWindow("1depth_mask", cv::WINDOW_NORMAL);
            imshow("1depth_mask", *depth_mask);
        }

        BOOST_LOG_TRIVIAL(debug) << "depth_mask nomorph 20x77: " << depth_mask->at<float>(77,20);
        int morph_size = 1;
        cv::Mat structuringElement = cv::getStructuringElement(
                    cv::MORPH_ELLIPSE,
                    cv::Size( 2*morph_size + 1, 2*morph_size+1 ),
                    cv::Point( morph_size, morph_size ));

        cv::morphologyEx( *depth_mask, *depth_mask,
                          cv::MORPH_CLOSE, structuringElement, cv::Point(-1,-1), 1, cv::BORDER_CONSTANT );
        //morfExMask(*depth_mask);
        //*depth_mask = ~*depth_mask;
        cv::morphologyEx( *depth_mask, *depth_mask,
                          cv::MORPH_CLOSE, structuringElement, cv::Point(-1,-1), 1, cv::BORDER_CONSTANT );

        BOOST_LOG_TRIVIAL(debug) << "depth 20x77: " << depth->at<float>(77,20);
        BOOST_LOG_TRIVIAL(debug) << "depth_mask 20x77: " << depth_mask->at<float>(77,20);

        cv::FileStorage fs(_maskPath+"/mask.yml", cv::FileStorage::WRITE);
        fs << "mask" << *depth_mask;

        if (dbg) {
            cv::namedWindow("2depth_mask", cv::WINDOW_NORMAL);
            imshow("2depth_mask", *depth_mask);
        }

        //fdepth = 0;
        //depth->copyTo(fdepth, *mask);
        depth->copyTo(fdepth);

        BOOST_LOG_TRIVIAL(debug) << "fdepth 20x77: " << fdepth.at<float>(77,20);
        BOOST_LOG_TRIVIAL(debug) << "depth_mask 20x77: " << depth_mask->at<float>(77,20);

        cv::Mat m = (fdepth > *depth_mask) & (*depth_mask > 0.);

        if (dbg) {
            cv::namedWindow("comp", cv::WINDOW_NORMAL);
            imshow("comp", m);
        }

        fdepth.setTo(0., m);

        //*depth &= *mask;
        if (dbg) {
            cv::namedWindow("depthmask masked", cv::WINDOW_NORMAL);
            imshow("depthmask masked", fdepth);
        }
        *depth = fdepth;

        //save it
        vector<int> compression_params;
        compression_params.push_back(cv::IMWRITE_PNG_COMPRESSION);
        compression_params.push_back(0);
        cv::imwrite( _maskPath+"/test.png", *old_mask,compression_params );

        fdepth.setTo(0.);
        depth->copyTo(fdepth, *old_mask);
        //*depth &= *mask;
        if (dbg) {
            cv::namedWindow("depth masked", cv::WINDOW_NORMAL);
            imshow("depth masked", fdepth);
        }
        *depth = fdepth;

        //Time/Angle mask creation control
        /*if(_uangle < 0) {
            //First mask
            out.addData(_out_mask,new_mask);
            //_uangle = new_ang;

        } else if (_uangle == new_ang) {
            //Same mask, do and op
            boost::shared_ptr<cv::Mat> old_mask;
            try {
                old_mask = in.getMatPtr(_out_mask);
            } catch(const boost::bad_any_cast &) {
                BOOST_LOG_TRIVIAL(debug) <<
                                            "Could not cast previous mask.";
                out.addData(_out_mask,new_mask);
                old_mask = new_mask;
            }
            *old_mask &= *new_mask;

            //save it
            vector<int> compression_params;
            compression_params.push_back(IMWRITE_PNG_COMPRESSION);
            compression_params.push_back(0);
            cv::imwrite( "test"+boost::lexical_cast<std::string>(_uangle)+".png", *old_mask,compression_params );
        }
        _uangle = new_ang;*/

    } else {

        //Load depth and ampl angle by side
        BOOST_LOG_TRIVIAL(debug) << "Loading mask.";
        boost::shared_ptr<cv::Mat> mask;
        try {
            mask = in.getMatPtr(_out_mask);
        } catch(const boost::bad_any_cast &) {
            BOOST_LOG_TRIVIAL(debug) << "Not mask.";
            mask.reset(new cv::Mat());
            out.addData("mask", mask);
        }

        if (mask->empty()) {
            //Load mask;
            BOOST_LOG_TRIVIAL(debug) << "New angle, loading.";
            *mask = cv::imread(_maskPath+"/test.png",cv::IMREAD_UNCHANGED);
            if (mask->data == NULL) {
                BOOST_LOG_TRIVIAL(warning) << "Could not load mask file: " <<
                                              _maskPath+"/test"+".png" <<
                                              ", filter  " << id();
            }
        }
        if (dbg) {
            cv::namedWindow("Mask", cv::WINDOW_NORMAL);
            imshow("Mask", *mask);
        }

        cv::Mat dmask;
        cv::FileStorage fs(_maskPath+"/mask.yml", cv::FileStorage::READ);
        fs["mask"] >> dmask;

        if (dbg) {
            cv::namedWindow("dmask", cv::WINDOW_NORMAL);
            imshow("dmask", dmask);
        }

        cv::Mat fdepth;
        //depth->copyTo(fdepth, *mask);
        depth->copyTo(fdepth);

        BOOST_LOG_TRIVIAL(debug) << "fdepth 20x77: " << fdepth.at<float>(77,20);
        BOOST_LOG_TRIVIAL(debug) << "dmask 20x77: " << dmask.at<float>(77,20);

        cv::Mat m = (fdepth > dmask) & (dmask > 0.);

        if (dbg) {
            cv::namedWindow("comp", cv::WINDOW_NORMAL);
            imshow("comp", m);
        }

        fdepth.setTo(0., m);

        //*depth &= *mask;
        if (dbg) {
            cv::namedWindow("depth masked", cv::WINDOW_NORMAL);
            imshow("depth masked", fdepth);
        }
        *depth = fdepth;
    }

    return true;
}

static inline double deg2rad(float degrees ) {
    return (degrees*M_PI)/180.0;
}

void Mask::groundProjection2() {
    BOOST_LOG_TRIVIAL(debug) << __FUNCTION__;

    cv::Mat channel[3];

    //Get every single axis values and his min and max
    cv::split(*img3d, channel);

    //maxSizeX = (tan(pcl::deg2rad(fovx)/2)*_dis*2)*_scale;
    //maxSizeY = (tan(pcl::deg2rad(fovy)/2)*_dis*2)*_scale;
    maxSizeX = (tan(deg2rad(fovx)/2)*_dis*2)*_scale;
    maxSizeY = (tan(deg2rad(fovy)/2)*_dis*2)*_scale;

    //BOOST_LOG_TRIVIAL(debug) << "maxSizeX: " << maxSizeX;
    //BOOST_LOG_TRIVIAL(debug) << "maxSizeY: " << maxSizeY;

    if (!proj2d || !proj2d->data || proj2d->size() != cv::Size(maxSizeX,maxSizeY)) {
        proj2d.reset(new cv::Mat(maxSizeY, maxSizeX, CV_32F,
                                 numeric_limits<float>::quiet_NaN ()));
    } else
        *proj2d = numeric_limits<float>::quiet_NaN ();

    //cout << "planar->size()" << planar->size() << endl;
    //cout << "proj2d->size()" << proj2d->size() << endl;
    //cout << "img3d->size()" << img3d->size() << endl;

    // For each 3D point maps the coordinates.
    for (int y=0; y < img3d->rows; ++y) {
        for (int x=0; x < img3d->cols; ++x) {
            //cout << "(" << x << "," << y << ")" << endl;
            // Avoing Z NaN values
            if (channel[2].at<float>(y,x) != channel[2].at<float>(y,x))												//test.at<float>(y,x) = numeric_limits<double>::quiet_NaN ();
                continue;
            //TODO PARAMETERS!!
            if (channel[2].at<float>(y,x) > _dis || channel[2].at<float>(y,x) < _dis*0.05)
                continue;
            // Go for cm
            int valX = channel[0].at<float>(y,x)*_scale;
            int valY = channel[1].at<float>(y,x)*_scale;
            //Shift the values to be all positive
            valX += maxSizeX/2;
            valY += maxSizeY/2;

            // Invalid pixels could make the cm coordinates goes
            // beyond the sizes.
            if (valX < 0 || valY < 0 || valX >= (int)maxSizeX || valY >= (int)maxSizeY) {
                //cout << "(" << valX << "," << valY << ")" << endl;
                continue;
            }
            //cout << "(" << valX << "," << valY << ")" << endl;
            proj2d->at<float>(valY,valX) = channel[2].at<float>(y,x);
        }
    }
    //TEST
    //channel[2].copyTo(*depth);
}

void Mask::projectBack() {
    BOOST_LOG_TRIVIAL(debug) << __FUNCTION__;

    if (!new_mask || !new_mask->data || new_mask->size() != depth->size()) {
        new_mask.reset(new cv::Mat(cv::Mat::zeros(depth->size(), CV_8U)));
    } else
	new_mask->setTo(0);

    //Project back
    maxSizeX /= _scale;
    maxSizeY /= _scale;
    static double fl_x_reciprocal = 1.f/ _cameraMatrix->at<double>(0,0);
    static double fl_y_reciprocal = 1.f/ _cameraMatrix->at<double>(1,1);
    for (int y=0; y < fground->rows; ++y) {
        for (int x=0; x < fground->cols; ++x) {
            // Avoing Z NaN values
            if (fground->at<float>(y,x) != fground->at<float>(y,x)) {
                continue;
            }

            double fValX = x/(_scale*1.f);
            double fValY = y/(_scale*1.f);

            fValX = fValX - maxSizeX/2;
            fValY = fValY - maxSizeY/2;

            //Projecting back to image 2D coordinates
            int iValX = (fValX/(fground->at<float>(y,x)*fl_x_reciprocal))+((depth->cols/2)-1);
            int iValY = (fValY/(fground->at<float>(y,x)*fl_y_reciprocal))+((depth->rows/2)-1);

            // Saving found pixel
            if ( iValX >= 0 && iValX < new_mask->cols
                 && iValY >= 0 && iValY < new_mask->rows )
                new_mask->at<unsigned char>(iValY, iValX) = 255;
            else
                BOOST_LOG_TRIVIAL(debug) << "ERR " << iValX << "/" << iValY;
        }
    }
}

void Mask::amplitudeMasking() {
    BOOST_LOG_TRIVIAL(debug) << __FUNCTION__;

    cv::Mat amp_mask;
    amp_mask = *ampl <= 150;
    bitwise_xor(*new_mask,amp_mask,*new_mask);
}

void Mask::morfExMask(cv::Mat &mask) {
    BOOST_LOG_TRIVIAL(debug) << __FUNCTION__;

    int morph_size = 0;
    cv::Mat structuringElement = cv::getStructuringElement(
                cv::MORPH_ELLIPSE,
                cv::Size( 2*morph_size + 1, 2*morph_size+1 ),
                cv::Point( morph_size, morph_size ));

    cv::morphologyEx( mask, mask,
                      cv::MORPH_OPEN, structuringElement);

    cv::Mat antiMask = ~mask.clone();
    if (dbg) {
        cv::namedWindow("morphologyEx antiMask", cv::WINDOW_NORMAL);
        cv::imshow("morphologyEx antiMask", antiMask);
    }

    morph_size = 3;
    structuringElement = cv::getStructuringElement(
                cv::MORPH_ELLIPSE, cv::Size( 2*morph_size + 1, 2*morph_size+1 ),
                cv::Point( morph_size, morph_size ));
    cv::morphologyEx( antiMask, antiMask,
                      cv::MORPH_OPEN, structuringElement );

    mask = ~antiMask.clone();

    if (dbg) {
        cv::namedWindow("morphologyEx mask", cv::WINDOW_NORMAL);
        cv::imshow("morphologyEx mask", mask);
    }
    //imshow("after new_mask", mask);

}

#if OCV_VERSION_MAJOR >= 3
#  define LINE_AA cv::LINE_AA
#  define FILLED cv::FILLED
#  define LINE_8 8
#else
#  define LINE_AA CV_AA
#  define FILLED -1
#  define LINE_8 8
#endif

void Mask::fillMaskGaps() {
    BOOST_LOG_TRIVIAL(debug) << __FUNCTION__;

    vector<vector<cv::Point> > contours;
    vector<cv::Vec4i> hierarchy;
    cv::findContours(new_mask->clone(), contours, hierarchy,
                     cv::RETR_CCOMP, cv::CHAIN_APPROX_SIMPLE/*, Point(1,1)*/);

    cv::Mat drawing2 = depth->clone();
    drawing2.convertTo(drawing2, cv::COLOR_GRAY2BGR);
    //cout << "contours.size() " << contours.size() << endl;
    for( size_t i = 0; i < contours.size(); i++ ) {
        cv::drawContours( drawing2, contours, i, cv::Scalar(255, 0, 255), 1,
                          LINE_AA, hierarchy, 0, cv::Point() );
    }
    if (dbg) cv::imshow("drawing2", drawing2);

    double maxArea = -1;
    int maxAreaIndex = -1;
    //cout << "contours.size() " << contours.size() << endl;
    for( size_t i = 0; i < contours.size(); i++ ) {
        double area = contourArea(contours[i]);
        ///cout << "area " << area << endl;
        if (area > 15000) {
            if (contours.size() == 1)
                contours.clear();
            else {
                contours.erase(contours.begin()+i);
                hierarchy.erase(hierarchy.begin()+i);
                i--;
            }
            continue;
        }
        if (area > maxArea) {
            maxArea = area;
            maxAreaIndex = i;
        }
    }

    //cout << "contours.size() " << contours.size() << endl;
    //cout << "maxAreaIndex " << maxAreaIndex << endl;
    if (maxAreaIndex >= 0) {
        if (contours.size() == 1)
            contours.clear();
        else {
            contours.erase(contours.begin()+maxAreaIndex);
            hierarchy.erase(hierarchy.begin()+maxAreaIndex);
        }
    }

    cv::Mat drawing = cv::Mat::zeros( new_mask->size(), CV_8U );
    //cout << "contours.size() " << contours.size() << endl;
    for( size_t i = 0; i < contours.size(); i++ ) {
        double area = contourArea(contours[i]);
        if (area*2 >= maxArea)
            continue;
        cv::drawContours( drawing, contours, i, cv::Scalar(255, 255, 255),
                          FILLED, LINE_8, hierarchy, 0, cv::Point() );
    }
    if (dbg) cv::imshow("drawing", drawing);
    *new_mask |= drawing;
}

void Mask::generate3D() {
    BOOST_LOG_TRIVIAL(debug) << __FUNCTION__;

    if (!img3d) {
        img3d.reset(new cv::Mat(depth->size(),CV_32FC3));
    }

    //Allocating the 3D points

    //Saving parameter from the camera matrix
    static double fl_x_reciprocal = 1.f/_cameraMatrix->at<double>(0,0);
    static double fl_y_reciprocal = 1.f/_cameraMatrix->at<double>(1,1);
    static double center_x = _cameraMatrix->at<double>(0,2);
    static double center_y = _cameraMatrix->at<double>(1,2);

    // Calculates the 3D point for each depth value
    for (int y=0; y < static_cast<int> (depth->rows); ++y) {
        for (int x=0; x < static_cast<int> (depth->cols); ++x) {
            //PointWithRange& point = getPointNoCheck (x, y);
            float *dptr = img3d->ptr<float>(y,x);
            float depthValue = depth->at<float>(y,x);

            dptr[0] = numeric_limits<float>::quiet_NaN (); //X
            dptr[1] = numeric_limits<float>::quiet_NaN (); //Y
            dptr[2] = numeric_limits<float>::quiet_NaN (); //Y
            //cout << " Filtered (" << x << "," << y << ")" << endl;

            //cout << "(" << x << "," << y << ")" << endl;
            //cout << frame.amp().at<unsigned short>(y,x) << endl;
            //point.z = depth;
            //point.x = (static_cast<float> (x) - center_x) * point.z * fl_x_reciprocal;
            //point.y = (static_cast<float> (y) - center_y) * point.z * fl_y_reciprocal;
            /*
         * cout << "x: " <<  (static_cast<float> (x) - center_x) * depthValue * fl_x_reciprocal;
         * cout << ", y: " <<  (static_cast<float> (y) - center_y) * depthValue * fl_y_reciprocal;
         * cout << ", z: " <<  depthValue << endl;
         */
            //Set 3D points
            dptr[0] = (static_cast<float> (x) - center_x) * depthValue * fl_x_reciprocal; //X
            dptr[1] = (static_cast<float> (y) - center_y) * depthValue * fl_y_reciprocal; //Y
            dptr[2] = depthValue; //Z
        }
    }
}