// Copyright (c) 2020 The PIVX developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or https://www.opensource.org/licenses/mit-license.php.

#include "wallet/test/wallet_test_fixture.h"

#include "consensus/merkle.h"
#include "primitives/block.h"
#include "random.h"
#include "sapling/note.h"
#include "sapling/noteencryption.h"
#include "sapling/transaction_builder.h"
#include "test/librust/utiltest.h"
#include "wallet/wallet.h"

#include <boost/filesystem.hpp>

#include <boost/test/unit_test.hpp>

CAmount fee = COIN; // Hardcoded fee

BOOST_FIXTURE_TEST_SUITE(wallet_shielded_balances_tests, WalletTestingSetup)

void setupWallet(CWallet& wallet)
{
    wallet.SetMinVersion(FEATURE_SAPLING);
    wallet.SetupSPKM(false);
}

// Find and set notes data in the tx + add any missing ivk to the wallet's keystore.
CWalletTx& SetWalletNotesData(CWallet* wallet, CWalletTx& wtx)
{
    Optional<mapSaplingNoteData_t> saplingNoteData{nullopt};
    wallet->FindNotesDataAndAddMissingIVKToKeystore(wtx, saplingNoteData);
    assert(static_cast<bool>(saplingNoteData));
    wtx.SetSaplingNoteData(*saplingNoteData);
    BOOST_CHECK(wallet->AddToWallet(wtx));
    // Updated tx
    return wallet->mapWallet[wtx.GetHash()];
}

/**
 * Creates and send a tx with an input of 'inputAmount' to 'vDest'.
 */
CWalletTx& AddShieldedBalanceToWallet(const CAmount& inputAmount,
                                      std::vector<ShieldedDestination> vDest,
                                      CWallet* wallet,
                                      const Consensus::Params& consensusParams)
{

    // Dummy wallet, used to generate the dummy transparent input key and sign it in the transaction builder
    CWallet dummyWallet;
    dummyWallet.SetMinVersion(FEATURE_SAPLING);
    dummyWallet.SetupSPKM(false, true);
    LOCK(dummyWallet.cs_wallet);

    // Create a transaction shielding balance to 'vDest' and load it to the wallet.
    CWalletTx wtx = GetValidSaplingReceive(consensusParams, dummyWallet, inputAmount, vDest, wallet);

    // Updated tx after load it to the wallet
    CWalletTx& wtxUpdated = SetWalletNotesData(wallet, wtx);
    // Check tx credit now
    BOOST_CHECK_EQUAL(wtxUpdated.GetCredit(ISMINE_ALL), inputAmount);
    BOOST_CHECK(wtxUpdated.IsAmountCached(CWalletTx::CREDIT, ISMINE_SPENDABLE_SHIELDED));
    return wtxUpdated;
}

CWalletTx& AddShieldedBalanceToWallet(libzcash::SaplingPaymentAddress& sendTo, CAmount amount,
                                CWallet& wallet, const Consensus::Params& consensusParams,
                                libzcash::SaplingExtendedSpendingKey& extskOut)
{
    // Create a transaction shielding balance to 'sendTo' and load it to the wallet.
    BOOST_CHECK(wallet.GetSaplingExtendedSpendingKey(sendTo, extskOut));
    std::vector<ShieldedDestination> vDest;
    vDest.push_back({extskOut, amount});
    return AddShieldedBalanceToWallet(amount, vDest, &wallet, consensusParams);
}

struct SaplingSpendValues {
    libzcash::SaplingNote note;
    const uint256 anchor;
    const SaplingWitness witness;
};

/**
 * Update the wallet internally as if the wallet would had received a valid block containing wtx.
 * Then return the note, anchor and witness for any subsequent spending process.
 */
SaplingSpendValues UpdateWalletInternalNotesData(CWalletTx& wtx, SaplingOutPoint& sapPoint, CWallet& wallet)
{
    // Get note
    SaplingNoteData nd = wtx.mapSaplingNoteData.at(sapPoint);
    assert(nd.IsMyNote());
    const auto& ivk = *(nd.ivk);
    auto maybe_pt = libzcash::SaplingNotePlaintext::decrypt(
            wtx.sapData->vShieldedOutput[sapPoint.n].encCiphertext,
            ivk,
            wtx.sapData->vShieldedOutput[sapPoint.n].ephemeralKey,
            wtx.sapData->vShieldedOutput[sapPoint.n].cmu);
    assert(static_cast<bool>(maybe_pt));
    boost::optional<libzcash::SaplingNotePlaintext> notePlainText = maybe_pt.get();
    libzcash::SaplingNote note = notePlainText->note(ivk).get();

    // Append note to the tree
    auto commitment = note.cmu().get();
    SaplingMerkleTree tree;
    tree.append(commitment);
    auto anchor = tree.root();
    auto witness = tree.witness();

    // Update wtx credit chain data
    // Pretend we mined the tx by adding a fake witness and nullifier to be able to spend it.
    wtx.mapSaplingNoteData[sapPoint].witnesses.push_front(tree.witness());
    wtx.mapSaplingNoteData[sapPoint].witnessHeight = 1;
    wallet.GetSaplingScriptPubKeyMan()->nWitnessCacheSize = 1;
    wallet.GetSaplingScriptPubKeyMan()->UpdateSaplingNullifierNoteMapWithTx(wtx);
    return {note, anchor, witness};
}

/**
 * Validates:
 * 1) CWalletTx getCredit for shielded credit.
 * Incoming spendable shielded balance must be cached in the cacheableAmounts.
 *
 * 2) CWalletTx getDebit & getCredit for shielded debit to transparent address.
 * Same wallet as point (1), spending half of the credit received in (1) to a transparent remote address.
 * The other half of the balance - minus fee - must appear as credit (shielded change).
 *
 */
BOOST_AUTO_TEST_CASE(GetShieldedSimpleCachedCreditAndDebit)
{

    ///////////////////////
    //////// Credit ////////
    ///////////////////////

    auto consensusParams = RegtestActivateSapling();

    // Main wallet
    CWallet &wallet = *pwalletMain;
    LOCK2(cs_main, wallet.cs_wallet);
    setupWallet(wallet);

    // First generate a shielded address
    libzcash::SaplingPaymentAddress pa = wallet.GenerateNewSaplingZKey();
    CAmount firstCredit = COIN * 10;

    // Add shielded balance.
    libzcash::SaplingExtendedSpendingKey extskOut;
    CWalletTx& wtxUpdated = AddShieldedBalanceToWallet(pa, firstCredit, wallet, consensusParams, extskOut);

    ///////////////////////
    //////// Debit ////////
    ///////////////////////

    // Update transaction and wallet internal state to be able to spend it.
    SaplingOutPoint sapPoint {wtxUpdated.GetHash(), 0};
    SaplingSpendValues sapSpendValues = UpdateWalletInternalNotesData(wtxUpdated, sapPoint, wallet);

    // Debit value
    CAmount firstDebit = COIN * 5;
    CAmount firstDebitShieldedChange = firstDebit - fee;

    // Create the spending transaction
    auto builder = TransactionBuilder(consensusParams, 1, &wallet);
    builder.SetFee(fee);
    builder.AddSaplingSpend(
            extskOut.expsk,
            sapSpendValues.note,
            sapSpendValues.anchor,
            sapSpendValues.witness);

    // Send to transparent address
    builder.AddTransparentOutput(CreateDummyDestinationScript(),
                                 firstDebit);

    CTransaction tx = builder.Build().GetTxOrThrow();
    // add tx to wallet and update it.
    wallet.AddToWallet({&wallet, tx});
    CWalletTx& wtxDebit = wallet.mapWallet[tx.GetHash()];
    // Update tx notes data (shielded change need it)
    CWalletTx& wtxDebitUpdated = SetWalletNotesData(&wallet, wtxDebit);

    // The debit need to be the entire first note value
    BOOST_CHECK_EQUAL(wtxDebitUpdated.GetDebit(ISMINE_ALL), firstCredit);
    BOOST_CHECK(wtxDebitUpdated.IsAmountCached(CWalletTx::DEBIT, ISMINE_SPENDABLE_SHIELDED));
    // The credit should be only the change.
    BOOST_CHECK_EQUAL(wtxDebitUpdated.GetCredit(ISMINE_ALL), firstDebitShieldedChange);
    BOOST_CHECK(wtxDebitUpdated.IsAmountCached(CWalletTx::CREDIT, ISMINE_SPENDABLE_SHIELDED));

    // Checks that the only shielded output of this tx is change.
    BOOST_CHECK(wallet.GetSaplingScriptPubKeyMan()->IsNoteSaplingChange(
            SaplingOutPoint(wtxDebitUpdated.GetHash(), 0), pa));

    // Revert to default
    RegtestDeactivateSapling();
}

libzcash::SaplingPaymentAddress getNewDummyShieldedAddress()
{
    HDSeed seed;
    auto m = libzcash::SaplingExtendedSpendingKey::Master(seed);
    return m.DefaultAddress();
}

CWalletTx& buildTxAndLoadToWallet(CWallet& wallet, libzcash::SaplingExtendedSpendingKey extskOut,
                                  const SaplingSpendValues& sapSpendValues, libzcash::SaplingPaymentAddress dest,
                                  const CAmount& destAmount, const Consensus::Params& consensus)
{
    // Create the spending transaction
    auto builder = TransactionBuilder(consensus, 1, &wallet);
    builder.SetFee(fee);
    builder.AddSaplingSpend(
            extskOut.expsk,
            sapSpendValues.note,
            sapSpendValues.anchor,
            sapSpendValues.witness);

    // Send to shielded address
    builder.AddSaplingOutput(
            extskOut.expsk.ovk,
            dest,
            destAmount,
            {}
    );

    CTransaction tx = builder.Build().GetTxOrThrow();
    // add tx to wallet and update it.
    wallet.AddToWallet({&wallet, tx});
    CWalletTx& wtx = wallet.mapWallet[tx.GetHash()];
    // Update tx notes data and return the updated wtx.
    return SetWalletNotesData(&wallet, wtx);
}

/**
 * Validates shielded to remote shielded + change cached balances.
 */
BOOST_AUTO_TEST_CASE(VerifyShieldedToRemoteShieldedCachedBalance)
{
    auto consensusParams = RegtestActivateSapling();

    // Main wallet
    CWallet &wallet = *pwalletMain;
    LOCK2(cs_main, wallet.cs_wallet);
    setupWallet(wallet);

    // First generate a shielded address
    libzcash::SaplingPaymentAddress pa = wallet.GenerateNewSaplingZKey();
    CAmount firstCredit = COIN * 20;

    // Add shielded balance.
    libzcash::SaplingExtendedSpendingKey extskOut;
    CWalletTx& wtxUpdated = AddShieldedBalanceToWallet(pa, firstCredit, wallet, consensusParams, extskOut);

    // Update transaction and wallet internal state to be able to spend it.
    SaplingOutPoint sapPoint {wtxUpdated.GetHash(), 0};
    SaplingSpendValues sapSpendValues = UpdateWalletInternalNotesData(wtxUpdated, sapPoint, wallet);

    // Remote destination values
    libzcash::SaplingPaymentAddress destShieldedAddress = getNewDummyShieldedAddress();
    CAmount destAmount = COIN * 8;

    // Create the spending transaction and load it to the wallet
    CWalletTx& wtxDebitUpdated = buildTxAndLoadToWallet(wallet,
                           extskOut,
                           sapSpendValues,
                           destShieldedAddress,
                           destAmount,
                           consensusParams);

    // Validate results
    CAmount expectedShieldedChange = firstCredit - destAmount - fee;

    // The debit need to be the entire first note value
    BOOST_CHECK_EQUAL(wtxDebitUpdated.GetDebit(ISMINE_ALL), firstCredit);
    BOOST_CHECK(wtxDebitUpdated.IsAmountCached(CWalletTx::DEBIT, ISMINE_SPENDABLE_SHIELDED));
    // The credit should be only the change.
    BOOST_CHECK_EQUAL(wtxDebitUpdated.GetCredit(ISMINE_ALL), expectedShieldedChange);
    BOOST_CHECK(wtxDebitUpdated.IsAmountCached(CWalletTx::CREDIT, ISMINE_SPENDABLE_SHIELDED));
    // Plus, change should be same and be cached as well
    BOOST_CHECK_EQUAL(wtxDebitUpdated.GetShieldedChange(), expectedShieldedChange);
    BOOST_CHECK(wtxDebitUpdated.fShieldedChangeCached);

    // Revert to default
    RegtestDeactivateSapling();
}

struct FakeBlock
{
    CBlock block;
    CBlockIndex* pindex;
};

FakeBlock SimpleFakeMine(CWalletTx& wtx, SaplingMerkleTree& currentTree)
{
    FakeBlock fakeBlock;
    fakeBlock.block.nVersion = 8;
    fakeBlock.block.vtx.emplace_back(MakeTransactionRef(wtx));
    fakeBlock.block.hashMerkleRoot = BlockMerkleRoot(fakeBlock.block);
    for (const OutputDescription& out : wtx.sapData->vShieldedOutput) {
        currentTree.append(out.cmu);
    }
    fakeBlock.block.hashFinalSaplingRoot = currentTree.root();
    fakeBlock.pindex = new CBlockIndex(fakeBlock.block);
    mapBlockIndex.insert(std::make_pair(fakeBlock.block.GetHash(), fakeBlock.pindex));
    fakeBlock.pindex->phashBlock = &mapBlockIndex.find(fakeBlock.block.GetHash())->first;
    chainActive.SetTip(fakeBlock.pindex);
    BOOST_CHECK(chainActive.Contains(fakeBlock.pindex));
    wtx.SetMerkleBranch(fakeBlock.pindex->GetBlockHash(), 0);
    return fakeBlock;
}

/**
 * Test:
 * 1) receive two shielded notes on the same tx.
 * 2) check available credit.
 * 3) spend one of them.
 * 4) force available credit cache recalculation and validate the updated amount.
 */
BOOST_AUTO_TEST_CASE(GetShieldedAvailableCredit)
{
    auto consensusParams = RegtestActivateSapling();

    // Main wallet
    CWallet &wallet = *pwalletMain;
    LOCK2(cs_main, wallet.cs_wallet);
    setupWallet(wallet);

    // 1) generate a shielded address and send 20 NOMD in two shielded outputs
    libzcash::SaplingPaymentAddress pa = wallet.GenerateNewSaplingZKey();
    CAmount credit = COIN * 20;

    // Add two equal shielded outputs.
    libzcash::SaplingExtendedSpendingKey extskOut;
    BOOST_CHECK(wallet.GetSaplingExtendedSpendingKey(pa, extskOut));

    std::vector<ShieldedDestination> vDest;
    vDest.push_back({extskOut, credit / 2});
    vDest.push_back({extskOut, credit / 2});
    CWalletTx& wtxUpdated = AddShieldedBalanceToWallet(credit, vDest, &wallet, consensusParams);

    // Available credit ISMINE_SPENDABLE must be 0
    // Available credit ISMINE_SHIELDED_SPENDABLE must be 'credit' and be cached.
    BOOST_CHECK_EQUAL(wtxUpdated.GetAvailableCredit(true, ISMINE_SPENDABLE), 0);
    BOOST_CHECK_EQUAL(wtxUpdated.GetShieldedAvailableCredit(), credit);
    BOOST_CHECK(wtxUpdated.IsAmountCached(CWalletTx::AVAILABLE_CREDIT, ISMINE_SPENDABLE_SHIELDED));

    // 2) Confirm the tx
    SaplingMerkleTree tree;
    FakeBlock fakeBlock = SimpleFakeMine(wtxUpdated, tree);
    wallet.ChainTip(fakeBlock.pindex, &fakeBlock.block, tree);
    wtxUpdated = wallet.mapWallet[wtxUpdated.GetHash()];

    // 3) Now can spend one output and recalculate the shielded credit.
    std::vector<SaplingNoteEntry> saplingEntries;
    Optional<libzcash::SaplingPaymentAddress> opPa(pa);
    wallet.GetSaplingScriptPubKeyMan()->GetFilteredNotes(saplingEntries,
                                                         opPa,
                                                         0);

    std::vector<SaplingOutPoint> ops = {saplingEntries[0].op};
    uint256 anchor;
    std::vector<boost::optional<SaplingWitness>> witnesses;
    pwalletMain->GetSaplingScriptPubKeyMan()->GetSaplingNoteWitnesses(ops, witnesses, anchor);
    SaplingSpendValues sapSpendValues{saplingEntries[0].note, anchor, *witnesses[0]};

    // Remote destination values
    libzcash::SaplingPaymentAddress destShieldedAddress = getNewDummyShieldedAddress();
    CAmount change = COIN * 1;
    CAmount destAmount = credit / 2 - fee - change; // one note - fee

    // Create the spending transaction and load it to the wallet
    CWalletTx& wtxDebitUpdated = buildTxAndLoadToWallet(wallet,
                                                        extskOut,
                                                        sapSpendValues,
                                                        destShieldedAddress,
                                                        destAmount,
                                                        consensusParams);

    // Check previous credit tx balance being the same and then force a recalculation
    BOOST_CHECK_EQUAL(wtxUpdated.GetShieldedAvailableCredit(), credit);
    BOOST_CHECK_EQUAL(wtxUpdated.GetShieldedAvailableCredit(false), credit / 2);
    BOOST_CHECK_EQUAL(wtxUpdated.GetShieldedChange(), 0);

    // Now check the debit tx
    BOOST_CHECK_EQUAL(wtxDebitUpdated.GetDebit(ISMINE_SPENDABLE_SHIELDED), credit / 2);
    BOOST_CHECK_EQUAL(wtxDebitUpdated.GetShieldedChange(), change);
    BOOST_CHECK_EQUAL(wtxDebitUpdated.GetCredit(ISMINE_SPENDABLE_SHIELDED), change);
}

BOOST_AUTO_TEST_SUITE_END()


#include "domains/team_comp_sar.h"
#include <math.h>
#include <stdlib.h>
#include "plan_trace.h"
#include "plangrapher.h"
#include <istream>
#include <nlohmann/json.hpp>
#include <boost/program_options.hpp>
namespace po = boost::program_options;

using json = nlohmann::json;

using namespace std;

int main(int argc, char* argv[]) {
  int R = 30;
  double e = 0.4;
  double alpha = 0.5;
  int aux_R = 10;
  std::string map_json = "../apps/data_parsing/Saturn_map_info.json";
  std::string cpm_json;
  try {
    po::options_description desc("Allowed options");
    desc.add_options()
      ("help,h", "produce help message")
      ("resource_cycles,R", po::value<int>(), "Number of resource cycles allowed for each search action (int)")
      ("exp_param,e",po::value<double>(),"The exploration parameter for the planner (double)")
      ("alpha,a", po::value<double>(), "default frequency measure for missing conditional probabilities in CFM (default)")
      ("map_json,m", po::value<std::string>(),"json file with map data (string)")
      ("cpm_json,j",po::value<std::string>(),"json file to parse CPM (string)")
      ("aux_r,a", po::value<int>(), "Auxiliary resources for bad expansions (int)")
    ;

    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

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

    if (vm.count("resource_cycles")) {
      R = vm["resource_cycles"].as<int>();
    }

    if (vm.count("exp_param")) {
      e = vm["exp_param"].as<double>();
    }

    if (vm.count("alpha")) {
      alpha = vm["alpha"].as<double>();
    }

    if (vm.count("aux_r")) {
      aux_R = vm["aux_r"].as<int>();
    }

    if (vm.count("cpm_json")) {
      cpm_json = vm["cpm_json"].as<std::string>();
    }

    if (vm.count("map_json")) {
      map_json = vm["map_json"].as<std::string>();
    }
  }
  catch(std::exception& e) {
    std::cerr << "error: " << e.what() << "\n";
    return 1;
  }
  catch(...) {
    std::cerr << "Exception of unknown type!\n";
  }

    std::ifstream f(map_json);
    json g;
    f >> g;

    auto state1 = TeamSARState();
    std::string agent1 = "A1";
    std::string agent2 = "A2";
    std::string agent3 = "A3";
    state1.agents.push_back(agent1);
    state1.agents.push_back(agent2);
    state1.agents.push_back(agent3);

    state1.class_only_boundary = "el";
    state1.change_zone = g["change_zone"].get<std::string>();
    for (auto& nvz : g["no_victim_zones"]) {
      state1.no_victim_zones.push_back(nvz.get<std::string>());
    }
    for (auto& mrz : g["multi_room_zones"]) {
      state1.multi_room_zones.push_back(mrz.get<std::string>());
    }
    for (auto& z : g["zones"]) {
      state1.zones.push_back(z.get<std::string>());
    }

    for (auto& [l,vl] : g["graph"].items()) {
      for (auto& c : vl) {
        state1.graph[l].push_back(c.get<std::string>());
      }
    }

    for (auto& [d,di] : g["dist_from_change_zone"].items()) {
        state1.dist_from_change_zone[d] = di.get<int>();
    }

    for (auto& mrz : g["multi_room_zones"]) {
      state1.multi_room_zones.push_back(mrz.get<std::string>());
    }
    for (auto& r : g["rooms"]) {
      state1.rooms.push_back(r.get<std::string>());
    }

    for (auto a : state1.agents) {
      state1.role[a] = "NONE";

      state1.agent_loc[a] = state1.change_zone;

      state1.holding[a] = false;

      state1.time[a] = 0;

      state1.loc_tracker[a] = {};

      state1.action_tracker[a] = {};

      for (auto s : state1.zones) {
        if (s == state1.change_zone) {
          state1.visited[a][s] = 1;
        }
        else {
          state1.visited[a][s] = 0;
        }
      }
    }

    for (auto s : state1.zones) {
      state1.blocks_broken[s] = 0;

      state1.r_triaged_here[s] = false;

      state1.c_triaged_here[s] = false;

      state1.c_awake[s] = false;

    }

    state1.c_triage_total = 0;

    state1.r_triage_total = 0;

    state1.c_max = 5;
    state1.r_max = 50;

    auto domain = TeamSARDomain();

    CPM cpm = {};

    if (cpm_json != "") {
      std::ifstream i(cpm_json);
      json j;
      i >> j;
      for (auto& [k1, v1] : j.items()) {
        for (auto& [k2,v2] : v1.items()) {
          for (auto& [k3, v3] : v2.items()) {
            cpm[k1][k2][k3] = v3;
          }
        }
      }
    }

    Tasks tasks = {
        {Task("SAR", Args({{"agent3", agent3},{"agent2", agent2},{"agent1",agent1}}),{"agent1","agent2","agent3"},{agent1,agent2,agent3})}};
    auto pt = cppMCTShop(state1, tasks, domain, cpm,R,e,alpha,4021,aux_R);

    json j_tree = generate_plan_trace_tree(pt.first,pt.second,true,"team_comp_sar_trace_tree.json");
    generate_graph_from_json(j_tree, "team_comp_sar_tree_graph.png");
    generate_plan_trace(pt.first,pt.second,true,"team_comp_sar_trace.json");
    return EXIT_SUCCESS;
}


#include <boost/python/class.hpp>
#include <boost/python/enum.hpp>
#include <boost/python/args.hpp>
#include <boost/python/return_value_policy.hpp>
#include <boost/python/return_by_value.hpp>
#include <boost/python/return_internal_reference.hpp>

#include <scitbx/array_family/boost_python/shared_wrapper.h>

#include <cctbx/miller/amplitude_normalisation.h>

namespace cctbx { namespace miller { namespace boost_python {

  template <typename FloatType>
  struct amplitude_normalisation_wrapper
  {
    typedef amplitude_normalisation<FloatType> wt;
    typedef typename wt::float_type float_type;

    static void wrap() {
      using namespace boost::python;
      return_value_policy<return_by_value> rbv;
      typedef return_internal_reference<> rir;

      af::boost_python::shared_wrapper<
        typename wt::form_factor_t, rir>::wrap(
        "shared_gaussian_form_factors");

      class_<wt>("amplitude_normalisation", no_init)
        .def(init<af::const_ref<typename wt::form_factor_t> const &,
                  af::const_ref<float_type> const &,
                  float_type,
                  float_type,
                  uctbx::unit_cell const &,
                  sgtbx::space_group const &,
                  af::const_ref<index<> > const &>(
              (arg("form_factors"),
               arg("multiplicities"),
               arg("wilson_intensity_scale_factor"),
               arg("wilson_b"),
               arg("unit_cell"),
               arg("space_group"),
               arg("indices"))))
        .add_property("normalisations", make_getter(&wt::normalisations, rbv))
        ;
    }
  };

  void wrap_amplitude_normalisation() {
    amplitude_normalisation_wrapper<double>::wrap();
  }
}}} // cctbx::miller::boostpython


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

#include <ored/model/crossassetmodelbuilder.hpp>
#include <ored/model/eqbsbuilder.hpp>
#include <ored/model/fxbsbuilder.hpp>
#include <ored/model/infdkbuilder.hpp>
#include <ored/model/lgmbuilder.hpp>
#include <ored/model/utilities.hpp>
#include <ored/utilities/correlationmatrix.hpp>
#include <ored/utilities/log.hpp>
#include <ored/utilities/parsers.hpp>

#include <qle/models/fxbsconstantparametrization.hpp>
#include <qle/models/fxbspiecewiseconstantparametrization.hpp>
#include <qle/models/fxeqoptionhelper.hpp>
#include <qle/models/irlgm1fconstantparametrization.hpp>
#include <qle/models/irlgm1fpiecewiseconstanthullwhiteadaptor.hpp>
#include <qle/models/irlgm1fpiecewiseconstantparametrization.hpp>
#include <qle/models/irlgm1fpiecewiselinearparametrization.hpp>
#include <qle/pricingengines/analyticcclgmfxoptionengine.hpp>
#include <qle/pricingengines/analyticdkcpicapfloorengine.hpp>
#include <qle/pricingengines/analyticlgmswaptionengine.hpp>
#include <qle/pricingengines/analyticxassetlgmeqoptionengine.hpp>

#include <ql/math/optimization/levenbergmarquardt.hpp>
#include <ql/models/shortrate/calibrationhelpers/swaptionhelper.hpp>
#include <ql/quotes/simplequote.hpp>
#include <ql/utilities/dataformatters.hpp>

#include <boost/algorithm/string/case_conv.hpp>
#include <boost/lexical_cast.hpp>

namespace ore {
namespace data {

CrossAssetModelBuilder::CrossAssetModelBuilder(const boost::shared_ptr<ore::data::Market>& market,
                                               const std::string& configurationLgmCalibration,
                                               const std::string& configurationFxCalibration,
                                               const std::string& configurationEqCalibration,
                                               const std::string& configurationInfCalibration,
                                               const std::string& configurationFinalModel, const DayCounter& dayCounter)
    : market_(market), configurationLgmCalibration_(configurationLgmCalibration),
      configurationFxCalibration_(configurationFxCalibration), configurationEqCalibration_(configurationEqCalibration),
      configurationInfCalibration_(configurationInfCalibration), configurationFinalModel_(configurationFinalModel),
      dayCounter_(dayCounter),
      optimizationMethod_(boost::shared_ptr<OptimizationMethod>(new LevenbergMarquardt(1E-8, 1E-8, 1E-8))),
      endCriteria_(EndCriteria(1000, 500, 1E-8, 1E-8, 1E-8)) {
    QL_REQUIRE(market != NULL, "CrossAssetModelBuilder: no market given");
}

boost::shared_ptr<QuantExt::CrossAssetModel>
CrossAssetModelBuilder::build(const boost::shared_ptr<CrossAssetModelData>& config) {

    LOG("Start building CrossAssetModel, configurations: LgmCalibration "
        << configurationLgmCalibration_ << ", FxCalibration " << configurationFxCalibration_ << ", EqCalibration "
        << configurationEqCalibration_ << ", InfCalibration " << configurationInfCalibration_ << ", FinalModel "
        << configurationFinalModel_);

    QL_REQUIRE(config->irConfigs().size() > 0, "missing IR configurations");
    QL_REQUIRE(config->irConfigs().size() == config->fxConfigs().size() + 1,
               "FX configuration size " << config->fxConfigs().size() << " inconsisitent with IR configuration size "
                                        << config->irConfigs().size());

    swaptionBaskets_.resize(config->irConfigs().size());
    optionExpiries_.resize(config->irConfigs().size());
    swaptionMaturities_.resize(config->irConfigs().size());
    swaptionCalibrationErrors_.resize(config->irConfigs().size());
    fxOptionBaskets_.resize(config->fxConfigs().size());
    fxOptionExpiries_.resize(config->fxConfigs().size());
    fxOptionCalibrationErrors_.resize(config->fxConfigs().size());
    eqOptionBaskets_.resize(config->eqConfigs().size());
    eqOptionExpiries_.resize(config->eqConfigs().size());
    eqOptionCalibrationErrors_.resize(config->eqConfigs().size());
    infCapFloorBaskets_.resize(config->infConfigs().size());
    infCapFloorExpiries_.resize(config->infConfigs().size());
    infCapFloorCalibrationErrors_.resize(config->infConfigs().size());

    /*******************************************************
     * Build the IR parametrizations and calibration baskets
     */
    std::vector<boost::shared_ptr<QuantExt::IrLgm1fParametrization>> irParametrizations;
    std::vector<RelinkableHandle<YieldTermStructure>> irDiscountCurves;
    std::vector<std::string> currencies, regions, crNames, eqNames, infIndices;
    std::vector<boost::shared_ptr<LgmBuilder>> irBuilder;

    for (Size i = 0; i < config->irConfigs().size(); i++) {
        boost::shared_ptr<IrLgmData> ir = config->irConfigs()[i];
        LOG("IR Parametrization " << i << " ccy " << ir->ccy());
        boost::shared_ptr<LgmBuilder> builder =
            boost::make_shared<LgmBuilder>(market_, ir, configurationLgmCalibration_, config->bootstrapTolerance());
        irBuilder.push_back(builder);
        boost::shared_ptr<QuantExt::IrLgm1fParametrization> parametrization = builder->parametrization();
        swaptionBaskets_[i] = builder->swaptionBasket();
        currencies.push_back(ir->ccy());
        irParametrizations.push_back(parametrization);
        irDiscountCurves.push_back(builder->discountCurve());
    }

    QL_REQUIRE(irParametrizations.size() > 0, "missing IR parametrizations");

    QuantLib::Currency domesticCcy = irParametrizations[0]->currency();

    /*******************************************************
     * Build the FX parametrizations and calibration baskets
     */
    std::vector<boost::shared_ptr<QuantExt::FxBsParametrization>> fxParametrizations;
    for (Size i = 0; i < config->fxConfigs().size(); i++) {
        LOG("FX Parametrization " << i);
        boost::shared_ptr<FxBsData> fx = config->fxConfigs()[i];
        QuantLib::Currency ccy = ore::data::parseCurrency(fx->foreignCcy());
        QuantLib::Currency domCcy = ore::data::parseCurrency(fx->domesticCcy());

        QL_REQUIRE(ccy.code() == irParametrizations[i + 1]->currency().code(),
                   "FX parametrization currency[" << i << "]=" << ccy << " does not match IR currrency[" << i + 1
                                                  << "]=" << irParametrizations[i + 1]->currency().code());

        QL_REQUIRE(domCcy == domesticCcy, "FX parametrization [" << i << "]=" << ccy << "/" << domCcy
                                                                 << " does not match domestic ccy " << domesticCcy);

        boost::shared_ptr<FxBsBuilder> builder =
            boost::make_shared<FxBsBuilder>(market_, fx, configurationFxCalibration_);
        boost::shared_ptr<QuantExt::FxBsParametrization> parametrization = builder->parametrization();

        fxOptionBaskets_[i] = builder->optionBasket();

        fxParametrizations.push_back(parametrization);
    }

    /*******************************************************
     * Build the EQ parametrizations and calibration baskets
     */
    std::vector<boost::shared_ptr<QuantExt::EqBsParametrization>> eqParametrizations;
    for (Size i = 0; i < config->eqConfigs().size(); i++) {
        LOG("EQ Parametrization " << i);
        boost::shared_ptr<EqBsData> eq = config->eqConfigs()[i];
        string eqName = eq->eqName();
        QuantLib::Currency eqCcy = ore::data::parseCurrency(eq->currency());
        QL_REQUIRE(std::find(currencies.begin(), currencies.end(), eqCcy.code()) != currencies.end(),
                   "Currency (" << eqCcy << ") for equity " << eqName << " not covered by CrossAssetModelData");
        boost::shared_ptr<EqBsBuilder> builder =
            boost::make_shared<EqBsBuilder>(market_, eq, domesticCcy, configurationEqCalibration_);
        boost::shared_ptr<QuantExt::EqBsParametrization> parametrization = builder->parametrization();
        eqOptionBaskets_[i] = builder->optionBasket();
        eqParametrizations.push_back(parametrization);
        eqNames.push_back(eqName);
    }

    /*******************************************************
     * Build the INF parametrizations and calibration baskets
     */
    std::vector<boost::shared_ptr<QuantExt::InfDkParametrization>> infParametrizations;
    for (Size i = 0; i < config->infConfigs().size(); i++) {
        LOG("INF Parametrization " << i);
        boost::shared_ptr<InfDkData> inf = config->infConfigs()[i];
        string infIndex = inf->infIndex();
        boost::shared_ptr<InfDkBuilder> builder =
            boost::make_shared<InfDkBuilder>(market_, inf, configurationInfCalibration_);
        boost::shared_ptr<QuantExt::InfDkParametrization> parametrization = builder->parametrization();
        infCapFloorBaskets_[i] = builder->optionBasket();
        infParametrizations.push_back(parametrization);
        infIndices.push_back(infIndex);
    }

    std::vector<boost::shared_ptr<QuantExt::Parametrization>> parametrizations;
    for (Size i = 0; i < irParametrizations.size(); i++)
        parametrizations.push_back(irParametrizations[i]);
    for (Size i = 0; i < fxParametrizations.size(); i++)
        parametrizations.push_back(fxParametrizations[i]);
    for (Size i = 0; i < eqParametrizations.size(); i++)
        parametrizations.push_back(eqParametrizations[i]);
    for (Size i = 0; i < infParametrizations.size(); i++)
        parametrizations.push_back(infParametrizations[i]);

    QL_REQUIRE(fxParametrizations.size() == irParametrizations.size() - 1, "mismatch in IR/FX parametrization sizes");

    /******************************
     * Build the correlation matrix
     */

    ore::data::CorrelationMatrixBuilder cmb;
    for (auto it = config->correlations().begin(); it != config->correlations().end(); it++) {
        std::string factor1 = it->first.first;
        std::string factor2 = it->first.second;
        Real corr = it->second;
        LOG("Add correlation for " << factor1 << " " << factor2);
        cmb.addCorrelation(factor1, factor2, corr);
    }

    LOG("Get correlation matrix for currencies:");
    for (auto c : currencies)
        LOG("Currency " << c);

    Matrix corrMatrix = cmb.correlationMatrix(currencies, infIndices, crNames, eqNames);

    /*****************************
     * Build the cross asset model
     */

    boost::shared_ptr<QuantExt::CrossAssetModel> model =
        boost::make_shared<QuantExt::CrossAssetModel>(parametrizations, corrMatrix);

    /*************************
     * Calibrate IR components
     */

    for (Size i = 0; i < irBuilder.size(); i++) {
        LOG("IR Calibration " << i);
        swaptionCalibrationErrors_[i] = irBuilder[i]->error();
    }

    /*************************
     * Relink LGM discount curves to curves used for FX calibration
     */

    for (Size i = 0; i < irParametrizations.size(); i++) {
        auto p = irParametrizations[i];
        irDiscountCurves[i].linkTo(*market_->discountCurve(p->currency().code(), configurationFxCalibration_));
        LOG("Relinked discounting curve for " << p->currency().code() << " for FX calibration");
    }

    /*************************
     * Calibrate FX components
     */

    for (Size i = 0; i < fxParametrizations.size(); i++) {
        boost::shared_ptr<FxBsData> fx = config->fxConfigs()[i];

        if (fx->calibrationType() == CalibrationType::None || !fx->calibrateSigma()) {
            LOG("FX Calibration " << i << " skipped");
            continue;
        }

        LOG("FX Calibration " << i);

        // attach pricing engines to helpers
        boost::shared_ptr<QuantExt::AnalyticCcLgmFxOptionEngine> engine =
            boost::make_shared<QuantExt::AnalyticCcLgmFxOptionEngine>(model, i);
        // enable caching for calibration
        // TODO: review this
        engine->cache(true);
        for (Size j = 0; j < fxOptionBaskets_[i].size(); j++)
            fxOptionBaskets_[i][j]->setPricingEngine(engine);

        if (fx->calibrationType() == CalibrationType::Bootstrap && fx->sigmaParamType() == ParamType::Piecewise)
            model->calibrateBsVolatilitiesIterative(CrossAssetModelTypes::FX, i, fxOptionBaskets_[i],
                                                    *optimizationMethod_, endCriteria_);
        else
            model->calibrateBsVolatilitiesGlobal(CrossAssetModelTypes::FX, i, fxOptionBaskets_[i], *optimizationMethod_,
                                                 endCriteria_);

        LOG("FX " << fx->foreignCcy() << " calibration errors:");
        fxOptionCalibrationErrors_[i] =
            logCalibrationErrors(fxOptionBaskets_[i], fxParametrizations[i], irParametrizations[0]);
        if (fx->calibrationType() == CalibrationType::Bootstrap) {
            QL_REQUIRE(fabs(fxOptionCalibrationErrors_[i]) < config->bootstrapTolerance(),
                       "calibration error " << fxOptionCalibrationErrors_[i] << " exceeds tolerance "
                                            << config->bootstrapTolerance());
        }
    }

    /*************************
     * Relink LGM discount curves to curves used for EQ calibration
     */

    for (Size i = 0; i < irParametrizations.size(); i++) {
        auto p = irParametrizations[i];
        irDiscountCurves[i].linkTo(*market_->discountCurve(p->currency().code(), configurationEqCalibration_));
        LOG("Relinked discounting curve for " << p->currency().code() << " for EQ calibration");
    }

    /*************************
     * Calibrate EQ components
     */

    for (Size i = 0; i < eqParametrizations.size(); i++) {
        boost::shared_ptr<EqBsData> eq = config->eqConfigs()[i];
        if (!eq->calibrateSigma()) {
            LOG("EQ Calibration " << i << " skipped");
            continue;
        }
        LOG("EQ Calibration " << i);
        // attach pricing engines to helpers
        Currency eqCcy = eqParametrizations[i]->currency();
        Size eqCcyIdx = model->ccyIndex(eqCcy);
        boost::shared_ptr<QuantExt::AnalyticXAssetLgmEquityOptionEngine> engine =
            boost::make_shared<QuantExt::AnalyticXAssetLgmEquityOptionEngine>(model, i, eqCcyIdx);
        for (Size j = 0; j < eqOptionBaskets_[i].size(); j++)
            eqOptionBaskets_[i][j]->setPricingEngine(engine);

        if (eq->calibrationType() == CalibrationType::Bootstrap && eq->sigmaParamType() == ParamType::Piecewise)
            model->calibrateBsVolatilitiesIterative(CrossAssetModelTypes::EQ, i, eqOptionBaskets_[i],
                                                    *optimizationMethod_, endCriteria_);
        else
            model->calibrateBsVolatilitiesGlobal(CrossAssetModelTypes::EQ, i, eqOptionBaskets_[i], *optimizationMethod_,
                                                 endCriteria_);

        LOG("EQ " << eq->eqName() << " calibration errors:");
        eqOptionCalibrationErrors_[i] =
            logCalibrationErrors(eqOptionBaskets_[i], eqParametrizations[i], irParametrizations[0]);
        if (eq->calibrationType() == CalibrationType::Bootstrap) {
            QL_REQUIRE(fabs(eqOptionCalibrationErrors_[i]) < config->bootstrapTolerance(),
                       "calibration error " << eqOptionCalibrationErrors_[i] << " exceeds tolerance "
                                            << config->bootstrapTolerance());
        }
    }

    /*************************
     * Relink LGM discount curves to curves used for INF calibration
     */

    for (Size i = 0; i < irParametrizations.size(); i++) {
        auto p = irParametrizations[i];
        irDiscountCurves[i].linkTo(*market_->discountCurve(p->currency().code(), configurationFinalModel_));
        LOG("Relinked discounting curve for " << p->currency().code() << " as final model curves");
    }

    /*************************
    * Calibrate INF components

    */

    for (Size i = 0; i < infParametrizations.size(); i++) {
        boost::shared_ptr<InfDkData> inf = config->infConfigs()[i];
        if ((!inf->calibrateA() && !inf->calibrateH()) || (inf->calibrationType() == CalibrationType::None)) {
            LOG("INF Calibration " << i << " skipped");
            continue;
        }
        LOG("INF Calibration " << i);
        // attach pricing engines to helpers

        Handle<ZeroInflationIndex> zInfIndex =
            market_->zeroInflationIndex(model->infdk(i)->name(), configurationInfCalibration_);
        Real baseCPI = zInfIndex->fixing(zInfIndex->zeroInflationTermStructure()->baseDate());

        boost::shared_ptr<QuantExt::AnalyticDkCpiCapFloorEngine> engine =
            boost::make_shared<QuantExt::AnalyticDkCpiCapFloorEngine>(model, i, baseCPI);
        for (Size j = 0; j < infCapFloorBaskets_[i].size(); j++)
            infCapFloorBaskets_[i][j]->setPricingEngine(engine);

        if (inf->calibrateA() && !inf->calibrateH()) {
            if (inf->calibrationType() == CalibrationType::Bootstrap && inf->aParamType() == ParamType::Piecewise) {
                model->calibrateInfDkVolatilitiesIterative(i, infCapFloorBaskets_[i], *optimizationMethod_,
                                                           endCriteria_);
            } else {
                model->calibrateInfDkVolatilitiesGlobal(i, infCapFloorBaskets_[i], *optimizationMethod_, endCriteria_);
            }
        } else if (!inf->calibrateA() && inf->calibrateH()) {
            if (inf->calibrationType() == CalibrationType::Bootstrap && inf->hParamType() == ParamType::Piecewise) {
                model->calibrateInfDkReversionsIterative(i, infCapFloorBaskets_[i], *optimizationMethod_, endCriteria_);
            } else {
                model->calibrateInfDkReversionsGlobal(i, infCapFloorBaskets_[i], *optimizationMethod_, endCriteria_);
            }
        } else {
            model->calibrate(infCapFloorBaskets_[i], *optimizationMethod_, endCriteria_);
        }

        LOG("INF " << inf->infIndex() << " calibration errors:");
        infCapFloorCalibrationErrors_[i] =
            logCalibrationErrors(infCapFloorBaskets_[i], infParametrizations[i], irParametrizations[0]);
        if (inf->calibrationType() == CalibrationType::Bootstrap) {
            QL_REQUIRE(fabs(infCapFloorCalibrationErrors_[i]) < config->bootstrapTolerance(),
                       "calibration error " << infCapFloorCalibrationErrors_[i] << " exceeds tolerance "
                                            << config->bootstrapTolerance());
        }
    }

    /*************************
     * Relink LGM discount curves to final model curves
     */

    for (Size i = 0; i < irParametrizations.size(); i++) {
        auto p = irParametrizations[i];
        irDiscountCurves[i].linkTo(*market_->discountCurve(p->currency().code(), configurationFinalModel_));
        LOG("Relinked discounting curve for " << p->currency().code() << " as final model curves");
    }

    // play safe (although the cache of the model should be empty at
    // this point from all what we know...)
    model->update();

    LOG("Building CrossAssetModel done");

    return model;
}
} // namespace data
} // namespace ore


#include <iostream>
#include <Eigen/Eigen>                        // for vector maths
#include <pangolin/pangolin.h>                // for open GL state management
#include <SceneGraph/SceneGraph.h>            // for open GL scene graph
#include "GetPot"                             // for command line parsing
#include "CVarHelpers.h"                      // for parsing Eigen vars as CVars
#include <cvars/CVar.h>                       // for glconsole
#include <ModelGraph/Models.h>            // Include the ModelGraph
#include "RenderClass.h"
#include "PhysicsClass.h"

using namespace std;
using namespace CVarUtils;
using namespace pangolin;
using namespace SceneGraph;

#define USAGE    \
"USAGE: Robot -n <name> -d <media directory>\n"\
"      Options:\n"\
"      --RobotName, -n        Name of this Robot process.\n"\
"      --MediaDir, -d         Directory where game media (meshes, maps, models etc.) are stored.\n"

class App
{
    public:
        ///////////////////////////////////////////////////////////////////
        SceneGraph::GLShadowLight   m_light;
        SceneGraph::GLBox           m_ground;
        SceneGraph::GLGrid          m_grid;
        SceneGraph::GLSceneGraph&   m_rSceneGraph;
        std::string                 m_sRobotName;
        SceneGraph::GLMesh          m_RobotMesh;
        std::string                 m_sRobotMeshFilename;
        std::string                 m_sMediaDir; // path to media files, meshes, etc
        Model                       m_RobotModel;// focus of our example app
        Phys*                       m_Phys;
        Render                      m_Render;

        // TODO TODO
        // rpg::CameraDevice           cam;
        //rpg::CarDevice              car;

        ///////////////////////////////////////////////////////////////////
        App(
                SceneGraph::GLSceneGraph& glGraph,  //< Input: reference to glGraph
                const std::string& sRobotName,      //< Input: name of robot proxy
                const std::string& sMediaDir        //< Input: location of meshes, models, maps etc
                )
            : m_rSceneGraph( glGraph )/*, m_pSim(pSim)*/
        {
            // look in the given media dir for the robot models/meshes
            m_sMediaDir = sMediaDir;
            m_sRobotMeshFilename = m_sMediaDir+"/model.blend";
            m_sRobotName = sRobotName;
            BuildRobot();
            PrintRobotGraph( m_RobotModel );
            Eigen::Vector6d RobotPose;
            RobotPose << 0, 0, -10, 0, 0, 0;
//            BuildRenderGraph(m_RobotModel, RobotPose);
            m_Phys = new Phys;
            m_Phys->Init();
            AssociatePhysicsBodies( m_RobotModel, RobotPose );
            AssociatePhysicsJoints( m_RobotModel, RobotPose );
        }

        ///////////////////////////////////////////////////////////////////
        void BuildRobot( void )
        {
            //  First enter the root node to build off of
//            BoxShape box = BoxShape(1,1,1);
//            Body* pChassis = new Body( std::string("Anchor"), box, 0 );
//            pChassis->SetPose( 0,0,0,0,0,0 );
//            m_RobotModel.SetName("SlideTest");
//            m_RobotModel.m_pParent = NULL;
//            m_RobotModel.SetBase( pChassis );

//            Body* pMass = new Body( std::string("Bob"), box, 1 );
//            pMass->SetPose( 0,0,4,0,0,0 );

            Eigen::Vector6d Ta, Tb;

            Ta << 0, 0, 13.5, 0, M_PI / 2, 0;
            Tb << 0, 0, 13.5, 0, M_PI / 2, 0;

            BoxShape box = BoxShape(2,1,3);
            Body* pChassis = new Body( std::string("Chassis"), box, 1.0f );
            pChassis->SetPose( 0,0,0,0,0,0 );
            m_RobotModel.SetName("Robot");
            m_RobotModel.m_pParent = NULL;
            m_RobotModel.SetBase( pChassis ); // main body

            box = BoxShape(1,1,1.5);
            Body* pUpperRArm = new Body( "UpperRArm", box, 1 );
            pUpperRArm->SetPose( 3,0,-1.5, 0,0, 2);

            box = BoxShape(1,1,1.5);
            Body* pUpperLArm = new Body( "UpperLArm", box, 1 );
            pUpperLArm->SetPose( -3,0,-1.5, 0,0, -2 );

            box = BoxShape(1,1,1);
            Body* pHead = new Body( "Head", box, 1 );
            pHead->SetPose( 0,0,-5, 0,0, 0 );

            CylinderShape cylinder = CylinderShape(1, 2);
            Body* pRWheel = new Body("RWheel", cylinder, 1);
            pRWheel->SetPose(2.5, 0, 2.8, 0, M_PI / 2, 0);

            Body* pLWheel = new Body("LWheel", cylinder, 1);
            pLWheel->SetPose(-2.5, 0, 2.8, 0, M_PI / 2, 0);

            box = BoxShape(0.5, 0.5, 0.5);
            Body* pRHand = new Body("RHand", box, 1);
            pRHand->SetPose(0, 0, 2.5, 0, 0, 0);

            box = BoxShape(0.5, 0.5, 0.5);
            Body* pLHand = new Body("LHand", box, 1);
            pLHand->SetPose(0, 0, 2.5, 0, 0, 0);

            // create a joint connecting two bodies
            HingeJoint* pRHinge = new HingeJoint( "RArmJoint", pChassis, pUpperRArm, 2, 0, -2.8, 1,0,0 );
            HingeJoint* pLHinge = new HingeJoint( "LArmJoint", pChassis, pUpperLArm, -2, 0, -2.8, 1,0,0 );
            HingeJoint* pRAxle = new HingeJoint( "RAxleJoint", pChassis, pRWheel, 2.5, 0, 2.8, 1,0,0, 0, -1, 0 );
            HingeJoint* pLAxle = new HingeJoint( "LAxleJoint", pChassis, pLWheel, -2.5, 0, 2.8, 1,0,0, 0, -1, 0 );

            Eigen::Vector3d axis1;
            Eigen::Vector3d axis2;
            Eigen::Vector3d anchor;
            Eigen::Vector3d LowerLinearLimit;
            Eigen::Vector3d UpperLinearLimit;
            Eigen::Vector3d LowerAngleLimit;
            Eigen::Vector3d UpperAngleLimit;
            anchor << 0, 0, -13.5;
            LowerLinearLimit << 0, 0, -3;
            UpperLinearLimit << 0, 0, -5;
            LowerAngleLimit << 0, 0, 0;
            UpperAngleLimit << 1, 0, 0;

            SliderJoint* pSlider = new SliderJoint( "Neck", pChassis, pHead, Ta, Tb, true);
            pSlider->setLowerLimit(5);
            pSlider->setUpperLimit(6);
//            Hinge2Joint* pNeck = new Hinge2Joint( "Neck", pChassis, pHead, axis1, axis2, anchor, 0.2, 0.2, LowerLinearLimit, UpperLinearLimit, LowerAngleLimit, UpperAngleLimit);

            axis1 << 0, 0, 2.5;
            axis2 << 0, 0, 1;
            Point2Point* pP2P = new Point2Point("RWrist", pUpperRArm, pRHand, axis1, axis2);

            Ta << 0, 0, 2.5, 0, 0, 0;
            Tb << 0, 0, 0, 0, 0, 0;
//            ConeTwist* pCT = new ConeTwist("LWrist", pUpperLArm, pLHand, Ta, Tb, false);
            GenericJoint* pGJ = new GenericJoint("LWrist", pUpperLArm, pLHand, Ta, Tb, false);
        }

        ///////////////////////////////////////////////////////////////////
        void AssociatePhysicsBodies( ModelNode& item, Eigen::Vector6d WorldPose )
        {
            if (dynamic_cast<Body*>(&item))
                m_Phys->RegisterObject( &item, item.GetName(), WorldPose);
            Eigen::Vector6d ChildWorldPose;
            for ( int count = 0; count < item.NumChildren(); count++ ) {
                ChildWorldPose = _T2Cart(_Cart2T(WorldPose)*(item.m_vChildren[count]->GetPoseMatrix()));
                AssociatePhysicsBodies(*(item.m_vChildren[count]), ChildWorldPose);
            }
        }

        ///////////////////////////////////////////////////////////////////
        void AssociatePhysicsJoints( ModelNode& item, Eigen::Vector6d WorldPose )
        {
            if (dynamic_cast<Joint*>(&item))
                m_Phys->RegisterObject( &item, item.GetName(), WorldPose);
            Eigen::Vector6d ChildWorldPose;
            for ( int count = 0; count < item.NumChildren(); count++ ) {
                ChildWorldPose = _T2Cart(_Cart2T(WorldPose)*(item.m_vChildren[count]->GetPoseMatrix()));
                AssociatePhysicsJoints(*(item.m_vChildren[count]), ChildWorldPose);
            }
        }

        ///////////////////////////////////////////////////////////////////
        void BuildRenderGraph( ModelNode& item, Eigen::Vector6d WorldPose )
        {
            m_Render.AddNode(&item, WorldPose);

            Eigen::Vector6d ChildWorldPose;
            for ( int count = 0; count < item.NumChildren(); count++ ) {
                ChildWorldPose = _T2Cart(_Cart2T(WorldPose)*(item.m_vChildren[count]->GetPoseMatrix()));
                BuildRenderGraph(*(item.m_vChildren[count]), ChildWorldPose);
            }
        }

        ///////////////////////////////////////////////////////////////////
        void PrintRobotGraph( ModelNode& mn )
        {
            if (mn.m_pParent == NULL)
                std::cout<<"I am "<<mn.GetName()<<" and I have "<<mn.NumChildren()<<" child(ren)."<<std::endl;
            else{
                std::cout<<"I am "<<mn.GetName()<<".  My parent is "<<mn.m_pParent->GetName()<<" and I have "<<mn.NumChildren()<<" child(ren)."<<std::endl;

                if (dynamic_cast<Body*>(&mn) != NULL){
                    Body* pBody = (Body*)(&mn);
                    if (dynamic_cast<BoxShape*>(pBody->m_RenderShape) != NULL)
                    {
                        std::cout<<"I am a Box"<<std::endl;
                    }
                    else if (dynamic_cast<CylinderShape*>(pBody->m_RenderShape) != NULL)
                    {
                        std::cout<<"I am a Cylinder"<<std::endl;
                    }
                    std::cout<<"I am a Body"<<std::endl;
                }else if (HingeJoint* test = dynamic_cast<HingeJoint*>(&mn)) {
                    std::cout<<"I am a HingeJoint"<<std::endl;
                } else if (HingeJoint* test = dynamic_cast<HingeJoint*>(&mn)) {
                    std::cout<<"I am a Hinge2Joint"<<std::endl;
                }
                else{
                    std::cout << "I don't seem to exist." << std::endl;
                }
            }

            for ( int count = 0; count < mn.NumChildren(); count++ ) {
                PrintRobotGraph(*(mn.m_vChildren[count]));
            }
        }

        ///////////////////////////////////////////////////////////////////
        void LeftKey()
        {
            Eigen::Matrix4d Tab = GLCart2T( 0, 0, 0, 0, 0, -0.1 );
            Eigen::Matrix4d Twa = GLCart2T( m_RobotMesh.GetPose() );
            Eigen::Matrix4d Twb = Twa*Tab;
            m_RobotMesh.SetPose( GLT2Cart(Twb) );
        }

        ///////////////////////////////////////////////////////////////////
        void RightKey()
        {
            Eigen::Matrix4d Tab = GLCart2T( 0, 0, 0, 0, 0, 0.1 );
            Eigen::Matrix4d Twa = GLCart2T( m_RobotMesh.GetPose() );
            Eigen::Matrix4d Twb = Twa*Tab;
            m_RobotMesh.SetPose( GLT2Cart(Twb) );
        }

        ///////////////////////////////////////////////////////////////////
        void ForwardKey()
        {
            Eigen::Matrix4d Tab = GLCart2T( 0.1, 0, 0, 0, 0, 0 );
            Eigen::Matrix4d Twa = GLCart2T( m_RobotMesh.GetPose() );
            Eigen::Matrix4d Twb = Twa*Tab;
            m_RobotMesh.SetPose( GLT2Cart(Twb) );
        }

        ///////////////////////////////////////////////////////////////////
        void ReverseKey()
        {
            Eigen::Matrix4d Tab = GLCart2T( -0.1, 0, 0, 0, 0, 0 );
            Eigen::Matrix4d Twa = GLCart2T( m_RobotMesh.GetPose() );
            Eigen::Matrix4d Twb = Twa*Tab;
            m_RobotMesh.SetPose( GLT2Cart(Twb) );
        }

        ///////////////////////////////////////////////////////////////////
        /// Re-allocate the simulator each time
        void InitReset()
        {
            m_rSceneGraph.Clear();
            m_Render.AddToScene( &m_rSceneGraph );

//            Eigen::Vector6d RobotPose;
//            RobotPose << 0, 0, -8, 0, 0, 0;
//            BuildRenderGraph(m_RobotModel, RobotPose);
//            if (m_Phys)
//            {
//                delete m_Phys;
//            }
//            m_Phys = new Phys;
//            m_Phys->Init();
//            AssociatePhysicsBodies( m_RobotModel, RobotPose );
//            AssociatePhysicsJoints( m_RobotModel, RobotPose );

            m_light.SetPosition( 10,10,-100 );
            m_rSceneGraph.AddChild( &m_light );

            m_grid.SetNumLines(20);
            m_grid.SetLineSpacing(1);
            m_rSceneGraph.AddChild(&m_grid);

            double dThickness = 1;
            m_ground.SetPose( 0,0, dThickness/2.0,0,0,0 );
            m_ground.SetExtent( 10,10, dThickness );

            BoxShape bs = BoxShape(10, 10, 1/2.0f);
            Body* ground = new Body("Ground", bs);
            ground->m_dMass = 0;
            ground->SetWPose( m_ground.GetPose4x4_po() );
            m_Phys->RegisterObject(ground, "Ground", m_ground.GetPose());

            Eigen::Vector6d InitPose;
            InitPose.setZero(6, 1);

            m_light.AddShadowReceiver( &m_ground );

            m_Render.UpdateScene();

        }

        void StepForward( void )
        {
            m_Phys->StepSimulation();
            m_Render.UpdateScene();
        }
};

///////////////////////////////////////////////////////////////////
int main( int argc, char** argv )
{
    // parse command line arguments
    GetPot cl( argc, argv );
    std::string sRobotName = cl.follow( "Bender", 2, "--RobotName", "-n" );
    std::string sMediaDir = cl.follow( "./", 2, "--MediaDir", "-d" ); // md specifies the media dir
//    if( argc != 5 ){
//        puts(USAGE);
//        return -1;
//    }

    // Create OpenGL window in single line thanks to GLUT
    pangolin::CreateWindowAndBind("Main",640,480);
    SceneGraph::GLSceneGraph::ApplyPreferredGlSettings();
    glClearColor(0, 0, 0, 0);
    glewInit();

    // sinle application context holds everything
    SceneGraph::GLSceneGraph  glGraph;
    App app( glGraph, sRobotName, sMediaDir/*, pSim*/ ); // initialize exactly one RobotProxy
    app.InitReset(); // this will populate the scene graph with objects and
    // register these objects with the simulator.

    //////////////////////////////////////////////
    // <Pangolin boilerplate>
    const SceneGraph::AxisAlignedBoundingBox bbox = glGraph.ObjectAndChildrenBounds();
    const Eigen::Vector3d center = bbox.Center();
    const double size = bbox.Size().norm();
    const double far = 10*size;
    const double near = far / 1E3;

    // Define Camera Render Object (for view / scene browsing)
    pangolin::OpenGlRenderState stacks3d(
            pangolin::ProjectionMatrix(640,480,420,420,320,240,near,far),
            pangolin::ModelViewLookAt(center(0), center(1) + size, center(2) - size/4,
                center(0), center(1), center(2), pangolin::AxisNegZ) );

    // We define a new view which will reside within the container.
    pangolin::View view3d;

    // We set the views location on screen and add a handler which will
    // let user input update the model_view matrix (stacks3d) and feed through
    // to our scenegraph
    view3d.SetBounds( 0.0, 1.0, 0.0, 1.0, -640.0f/480.0f );
    view3d.SetHandler( new SceneGraph::HandlerSceneGraph( glGraph, stacks3d) );
    view3d.SetDrawFunction( SceneGraph::ActivateDrawFunctor( glGraph, stacks3d) );

    // Add our views as children to the base container.
    pangolin::DisplayBase().AddDisplay( view3d );

    // register a keyboard hook to trigger the reset method
    pangolin::RegisterKeyPressCallback( pangolin::PANGO_CTRL + 'r',
            std::bind( &App::InitReset, &app ) );

    // simple asdw control
//    RegisterKeyPressCallback( 'a', bind( &Robot::LeftKey, &app ) );
//    RegisterKeyPressCallback( 'A', bind( &Robot::LeftKey, &app ) );

//    RegisterKeyPressCallback( 's', bind( &Robot::ReverseKey, &app ) );
//    RegisterKeyPressCallback( 'S', bind( &Robot::ReverseKey, &app ) );

//    RegisterKeyPressCallback( 'd', bind( &Robot::RightKey, &app ) );
//    RegisterKeyPressCallback( 'D', bind( &Robot::RightKey, &app ) );

//    RegisterKeyPressCallback( 'w', bind( &Robot::ForwardKey, &app ) );
//    RegisterKeyPressCallback( 'W', bind( &Robot::ForwardKey, &app ) );

    RegisterKeyPressCallback( ' ', bind( &App::StepForward, &app ) );
    RegisterKeyPressCallback( PANGO_CTRL + 'r', bind( &App::InitReset, &app ) );

    // </Pangolin boilerplate>
    //////////////////////////////////////////////

    // Default hooks for exiting (Esc) and fullscreen (tab).
    while( !pangolin::ShouldQuit() ) {

        glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );
        GLColor(1, 1, 1, 1);

        DisplayBase().ActivateScissorAndClear();
        // 1) SENSE: Read from robot sensors

        // 2) PLAN:  do stuff

        // 3) ACT: Send commands to the robot

        // optionally, draw info for a human to look at

        app.m_Phys->DebugDrawWorld();
//        app.m_Phys->StepSimulation();
//        app.m_Render.UpdateScene();

        // loop through robot graph and for each body update the associated GL object pose

        pangolin::FinishFrame();
    }

    return 0;
}


#include <stan/math/rev.hpp>
#include <boost/mp11.hpp>
#include <gtest/gtest.h>
#include <test/unit/math/rev/functor/test_fixture_ode.hpp>
#include <test/unit/math/rev/functor/test_fixture_ode_lorenz.hpp>
#include <stan/math/torsten/test/unit/dsolve/ode_test_functors.hpp>

/**
 *
 * Use same solver functor type for both w & w/o tolerance control
 */
template <typename solve_type, typename... Ts>
using ode_test_tuple = std::tuple<solve_type, solve_type, Ts...>;

/**
 * Outer product of test types
 */
using lorenz_test_types = boost::mp11::mp_product<
    ode_test_tuple,
    ::testing::Types<pmx_ode_adams_functor, pmx_ode_bdf_functor, pmx_ode_ckrk_functor,
                     pmx_ode_rk45_functor>>;

TYPED_TEST_SUITE_P(lorenz_test);
TYPED_TEST_P(lorenz_test, param_and_data_finite_diff) {
  if (std::is_same<TypeParam,
                   std::tuple<pmx_ode_rk45_functor, pmx_ode_rk45_functor>>::value) {
    this->test_fd_vd(1.e-6, 3e-2);
    this->test_fd_dv(1.e-6, 3e-2);
    this->test_fd_vv(1.e-6, 3e-2);
  } else if (std::is_same<TypeParam, std::tuple<pmx_ode_ckrk_functor,
                                                pmx_ode_ckrk_functor>>::value) {
    this->test_fd_vd(1.e-6, 5e-2);
    this->test_fd_dv(1.e-6, 5e-2);
    this->test_fd_vv(1.e-6, 5e-2);
  } else {
    this->test_fd_vd(1.e-6, 1e-2);
    this->test_fd_dv(1.e-6, 1e-2);
    this->test_fd_vv(1.e-6, 1e-2);
  }
}
REGISTER_TYPED_TEST_SUITE_P(lorenz_test, param_and_data_finite_diff);
INSTANTIATE_TYPED_TEST_SUITE_P(StanOde, lorenz_test, lorenz_test_types);


// Boost.Geometry Index
//
// R-tree strategies
//
// Copyright (c) 2019, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle
//
// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_INDEX_HPP
#define BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_INDEX_HPP

#include <boost/geometry/strategies/geographic/distance.hpp>
#include <boost/geometry/strategies/geographic/distance_andoyer.hpp> // backward compatibility
#include <boost/geometry/strategies/geographic/distance_cross_track.hpp>
#include <boost/geometry/strategies/geographic/distance_cross_track_point_box.hpp>
#include <boost/geometry/strategies/geographic/distance_segment_box.hpp>
#include <boost/geometry/strategies/geographic/distance_thomas.hpp> // backward compatibility
#include <boost/geometry/strategies/geographic/distance_vincenty.hpp> // backward compatibility
#include <boost/geometry/strategies/geographic/envelope_segment.hpp>
#include <boost/geometry/strategies/geographic/expand_segment.hpp>
#include <boost/geometry/strategies/geographic/intersection.hpp>
#include <boost/geometry/strategies/geographic/point_in_poly_winding.hpp>

#include <boost/geometry/strategies/spherical/index.hpp>

namespace boost { namespace geometry { namespace strategy { namespace index
{

template
<
    typename FormulaPolicy = strategy::andoyer,
    typename Spheroid = geometry::srs::spheroid<double>,
    typename CalculationType = void
>
struct geographic
    : spherical<CalculationType>
{
    typedef geographic_tag cs_tag;

    typedef geometry::strategy::envelope::geographic_segment
        <
            FormulaPolicy, Spheroid, CalculationType
        > envelope_segment_strategy_type;

    inline envelope_segment_strategy_type get_envelope_segment_strategy() const
    {
        return envelope_segment_strategy_type(m_spheroid);
    }

    typedef geometry::strategy::expand::geographic_segment
        <
            FormulaPolicy, Spheroid, CalculationType
        > expand_segment_strategy_type;

    inline expand_segment_strategy_type get_expand_segment_strategy() const
    {
        return expand_segment_strategy_type(m_spheroid);
    }

    // used in equals(Seg, Seg) but only to get_point_in_point_strategy()
    typedef geometry::strategy::intersection::geographic_segments
        <
            FormulaPolicy,
            // If index::geographic formula is derived from intersection::geographic_segments
            // formula with different Order this may cause an inconsistency
            strategy::default_order<FormulaPolicy>::value,
            Spheroid,
            CalculationType
        > relate_segment_segment_strategy_type;

    inline relate_segment_segment_strategy_type get_relate_segment_segment_strategy() const
    {
        return relate_segment_segment_strategy_type(m_spheroid);
    }

	typedef geometry::strategy::distance::geographic
        <
            FormulaPolicy, Spheroid, CalculationType
        > comparable_distance_point_point_strategy_type;

    inline comparable_distance_point_point_strategy_type get_comparable_distance_point_point_strategy() const
    {
        return comparable_distance_point_point_strategy_type(m_spheroid);
    }

    typedef geometry::strategy::distance::geographic_cross_track_point_box
        <
            FormulaPolicy, Spheroid, CalculationType
        > comparable_distance_point_box_strategy_type;

    inline comparable_distance_point_box_strategy_type get_comparable_distance_point_box_strategy() const
    {
        return comparable_distance_point_box_strategy_type(m_spheroid);
    }

    typedef geometry::strategy::distance::geographic_cross_track
        <
            FormulaPolicy, Spheroid, CalculationType
        > comparable_distance_point_segment_strategy_type;

    inline comparable_distance_point_segment_strategy_type get_comparable_distance_point_segment_strategy() const
    {
        return comparable_distance_point_segment_strategy_type(m_spheroid);
    }

    typedef geometry::strategy::distance::geographic_segment_box
        <
            FormulaPolicy, Spheroid, CalculationType
        > comparable_distance_segment_box_strategy_type;

    inline comparable_distance_segment_box_strategy_type get_comparable_distance_segment_box_strategy() const
    {
        return comparable_distance_segment_box_strategy_type(m_spheroid);
    }

    geographic()
        : m_spheroid()
    {}

    explicit geographic(Spheroid const& spheroid)
        : m_spheroid(spheroid)
    {}

public:
    Spheroid m_spheroid;
};

namespace services
{

template <typename Geometry>
struct default_strategy<Geometry, geographic_tag>
{
    typedef geographic<> type;
};

// within and relate (MPt, Mls/MPoly)
template <typename Point1, typename Point2, typename Formula, typename Spheroid, typename CalculationType>
struct from_strategy<within::geographic_winding<Point1, Point2, Formula, Spheroid, CalculationType> >
{
    typedef strategy::index::geographic<Formula, Spheroid, CalculationType> type;

    static inline type get(within::geographic_winding<Point1, Point2, Formula, Spheroid, CalculationType> const& strategy)
    {
        return type(strategy.model());
    }
};

// distance (MPt, MPt)
template <typename Formula, typename Spheroid, typename CalculationType>
struct from_strategy<distance::geographic<Formula, Spheroid, CalculationType> >
{
    typedef strategy::index::geographic<Formula, Spheroid, CalculationType> type;

    static inline type get(distance::geographic<Formula, Spheroid, CalculationType> const& strategy)
    {
        return type(strategy.model());
    }
};
template <typename Spheroid, typename CalculationType>
struct from_strategy<distance::andoyer<Spheroid, CalculationType> >
{
    typedef strategy::index::geographic<strategy::andoyer, Spheroid, CalculationType> type;

    static inline type get(distance::andoyer<Spheroid, CalculationType> const& strategy)
    {
        return type(strategy.model());
    }
};
template <typename Spheroid, typename CalculationType>
struct from_strategy<distance::thomas<Spheroid, CalculationType> >
{
    typedef strategy::index::geographic<strategy::thomas, Spheroid, CalculationType> type;

    static inline type get(distance::thomas<Spheroid, CalculationType> const& strategy)
    {
        return type(strategy.model());
    }
};
template <typename Spheroid, typename CalculationType>
struct from_strategy<distance::vincenty<Spheroid, CalculationType> >
{
    typedef strategy::index::geographic<strategy::vincenty, Spheroid, CalculationType> type;

    static inline type get(distance::vincenty<Spheroid, CalculationType> const& strategy)
    {
        return type(strategy.model());
    }
};

// distance (MPt, Linear/Areal)
template <typename Formula, typename Spheroid, typename CalculationType>
struct from_strategy<distance::geographic_cross_track<Formula, Spheroid, CalculationType> >
{
    typedef strategy::index::geographic<Formula, Spheroid, CalculationType> type;

    static inline type get(distance::geographic_cross_track<Formula, Spheroid, CalculationType> const& strategy)
    {
        return type(strategy.model());
    }
};

} // namespace services

}}}} // namespace boost::geometry::strategy::index

#endif // BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_INDEX_HPP