// Copyright (C) 2011 by the BEM++ Authors
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.

#include "bempp/common/config_trilinos.hpp"

#ifdef WITH_TRILINOS

#include "../type_template.hpp"
#include "../check_arrays_are_close.hpp"
#include "../random_arrays.hpp"

#include "assembly/discrete_dense_boundary_operator.hpp"
#include "common/scalar_traits.hpp"
#include "linalg/belos_solver_wrapper.hpp"

#include "common/armadillo_fwd.hpp"
#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/type_traits/is_complex.hpp>
#include <boost/utility/enable_if.hpp>
#include <boost/version.hpp>

#include <Thyra_DefaultSpmdVector.hpp>
#include <Thyra_DefaultSpmdVectorSpace_decl.hpp>
#include <Thyra_DefaultPreconditioner.hpp>

// Tests

BOOST_AUTO_TEST_SUITE(BelosSolverWrapper)

BOOST_AUTO_TEST_CASE_TEMPLATE(solve_works_for_single_rhs, ValueType, result_types)
{
    std::srand(1);

    const int size = 50;
    arma::Mat<ValueType> mat = arma::eye<arma::Mat<ValueType> >(size, size) +
        0.1 * generateRandomMatrix<ValueType>(size, size);
    Bempp::DiscreteDenseBoundaryOperator<ValueType> op(mat);

    arma::Col<ValueType> rhs = generateRandomMatrix<ValueType>(size, 1);
    arma::Col<ValueType> sol(size);
    sol.fill(static_cast<ValueType>(0.));

    typedef Thyra::DefaultSpmdVector<ValueType> DenseVector;
    Teuchos::ArrayRCP<ValueType> rhsArray =
            Teuchos::arcp(rhs.memptr(), 0 /* lowerOffset */,
                          size, false /* doesn't own memory */);
    DenseVector rhsVector(Thyra::defaultSpmdVectorSpace<ValueType>(size),
                    rhsArray, 1 /* stride */);
    Teuchos::ArrayRCP<ValueType> solArray =
            Teuchos::arcp(sol.memptr(), 0 /* lowerOffset */,
                          size, false /* doesn't own memory */);
    DenseVector solVector(Thyra::defaultSpmdVectorSpace<ValueType>(size),
                    solArray, 1 /* stride */);

    typedef Bempp::BelosSolverWrapper<ValueType> Solver;
    Solver solver(Teuchos::rcpFromRef<const Thyra::LinearOpBase<ValueType> >(op));
    typedef typename Bempp::ScalarTraits<ValueType>::RealType MagnitudeType;
    const MagnitudeType tol = std::numeric_limits<MagnitudeType>::epsilon() * 1000.;
    solver.initializeSolver(Bempp::defaultGmresParameterList(tol));

    Thyra::SolveStatus<typename Solver::MagnitudeType > status =
            solver.solve(Thyra::NOTRANS, rhsVector,
                         Teuchos::ptr<Thyra::MultiVectorBase<ValueType> >(&solVector));
    BOOST_CHECK_EQUAL(status.solveStatus, Thyra::SOLVE_STATUS_CONVERGED);

    // Solve system with armadillo

    arma::Col<ValueType> armaSol = arma::solve(mat, rhs);

    BOOST_CHECK(check_arrays_are_close<ValueType>(sol, armaSol, tol * 10));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(solve_with_trivial_right_preconditioner_works_for_single_rhs,
                              ValueType, result_types)
{
    std::srand(1);

    const int size = 50;
    arma::Mat<ValueType> mat = arma::eye<arma::Mat<ValueType> >(size, size) +
        0.1 * generateRandomMatrix<ValueType>(size, size);
    Bempp::DiscreteDenseBoundaryOperator<ValueType> op(mat);

    arma::Col<ValueType> rhs = generateRandomMatrix<ValueType>(size, 1);
    arma::Col<ValueType> sol(size);
    sol.fill(static_cast<ValueType>(0.));

    arma::Mat<ValueType> precMat;
    precMat.eye(size, size);
    Bempp::DiscreteDenseBoundaryOperator<ValueType> precOp(precMat);

    typedef Thyra::DefaultSpmdVector<ValueType> DenseVector;
    Teuchos::ArrayRCP<ValueType> rhsArray =
            Teuchos::arcp(rhs.memptr(), 0 /* lowerOffset */,
                          size, false /* doesn't own memory */);
    DenseVector rhsVector(Thyra::defaultSpmdVectorSpace<ValueType>(size),
                    rhsArray, 1 /* stride */);
    Teuchos::ArrayRCP<ValueType> solArray =
            Teuchos::arcp(sol.memptr(), 0 /* lowerOffset */,
                          size, false /* doesn't own memory */);
    DenseVector solVector(Thyra::defaultSpmdVectorSpace<ValueType>(size),
                    solArray, 1 /* stride */);

    Teuchos::RCP<const Thyra::DefaultPreconditioner<ValueType> > prec =
            Thyra::rightPrec(Teuchos::rcpFromRef<const Thyra::LinearOpBase<ValueType> >(precOp));

    typedef Bempp::BelosSolverWrapper<ValueType> Solver;
    Solver solver(Teuchos::rcpFromRef<const Thyra::LinearOpBase<ValueType> >(op));
    typedef typename Bempp::ScalarTraits<ValueType>::RealType MagnitudeType;
    const MagnitudeType tol = std::numeric_limits<MagnitudeType>::epsilon() * 100.;
    solver.setPreconditioner(prec);
    solver.initializeSolver(Bempp::defaultGmresParameterList(tol));

    Thyra::SolveStatus<typename Solver::MagnitudeType > status =
            solver.solve(Thyra::NOTRANS, rhsVector,
                         Teuchos::ptr<Thyra::MultiVectorBase<ValueType> >(&solVector));
    BOOST_CHECK_EQUAL(status.solveStatus, Thyra::SOLVE_STATUS_CONVERGED);

    // Solve system with armadillo

    arma::Col<ValueType> armaSol = arma::solve(mat, rhs);

    BOOST_CHECK(check_arrays_are_close<ValueType>(sol, armaSol, tol * 10));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(solve_with_scaled_trivial_right_preconditioner_works_for_single_rhs,
                              ValueType, result_types)
{
    std::srand(1);

    const int size = 50;
    arma::Mat<ValueType> mat = arma::eye<arma::Mat<ValueType> >(size, size) +
        0.1 * generateRandomMatrix<ValueType>(size, size);
    Bempp::DiscreteDenseBoundaryOperator<ValueType> op(mat);

    arma::Mat<ValueType> precMat;
    precMat.eye(size, size);
    Bempp::DiscreteDenseBoundaryOperator<ValueType> precOp(1e-16 * precMat);

    arma::Col<ValueType> rhs = generateRandomMatrix<ValueType>(size, 1);
    arma::Col<ValueType> sol(size);
    sol.fill(static_cast<ValueType>(0.));

    typedef Thyra::DefaultSpmdVector<ValueType> DenseVector;
    Teuchos::ArrayRCP<ValueType> rhsArray =
            Teuchos::arcp(rhs.memptr(), 0 /* lowerOffset */,
                          size, false /* doesn't own memory */);
    DenseVector rhsVector(Thyra::defaultSpmdVectorSpace<ValueType>(size),
                    rhsArray, 1 /* stride */);
    Teuchos::ArrayRCP<ValueType> solArray =
            Teuchos::arcp(sol.memptr(), 0 /* lowerOffset */,
                          size, false /* doesn't own memory */);
    DenseVector solVector(Thyra::defaultSpmdVectorSpace<ValueType>(size),
                    solArray, 1 /* stride */);

    Teuchos::RCP<const Thyra::DefaultPreconditioner<ValueType> > prec =
            Thyra::rightPrec(Teuchos::rcpFromRef<const Thyra::LinearOpBase<ValueType> >(precOp));

    typedef Bempp::BelosSolverWrapper<ValueType> Solver;
    Solver solver(Teuchos::rcpFromRef<const Thyra::LinearOpBase<ValueType> >(op));
    typedef typename Bempp::ScalarTraits<ValueType>::RealType MagnitudeType;
    const MagnitudeType tol = std::numeric_limits<MagnitudeType>::epsilon() * 1000.;
    solver.setPreconditioner(prec);
    solver.initializeSolver(Bempp::defaultGmresParameterList(tol));

    Thyra::SolveStatus<typename Solver::MagnitudeType > status =
            solver.solve(Thyra::NOTRANS, rhsVector,
                         Teuchos::ptr<Thyra::MultiVectorBase<ValueType> >(&solVector));
    BOOST_CHECK_EQUAL(status.solveStatus, Thyra::SOLVE_STATUS_CONVERGED);

    // Solve system with armadillo

    arma::Col<ValueType> armaSol = arma::solve(mat, rhs);

    BOOST_CHECK(check_arrays_are_close<ValueType>(sol, armaSol, tol * 10));
}

// BOOST_AUTO_TEST_CASE_TEMPLATE(solve_with_trivial_left_preconditioner_works_for_single_rhs,
//                               ValueType, result_types)
// {
//     std::srand(1);

//     const int size = 50;
//     arma::Mat<ValueType> mat = arma::eye<arma::Mat<ValueType> >(size, size) +
//         0.1 * generateRandomMatrix<ValueType>(size, size);
//     Bempp::DiscreteDenseBoundaryOperator<ValueType> op(mat);

//     arma::Mat<ValueType> precMat;
//     precMat.eye(size, size);
//     Bempp::DiscreteDenseBoundaryOperator<ValueType> precOp(precMat);

//     arma::Col<ValueType> rhs = generateRandomMatrix<ValueType>(size, 1);
//     arma::Col<ValueType> sol(size);
//     sol.fill(static_cast<ValueType>(0.));

//     typedef Thyra::DefaultSpmdVector<ValueType> DenseVector;
//     Teuchos::ArrayRCP<ValueType> rhsArray =
//             Teuchos::arcp(rhs.memptr(), 0 /* lowerOffset */,
//                           size, false /* doesn't own memory */);
//     DenseVector rhsVector(Thyra::defaultSpmdVectorSpace<ValueType>(size),
//                     rhsArray, 1 /* stride */);
//     Teuchos::ArrayRCP<ValueType> solArray =
//             Teuchos::arcp(sol.memptr(), 0 /* lowerOffset */,
//                           size, false /* doesn't own memory */);
//     DenseVector solVector(Thyra::defaultSpmdVectorSpace<ValueType>(size),
//                     solArray, 1 /* stride */);

//     Teuchos::RCP<const Thyra::DefaultPreconditioner<ValueType> > prec =
//             Thyra::leftPrec(Teuchos::rcpFromRef<const Thyra::LinearOpBase<ValueType> >(precOp));

//     typedef Bempp::BelosSolverWrapper<ValueType> Solver;
//     Solver solver(Teuchos::rcpFromRef<const Thyra::LinearOpBase<ValueType> >(op));
//     typedef typename Bempp::ScalarTraits<ValueType>::RealType MagnitudeType;
//     const MagnitudeType tol = std::numeric_limits<MagnitudeType>::epsilon() * 1000.;
//     solver.setPreconditioner(prec);
//     solver.initializeSolver(Bempp::defaultGmresParameterList(tol));

//     Thyra::SolveStatus<typename Solver::MagnitudeType > status =
//             solver.solve(Thyra::NOTRANS, rhsVector,
//                          Teuchos::ptr<Thyra::MultiVectorBase<ValueType> >(&solVector));
//     BOOST_CHECK_EQUAL(status.solveStatus, Thyra::SOLVE_STATUS_CONVERGED);

//     // Solve system with armadillo

//     arma::Col<ValueType> armaSol = arma::solve(mat, rhs);

//     BOOST_CHECK(check_arrays_are_close<ValueType>(sol, armaSol, tol * 10));
// }

// BOOST_AUTO_TEST_CASE_TEMPLATE(solve_with_scaled_trivial_left_preconditioner_works_for_single_rhs,
//                               ValueType, result_types)
// {
//     std::srand(1);

//     const int size = 50;
//     arma::Mat<ValueType> mat = arma::eye<arma::Mat<ValueType> >(size, size) +
//         0.1 * generateRandomMatrix<ValueType>(size, size);
//     Bempp::DiscreteDenseBoundaryOperator<ValueType> op(mat);

//     arma::Mat<ValueType> precMat;
//     precMat.eye(size, size);
//     Bempp::DiscreteDenseBoundaryOperator<ValueType> precOp(1e-16 * precMat);

//     arma::Col<ValueType> rhs = generateRandomMatrix<ValueType>(size, 1);
//     arma::Col<ValueType> sol(size);
//     sol.fill(static_cast<ValueType>(0.));

//     typedef Thyra::DefaultSpmdVector<ValueType> DenseVector;
//     Teuchos::ArrayRCP<ValueType> rhsArray =
//             Teuchos::arcp(rhs.memptr(), 0 /* lowerOffset */,
//                           size, false /* doesn't own memory */);
//     DenseVector rhsVector(Thyra::defaultSpmdVectorSpace<ValueType>(size),
//                     rhsArray, 1 /* stride */);
//     Teuchos::ArrayRCP<ValueType> solArray =
//             Teuchos::arcp(sol.memptr(), 0 /* lowerOffset */,
//                           size, false /* doesn't own memory */);
//     DenseVector solVector(Thyra::defaultSpmdVectorSpace<ValueType>(size),
//                     solArray, 1 /* stride */);

//     Teuchos::RCP<const Thyra::DefaultPreconditioner<ValueType> > prec =
//             Thyra::leftPrec(Teuchos::rcpFromRef<const Thyra::LinearOpBase<ValueType> >(precOp));

//     typedef Bempp::BelosSolverWrapper<ValueType> Solver;
//     Solver solver(Teuchos::rcpFromRef<const Thyra::LinearOpBase<ValueType> >(op));
//     typedef typename Bempp::ScalarTraits<ValueType>::RealType MagnitudeType;
//     const MagnitudeType tol = std::numeric_limits<MagnitudeType>::epsilon() * 1000.;
//     solver.setPreconditioner(prec);
//     solver.initializeSolver(Bempp::defaultGmresParameterList(tol));

//     Thyra::SolveStatus<typename Solver::MagnitudeType > status =
//             solver.solve(Thyra::NOTRANS, rhsVector,
//                          Teuchos::ptr<Thyra::MultiVectorBase<ValueType> >(&solVector));
//     BOOST_CHECK_EQUAL(status.solveStatus, Thyra::SOLVE_STATUS_CONVERGED);

//     // Solve system with armadillo

//     arma::Col<ValueType> armaSol = arma::solve(mat, rhs);

//     BOOST_CHECK(check_arrays_are_close<ValueType>(sol, armaSol, tol * 10));
// }

BOOST_AUTO_TEST_SUITE_END()

#endif


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

#include <boost/test/unit_test.hpp>

#include <chainparams.h>
#include <consensus/merkle.h>
#include <consensus/validation.h>
#include <miner.h>
#include <pow.h>
#include <random.h>
#include <test/test_veil.h>
#include <validation.h>
#include <validationinterface.h>
#include <veil/budget.h>
#include <script/standard.h>
#include <key_io.h>
#include <veil/zerocoin/accumulators.h>

BOOST_FIXTURE_TEST_SUITE(proofofstake_tests, BasicTestingSetup)

BOOST_AUTO_TEST_CASE(proofofstake_block)
{

    CMutableTransaction txCoinBase;
    txCoinBase.vin.resize(1);
    txCoinBase.vin[0].prevout.SetNull();
    txCoinBase.vin[0].scriptSig = CScript() << 1 << OP_0;
    txCoinBase.vpout.resize(1);
    txCoinBase.vpout[0] = MAKE_OUTPUT<CTxOutStandard>();

    CTxOut txOut_coinbase(0, CScript());
    txCoinBase.vpout.clear();
    txCoinBase.vpout.emplace_back(txOut_coinbase.GetSharedPtr());
//    txCoinBase.vpout[0]->SetValue(0);
//    txCoinBase.vpout[0]->SetScriptPubKey(CScript());
    CBlock block;
    block.vtx.emplace_back(MakeTransactionRef(std::move(txCoinBase)));
        block.vtx.resize(2);

    //load our serialized pubcoin
    libzerocoin::PrivateCoin privateCoin_stake(Params().Zerocoin_Params(), libzerocoin::CoinDenomination::ZQ_TEN, true);
    auto pubCoin = privateCoin_stake.getPublicCoin();
    BOOST_CHECK_MESSAGE(pubCoin.validate(), "Failed to validate pubCoin created from hex string");

    //initialize and Accumulator and AccumulatorWitness
    libzerocoin::Accumulator accumulator(Params().Zerocoin_Params(), libzerocoin::CoinDenomination::ZQ_TEN);
    libzerocoin::AccumulatorWitness witness(Params().Zerocoin_Params(), accumulator, pubCoin);

    //Add random zerocoins to the accumulator
    for (unsigned int i = 0; i < 5; i++) {
        libzerocoin::PrivateCoin pc(Params().Zerocoin_Params(), libzerocoin::CoinDenomination::ZQ_TEN, true);
        auto pub = pc.getPublicCoin();
        accumulator.accumulate(pub);
        witness.AddElement(pub);
    }

    //Add our own coin
    accumulator.accumulate(pubCoin);

    //Get the checksum of the accumulator we use for the spend and also add it to our checksum map
    auto hashChecksum = GetChecksum(accumulator.getValue());
    libzerocoin::CoinSpend spend(Params().Zerocoin_Params(), privateCoin_stake, accumulator, hashChecksum, witness, uint256(), libzerocoin::SpendType::STAKE);

    // Deserialize the CoinSpend intro a fresh object
    CDataStream serializedCoinSpend(SER_NETWORK, PROTOCOL_VERSION);
    serializedCoinSpend << spend;
    std::vector<unsigned char> data(serializedCoinSpend.begin(), serializedCoinSpend.end());

    CTxIn txin;
    //Add the coin spend into a transaction
    txin.scriptSig = CScript() << OP_ZEROCOINSPEND << data.size();
    txin.scriptSig.insert(txin.scriptSig.end(), data.begin(), data.end());
    txin.prevout.SetNull();

    //use nSequence as a shorthand lookup of denomination
    //NOTE that this should never be used in place of checking the value in the final blockchain acceptance/verification
    //of the transaction
    txin.nSequence = spend.getDenomination();
    txin.nSequence |= CTxIn::SEQUENCE_LOCKTIME_DISABLE_FLAG; //Don't use any relative locktime for zerocoin spend

    CMutableTransaction txCoinStake;
    txCoinStake.vin.emplace_back(txin);

    //Coinstake uses empty vout[0]
    txCoinStake.vpout.emplace_back(CTxOut(0, CScript()).GetSharedPtr());

    //Add mints to tx
    for (unsigned int i = 0; i < 5; i++) {
        libzerocoin::PrivateCoin pc(Params().Zerocoin_Params(), libzerocoin::CoinDenomination::ZQ_TEN, true);
        auto pub = pc.getPublicCoin();
        CScript scriptSerializedCoin = CScript() << OP_ZEROCOINMINT << pub.getValue().getvch().size() << pub.getValue().getvch();
        txCoinStake.vpout.emplace_back(CTxOut(libzerocoin::ZerocoinDenominationToAmount(pub.getDenomination()), scriptSerializedCoin).GetSharedPtr());
    }

    block.vtx[1] = MakeTransactionRef(std::move(txCoinStake));

    //This is done in incrementextranonce for PoW blocks
    block.hashMerkleRoot = BlockMerkleRoot(block);

    std::cout << "vpout size: "<<block.vtx[0]->vpout.size()<<"\n Block \n " << block.ToString() << std::endl;
    BOOST_CHECK_MESSAGE(block.IsProofOfStake(), "block is not marked as proof of stake");

    auto hashBlock = block.GetHash();
    CDataStream ss(SER_DISK, 0);
    ss << block;
    std::cout << "vpout size after serialize: "<<block.vtx[0]->vpout.size()<<"\n";
    CBlock block2;
    ss >> block2;

    BOOST_CHECK_MESSAGE(block2.GetHash() == hashBlock, "Block hash does not match after serialization");
    //BOOST_CHECK_MESSAGE(block2 == block, "Block changed after serialization");
    std::cout << "Block 2\n " << block2.ToString() << std::endl;

        BOOST_CHECK_MESSAGE(block.vtx[0]->GetHash() == block2.vtx[0]->GetHash(),
                            "Coinbase tx was malleated during serialization");
    try {
        auto txin1 = block.vtx[0]->vin[0];
        auto txin2 = block2.vtx[0]->vin[0];
        std::cout << "1\n";
        BOOST_CHECK_MESSAGE(txin1 == txin2, "Coinbase input was malleated during serialization");
        BOOST_CHECK_MESSAGE(txin1.scriptSig == txin2.scriptSig, "txin scriptsig mismatch");
        BOOST_CHECK_MESSAGE(txin1.prevout == txin2.prevout, "txin prevout mismatch");
        BOOST_CHECK_MESSAGE(txin1.nSequence == txin2.nSequence, "txin sequence mismatch");
        BOOST_CHECK_MESSAGE(txin1.scriptData.stack == txin2.scriptData.stack, "txin scriptdata mismatch");
        BOOST_CHECK_MESSAGE(txin1.scriptWitness.stack == txin2.scriptWitness.stack, "txin scriptwitness mismatch");

        std::cout << "2\n";

        CTxOut out1;
        block.vtx[0]->vpout[0]->GetTxOut(out1);
        CTxOut out2;
        block2.vtx[0]->vpout[0]->GetTxOut(out2);
        BOOST_CHECK_MESSAGE(out1 == out2, "Coinbase output was malleated during serialization");
    } catch (...) {
        BOOST_CHECK(false);
    }

    auto txCoinBase2 = block2.vtx[0];
    auto txCoinBase_block = block.vtx[0];
    BOOST_CHECK_MESSAGE(txCoinBase2->nLockTime == txCoinBase_block->nLockTime, "locktime incorrect");
    BOOST_CHECK_MESSAGE(txCoinBase2->nVersion == txCoinBase_block->nVersion, "version incorrect");
    BOOST_CHECK_MESSAGE(txCoinBase2->vin == txCoinBase_block->vin, "vin incorrect");
    BOOST_CHECK_MESSAGE(txCoinBase2->vpout.size() == txCoinBase_block->vpout.size(), "vpoutsize incorrect");
    std::cout << "vpoutsize2:"<< txCoinBase_block->vpout.size() << " vpoutsize1:" << txCoinBase_block->vpout.size() << std::endl;
    BOOST_CHECK_MESSAGE(txCoinBase2->HasWitness() == txCoinBase_block->HasWitness(), "witness flag incorrect");

}

BOOST_AUTO_TEST_SUITE_END()


#pragma once

#ifdef USE_FIBONACCI_HEAP
#include <boost/heap/fibonacci_heap.hpp>
#endif

#include <boost/heap/d_ary_heap.hpp>
#include <unordered_map>
#include <unordered_set>

#include "neighbor.hpp"
#include "planresult.hpp"

// #define REBUILT_FOCAL_LIST
// #define CHECK_FOCAL_LIST

namespace libMultiRobotPlanning {

/*!
  \example a_star_epsilon.cpp Simple example using a 2D grid world and
  up/down/left/right
  actions
*/

/*! \brief A*_epsilon Algorithm to find the shortest path with a given
suboptimality bound (also known as focal search)
This class implements the A*_epsilon algorithm, an informed search
algorithm
that finds the shortest path for a given map up to a suboptimality factor.
It uses an admissible heuristic (to keep track of the optimum) and an
inadmissible heuristic (
to guide the search within a suboptimal bound w.)
Details of the algorithm can be found in the following paper:\n
Judea Pearl, Jin H. Kim:\n
"Studies in Semi-Admissible Heuristics."" IEEE Trans. Pattern Anal. Mach.
Intell. 4(4): 392-399 (1982)\n
https://doi.org/10.1109/TPAMI.1982.4767270
This class can either use a fibonacci heap, or a d-ary heap. The latter is the
default. Define "USE_FIBONACCI_HEAP" to use the fibonacci heap instead.
\tparam State Custom state for the search. Needs to be copy'able
\tparam Action Custom action for the search. Needs to be copy'able
\tparam Cost Custom Cost type (integer or floating point types)
\tparam Environment This class needs to provide the custom logic. In
    particular, it needs to support the following functions:
  - `Cost admissibleHeuristic(const State& s)`\n
    This function can return 0 if no suitable heuristic is available.
  - `Cost focalStateHeuristic(const State& s, Cost gScore)`\n
    This function computes a (potentially inadmissible) heuristic for the given
state.
  - `Cost focalTransitionHeuristic(const State& s1, const State& s2, Cost
gScoreS1, Cost gScoreS2)`\n
    This function computes a (potentially inadmissible) heuristic for the given
state transition.
  - `bool isSolution(const State& s)`\n
    Return true if the given state is a goal state.
  - `void getNeighbors(const State& s, std::vector<Neighbor<State, Action,
   int> >& neighbors)`\n
    Fill the list of neighboring state for the given state s.
  - `void onExpandNode(const State& s, int fScore, int gScore)`\n
    This function is called on every expansion and can be used for statistical
purposes.
  - `void onDiscover(const State& s, int fScore, int gScore)`\n
    This function is called on every node discovery and can be used for
   statistical purposes.
    \tparam StateHasher A class to convert a state to a hash value. Default:
   std::hash<State>
*/
template <typename State, typename Action, typename Cost, typename Environment,
          typename StateHasher = std::hash<State> >
class IOS {
 public:
  IOS(Environment& environment, float w)
      : m_env(environment), m_w(w) {}

  bool search(const State& startState,
              PlanResult<State, Action, Cost>& solution) {
    solution.states.clear();
    solution.states.push_back(std::make_pair<>(startState, 0));
    solution.actions.clear();
    solution.cost = INT32_MAX;

    openSet_t openSet;
    focalSet_t
        focalSet;  // subset of open nodes that are within suboptimality bound
    anopenset_t anopen;
    std::unordered_map<State, fibHeapHandle_t, StateHasher> stateToHeap;
    std::unordered_map<State, fibHeapHandle_t, StateHasher> stateToFocal;
    std::unordered_set<State, StateHasher> closedSet;
    std::unordered_map<State, std::tuple<State, Action, Cost, Cost>,
                       StateHasher>
        cameFrom;

    auto handle = openSet.push(
        Node(startState, m_env.admissibleHeuristic(startState), 0, 0,0));
    stateToHeap.insert(std::make_pair<>(startState, handle));
    (*handle).handle = handle;

    anopen.push(handle);
    focalSet.push(handle);

    stateToFocal.insert(std::make_pair<>(startState, handle));

    std::vector<Neighbor<State, Action, Cost> > neighbors;
    neighbors.reserve(10);

    Cost bestFScore = (*handle).fScore;

    std::cout << "new search" << std::endl;

//my work
while(!openSet.empty()){

  //printf("In Open\n");

  const auto& nopen = openSet.top();
  //std:: cout << m_w <<std::endl;
  if(solution.cost <= m_w * nopen.fScore ){
        //printf("Found\n");
        return true;
  }
  //Node nopen = *nopen_handle;
  if(focalSet.empty()){
    printf("Empty\n");
    return false;
  }
  auto currentHandle = focalSet.top();
  Node current = *currentHandle;

  auto currentHandle_anop = anopen.top();
  Node current_anop = *currentHandle_anop;

  if(current.focalHeuristic < solution.cost){
    // Do greedy step

    //printf("greedy\n");

    focalSet.pop();
    // openSet.erase(currentHandle);
    stateToFocal.erase(current.state);
    closedSet.insert(current.state);

    m_env.onExpandNode(current.state, current.fScore, current.gScore);

    if (m_env.isSolution(current.state)) {
        printf("Solution\n");
        solution.states.clear();
        solution.actions.clear();
        auto iter = cameFrom.find(current.state);
        while (iter != cameFrom.end()) {
          solution.states.push_back(
              std::make_pair<>(iter->first, std::get<3>(iter->second)));
          solution.actions.push_back(std::make_pair<>(
              std::get<1>(iter->second), std::get<2>(iter->second)));
          iter = cameFrom.find(std::get<0>(iter->second));
        }
        solution.states.push_back(std::make_pair<>(startState, 0));
        std::reverse(solution.states.begin(), solution.states.end());
        std::reverse(solution.actions.begin(), solution.actions.end());
        solution.cost = current.gScore;
        if(openSet.empty())
          solution.fmin = current.fScore;
        else
          solution.fmin = openSet.top().fScore;
        if(current.focalHeuristic <= (m_w*current_anop.fopenmax)){
          return true;
        }

      }
    else{
      neighbors.clear();
      m_env.getNeighbors(current.state, neighbors);
      for (const Neighbor<State, Action, Cost>& neighbor : neighbors) {
        //printf("Neighbour\n");
        if (closedSet.find(neighbor.state) == closedSet.end()) {
          Cost tentative_gScore = current.gScore + neighbor.cost;
          auto iter = stateToHeap.find(neighbor.state);
          auto iterFocal = stateToFocal.find(neighbor.state);
          if (iter == stateToHeap.end() && iterFocal == stateToFocal.end()) {  // Discover a new node
            //std::cout << "  this is a new node" << std::endl;

            // I didn't add Improved termination condition

            Cost fScore =
                tentative_gScore + m_env.admissibleHeuristic(neighbor.state);

            // std::cout << tentative_gScore << " " << current.focalHeuristic << " " << m_env.focalStateHeuristic(neighbor.state, tentative_gScore) << " " << m_env.focalTransitionHeuristic(current.state, neighbor.state,
            //                                    current.gScore,
            //                                    tentative_gScore) << std::endl;

            Cost focalHeuristic = tentative_gScore + ((2*m_w-1) * m_env.admissibleHeuristic(neighbor.state)) ;

            Cost fopenmax = (tentative_gScore/m_w) +  m_env.admissibleHeuristic(neighbor.state);

            /*
            ((2*m_w - 1)*
                (m_env.focalStateHeuristic(neighbor.state, tentative_gScore) +
                 m_env.focalTransitionHeuristic(current.state, neighbor.state,
                                               current.gScore,
                                               tentative_gScore)));
                                               */

            auto handle = openSet.push(
                Node(neighbor.state, fScore, tentative_gScore, focalHeuristic, fopenmax));
            (*handle).handle = handle;
            focalSet.push(handle);
            anopen.push(handle);
            // if (fScore <= bestFScore * m_w) {
            //   // std::cout << "focalAdd: " << *handle << std::endl;
            //   focalSet.push(handle);
            // }
            stateToHeap.insert(std::make_pair<>(neighbor.state, handle));
            stateToFocal.insert(std::make_pair<>(neighbor.state, handle));
            m_env.onDiscover(neighbor.state, fScore, tentative_gScore);
            // std::cout << "  this is a new node " << fScore << "," <<
            // tentative_gScore << std::endl;
            cameFrom.erase(neighbor.state);
            cameFrom.insert(std::make_pair<>(
              neighbor.state,
              std::make_tuple<>(current.state, neighbor.action, neighbor.cost,
                                tentative_gScore)));
          }
        }
      }
    }

  }
  else{

    //Do optimal step

    std::cout<< "In Optimal" <<std::endl;

    const auto& nopen2 = openSet.top();
    std:: cout<< anopen.size() << std:: endl;
    openSet.pop();
    std:: cout<< anopen.size() << std:: endl;
    auto iter_for_open = stateToHeap.find(nopen2.state);
    currentHandle = iter_for_open->second;
    current = *currentHandle;
    // openSet.pop();
    // openSet.erase(currentHandle);
    //anopen.erase(current);
    stateToHeap.erase(current.state);
    if(closedSet.find(current.state)==closedSet.end()){
      std::cout<< "New Low level node" << std::endl;
      //m_env.onExpandNode(current.state, current.fScore, current.gScore);
      closedSet.insert(current.state);
    }

    //Do i need to add this node to closed set

    m_env.onExpandNode(current.state, current.fScore, current.gScore);

    if (m_env.isSolution(current.state)) {
        solution.states.clear();
        solution.actions.clear();
        auto iter = cameFrom.find(current.state);
        while (iter != cameFrom.end()) {
          solution.states.push_back(
              std::make_pair<>(iter->first, std::get<3>(iter->second)));
          solution.actions.push_back(std::make_pair<>(
              std::get<1>(iter->second), std::get<2>(iter->second)));
          iter = cameFrom.find(std::get<0>(iter->second));
        }
        solution.states.push_back(std::make_pair<>(startState, 0));
        std::reverse(solution.states.begin(), solution.states.end());
        std::reverse(solution.actions.begin(), solution.actions.end());
        solution.cost = current.gScore;
        solution.fmin = current.fScore;
        return true;
      }
    else{
      neighbors.clear();
      m_env.getNeighbors(current.state, neighbors);
      for (const Neighbor<State, Action, Cost>& neighbor : neighbors) {

        Cost tentative_gScore = current.gScore + neighbor.cost;
        auto iter = stateToHeap.find(neighbor.state);
        auto iterFocal = stateToFocal.find(neighbor.state);

        if (iter != stateToHeap.end()) {
          std::cout<< "In open" <<std::endl;
          auto handle = iter->second;
            // We found this node with a better path than previous path
            if (tentative_gScore < (*handle).gScore) {
              std:: cout << "Update Open" <<std::endl;
              (*handle).gScore = tentative_gScore;
              (*handle).fScore = tentative_gScore + m_env.admissibleHeuristic(neighbor.state);
              (*handle).focalHeuristic = tentative_gScore + ((2*m_w-1) * m_env.admissibleHeuristic(neighbor.state)) ;
              (*handle).fopenmax = (tentative_gScore/m_w) +  m_env.admissibleHeuristic(neighbor.state);
              // (*handle).focalHeuristic = tentative_gScore + ((2*m_w - 1)*
              //   (m_env.focalStateHeuristic(neighbor.state, tentative_gScore) +
              //    m_env.focalTransitionHeuristic(current.state, neighbor.state,
              //                                  current.gScore,
              //                                  tentative_gScore)));
                openSet.update(handle);
                // m_env.onDiscover(neighbor.state, (*handle).fScore,
                //              (*handle).gScore);

                //Do i need to include this node to focal
                if(iterFocal != stateToFocal.end()){
                  std:: cout << "Update Focal" <<std::endl;
                  // handle = iterFocal->second;
                  // focalSet.update(handle);

                }
              cameFrom.erase(neighbor.state);
                  cameFrom.insert(std::make_pair<>(
                  neighbor.state,
                  std::make_tuple<>(current.state, neighbor.action, neighbor.cost,
                                    tentative_gScore)));
            }
        }
        else if(closedSet.find(neighbor.state) == closedSet.end()){

          if (iter == stateToHeap.end()) {  // Discover a new node
            // std::cout << "  this is a new node" << std::endl;
            Cost fScore =
                tentative_gScore + m_env.admissibleHeuristic(neighbor.state);

            Cost focalHeuristic = tentative_gScore + ((2*m_w-1) * m_env.admissibleHeuristic(neighbor.state)) ;

            Cost fopenmax = (tentative_gScore/m_w) +  m_env.admissibleHeuristic(neighbor.state);

            // Cost focalHeuristic = tentative_gScore + ((2*m_w - 1)*
            //     (m_env.focalStateHeuristic(neighbor.state, tentative_gScore) +
            //      m_env.focalTransitionHeuristic(current.state, neighbor.state,
            //                                    current.gScore,
            //                                    tentative_gScore)));
            auto handle = openSet.push(
                Node(neighbor.state, fScore, tentative_gScore, focalHeuristic,fopenmax));
            (*handle).handle = handle;

            focalSet.push(handle);
            anopen.push(handle);

            stateToHeap.insert(std::make_pair<>(neighbor.state, handle));
            stateToFocal.insert(std::make_pair<>(neighbor.state, handle));
            m_env.onDiscover(neighbor.state, fScore, tentative_gScore);
            // std::cout << "  this is a new node " << fScore << "," <<
            // tentative_gScore << std::endl;

            cameFrom.erase(neighbor.state);
            cameFrom.insert(std::make_pair<>(
              neighbor.state,
              std::make_tuple<>(current.state, neighbor.action, neighbor.cost,
                                tentative_gScore)));
        }
        }
      }

    }

  }
}

    return false;
  }

 private:
  struct Node;

#ifdef USE_FIBONACCI_HEAP
  typedef typename boost::heap::fibonacci_heap<Node> openSet_t;
  typedef typename openSet_t::handle_type fibHeapHandle_t;
// typedef typename boost::heap::fibonacci_heap<fibHeapHandle_t,
// boost::heap::compare<compareFocalHeuristic> > focalSet_t;
#else
  typedef typename boost::heap::d_ary_heap<Node, boost::heap::arity<2>,
                                           boost::heap::mutable_<true> >
      openSet_t;
  typedef typename openSet_t::handle_type fibHeapHandle_t;
// typedef typename boost::heap::d_ary_heap<fibHeapHandle_t,
// boost::heap::arity<2>, boost::heap::mutable_<true>,
// boost::heap::compare<compareFocalHeuristic> > focalSet_t;
#endif

  struct Node {
    Node(const State& state, Cost fScore, Cost gScore, Cost focalHeuristic, Cost fopenmax)
        : state(state),
          fScore(fScore),
          gScore(gScore),
          focalHeuristic(focalHeuristic),
          fopenmax(fopenmax) {}

    bool operator<(const Node& other) const {
      //printf("InOpenCompare\n");
      // Sort order
      // 1. lowest fScore
      // 2. highest gScore

      // Our heap is a maximum heap, so we invert the comperator function here
      if (fScore != other.fScore) {
        return fScore > other.fScore;
      } else {
        return gScore < other.gScore;
      }
    }

    friend std::ostream& operator<<(std::ostream& os, const Node& node) {
      os << "state: " << node.state << " fScore: " << node.fScore
         << " gScore: " << node.gScore << " focal: " << node.focalHeuristic;
      return os;
    }

    State state;

    Cost fScore;
    Cost gScore;
    Cost focalHeuristic;
    Cost fopenmax;

    fibHeapHandle_t handle;
    // #ifdef USE_FIBONACCI_HEAP
    //   typename boost::heap::fibonacci_heap<Node>::handle_type handle;
    // #else
    //   typename boost::heap::d_ary_heap<Node, boost::heap::arity<2>,
    //   boost::heap::mutable_<true> >::handle_type handle;
    // #endif
  };

  struct compareFocalHeuristic {
    bool operator()(const fibHeapHandle_t& h1,
                    const fibHeapHandle_t& h2) const {
      // Sort order (see "Improved Solvers for Bounded-Suboptimal Multi-Agent
      // Path Finding" by Cohen et. al.)
      // 1. lowest focalHeuristic
      // 2. lowest fScore
      // 3. highest gScore

      //printf("InfocalCompare\n");

      // Our heap is a maximum heap, so we invert the comperator function here

      //compare fun() for A*eps
      if ((*h1).focalHeuristic != (*h2).focalHeuristic) {
        return (*h1).focalHeuristic > (*h2).focalHeuristic;
      }
      else if ((*h1).fScore != (*h2).fScore) {
          return (*h1).fScore > (*h2).fScore;
      }
      else {
        return (*h1).gScore < (*h2).gScore;
      }

      //compare fun() for IOS
      // if ((*h1).focalHeuristic != (*h2).focalHeuristic) {
      //   return (*h1).focalHeuristic > (*h2).focalHeuristic;
      //   // } else if ((*h1).fScore != (*h2).fScore) {
      //   //   return (*h1).fScore > (*h2).fScore;
      // }
      // // else if ((*h1).fScore != (*h2).fScore) {
      // //   return (*h1).fScore > (*h2).fScore;
      // //}
      // else {
      //   return (*h1).gScore < (*h2).gScore;
      // }

    }
  };

#ifdef USE_FIBONACCI_HEAP
  // typedef typename boost::heap::fibonacci_heap<Node> openSet_t;
  // typedef typename openSet_t::handle_type fibHeapHandle_t;
  typedef typename boost::heap::fibonacci_heap<
      fibHeapHandle_t, boost::heap::compare<compareFocalHeuristic> >
      focalSet_t;
#else
  // typedef typename boost::heap::d_ary_heap<Node, boost::heap::arity<2>,
  // boost::heap::mutable_<true> > openSet_t;
  // typedef typename openSet_t::handle_type fibHeapHandle_t;
  typedef typename boost::heap::d_ary_heap<
      fibHeapHandle_t, boost::heap::arity<2>, boost::heap::mutable_<true>,
      boost::heap::compare<compareFocalHeuristic> >
      focalSet_t;
#endif

struct compareOpenMax {
    bool operator()(const fibHeapHandle_t& h1,
                    const fibHeapHandle_t& h2) const {
      // Sort order (see "Improved termination condition")

      // Our heap is a maximum heap, so we invert the comperator function here

      //compare fun() for A*eps
      return (*h1).fopenmax < (*h2).fopenmax;

    }
  };

  typedef typename boost::heap::d_ary_heap<
      fibHeapHandle_t, boost::heap::arity<2>, boost::heap::mutable_<true>,
      boost::heap::compare<compareOpenMax> >
      anopenset_t;

 private:
  Environment& m_env;
  float m_w;
};

}  // namespace libMultiRobotPlanning


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG, www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also tools/license/license.mtl.txt in the distribution.

#ifndef ITL_PC_SUB_MATRIX_PC_INCLUDE
#define ITL_PC_SUB_MATRIX_PC_INCLUDE

#include <boost/static_assert.hpp>
#include <boost/mpl/if.hpp>

#include <boost/numeric/mtl/utility/range_generator.hpp>
#include <boost/numeric/mtl/utility/property_map.hpp>
#include <boost/numeric/mtl/vector/dense_vector.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>
#include <boost/numeric/itl/pc/solver.hpp>

namespace itl { namespace pc {

/// Class for applying \tparam Preconditioner only on a sub-matrix
/** Other entries are just copied.
    Optionally preconditioner can be referred from outside instead of storing it by setting
    \tparam Store to true.
**/
template <typename Preconditioner, typename Matrix, bool Store= true>
class sub_matrix_pc
{
    typedef mtl::dense_vector<bool>                                               tag_type;
    typedef typename boost::mpl::if_c<Store, Preconditioner, const Preconditioner&>::type pc_type;

    struct matrix_container
    {
	matrix_container() : Ap(0) {}

	matrix_container(const tag_type& tags, const Matrix& src)
	{
	    using std::size_t;
	    using namespace mtl;

	    mtl::dense_vector<size_t> perm(size(tags));
	    size_t n= 0;
	    for (size_t i= 0; i < size(tags); ++i) {
		perm[i]= n;
		if (tags[i])
		    ++n;
	    }

	    Ap= new Matrix(n, n);

	    typename traits::row<Matrix>::type             row(src);
	    typename traits::col<Matrix>::type             col(src);
	    typename traits::const_value<Matrix>::type     value(src);
	    typedef typename traits::range_generator<tag::major, Matrix>::type  cursor_type;

	    mat::inserter<Matrix> ins(*Ap, Ap->nnz() / Ap->dim1());

	    for (cursor_type cursor = mtl::begin<tag::major>(src), cend = mtl::end<tag::major>(src);
		 cursor != cend; ++cursor) {
		// std::cout << dest << '\n';

		typedef typename traits::range_generator<tag::nz, cursor_type>::type icursor_type;
		for (icursor_type icursor = mtl::begin<tag::nz>(cursor), icend = mtl::end<tag::nz>(cursor);
		     icursor != icend; ++icursor)
		    if (tags[row(*icursor)] && tags[col(*icursor)])
			ins(perm[row(*icursor)], perm[col(*icursor)]) << value(*icursor);
	    }
	}

	~matrix_container() { delete Ap; }

	Matrix* Ap;
    };

    size_t count_entries() const
    {
	using mtl::size;
	size_t n= 0;
	for (size_t i= 0; i < size(tags); ++i)
	    if (tags[i]) ++n;
	return n;
    }

  public:

    sub_matrix_pc(const tag_type& tags, const Matrix& A)
      : tags(tags), n(count_entries()), mc(tags, A), P(*mc.Ap)
    {
	BOOST_STATIC_ASSERT((Store));
	delete mc.Ap;
	mc.Ap= 0;
    }

#if 0 // to do later
    sub_matrix_pc(const tag_type& tags, const Preconditioner& P)
      : tags(tags), P(P)
    {
	// check sizes
    }
#endif

  private:
    template <typename VectorIn>
    VectorIn& create_x0(VectorIn) const
    {
	static VectorIn  x0(n);
	return x0;
    }

    template <typename VectorOut>
    VectorOut& create_y0(VectorOut) const
    {
	static VectorOut  y0(n);
	return y0;
    }

    template <typename VectorIn>
    void restrict(const VectorIn& x, VectorIn& x0) const
    {
	for (size_t i= 0, j= 0; i < size(tags); ++i)
	    if (tags[i])
		x0[j++]= x[i];
    }

    template <typename VectorIn, typename VectorOut>
    void prolongate(const VectorIn& x, const VectorOut& y0, VectorOut& y) const
    {
	for (size_t i= 0, j= 0; i < size(tags); ++i)
	    if (tags[i])
		y[i]= y0[j++];
	    else
		y[i]= x[i];
    }

  public:
    /// Solve Px = y approximately on according sub-system; remaining entries are copied
    template <typename VectorIn, typename VectorOut>
    void solve(const VectorIn& x, VectorOut& y) const
    {
	mtl::vampir_trace<5056> tracer;
	y.checked_change_resource(x);

	VectorIn&  x0= create_x0(x);
	VectorOut& y0= create_y0(y);

	restrict(x, x0);
	P.solve(x0, y0);
	// y0= solve(P, x0); // doesn't compile yet for unknown reasons
	prolongate(x, y0, y);
    }

    /// Solve Px = y approximately on according sub-system; remaining entries are copied
    template <typename VectorIn, typename VectorOut>
    void adjoint_solve(const VectorIn& x, VectorOut& y) const
    {
	mtl::vampir_trace<5057> tracer;
	y.checked_change_resource(x);

	VectorIn&  x0= create_x0(x);
	VectorOut& y0= create_y0(y);

	restrict(x, x0);
	P.adjoint_solve(x0, y0);
	// y0= adjoint_solve(P, x0); // doesn't compile yet for unknown reasons
	prolongate(x, y0, y);
    }

  private:
    tag_type          tags;
    size_t            n;
    matrix_container  mc;
    pc_type           P;
};

template <typename Preconditioner, typename Matrix, bool Store, typename Vector>
solver<sub_matrix_pc<Preconditioner, Matrix, Store>, Vector, false>
inline solve(const sub_matrix_pc<Preconditioner, Matrix, Store>& P, const Vector& x)
{
    return solver<sub_matrix_pc<Preconditioner, Matrix, Store>, Vector, false>(P, x);
}

template <typename Preconditioner, typename Matrix, bool Store, typename Vector>
solver<sub_matrix_pc<Preconditioner, Matrix, Store>, Vector, true>
inline adjoint_solve(const sub_matrix_pc<Preconditioner, Matrix, Store>& P, const Vector& x)
{
    return solver<sub_matrix_pc<Preconditioner, Matrix, Store>, Vector, true>(P, x);
}

}} // namespace itl::pc

#endif // ITL_PC_SUB_MATRIX_PC_INCLUDE


#include <boost/simd/memory/input_range.hpp>
#include <boost/range/algorithm/copy.hpp>

#include <algorithm>
#include <iterator>
#include <iostream>
#include <vector>

int main()
{
  using boost::simd::pack;
  using boost::simd::input_range;

  std::vector<float> data(16);

  for(std::size_t i=0;i<data.size();++i)
    data[i]  = 1 + i;

  boost::copy( input_range<4>(data), std::ostream_iterator< pack<float,4> >(std::cout, " "));
  std::cout << std::endl;
}


//==================================================================================================
/*!

  Copyright 2015 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/constant/log10_2hi.hpp>
#include <boost/simd/as.hpp>
#include <simd_test.hpp>

STF_CASE_TPL( "Check log10_2hi behavior for integral types"
            , (std::uint32_t)(std::uint64_t)
              (std::int32_t)(std::int64_t)
            )
{
  using boost::simd::as;
  using boost::simd::detail::log10_2hi;
  using boost::simd::Log10_2hi;
  T ref = T(0);
  STF_TYPE_IS(decltype(Log10_2hi<T>()), T);
  STF_EQUAL(Log10_2hi<T>(), ref);
  STF_EQUAL(log10_2hi( as(T{}) ),ref);
}

STF_CASE_TPL( "Check log10_2hi behavior for double"
            , (double)
            )
{
  using boost::simd::as;
  using boost::simd::detail::log10_2hi;
  using boost::simd::Log10_2hi;
  T ref = T(3.0102539062500000e-1);

  STF_TYPE_IS(decltype(Log10_2hi<T>()), T);
  STF_IEEE_EQUAL(Log10_2hi<T>(), ref);
  STF_IEEE_EQUAL(log10_2hi( as(T{}) ), ref);
}

STF_CASE_TPL( "Check log10_2hi behavior for float"
            , (float)
            )
{
  using boost::simd::as;
  using boost::simd::detail::log10_2hi;
  using boost::simd::Log10_2hi;
  T ref = T(3.0078125000000000e-1);
  STF_TYPE_IS(decltype(Log10_2hi<T>()), T);
  STF_IEEE_EQUAL(Log10_2hi<T>(), ref);
  STF_IEEE_EQUAL(log10_2hi( as(T{}) ), ref);
}


#include <iostream>
#include <fstream>
#include <algorithm>
#include <vector>

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>

#include "TestSet.hpp"
#include "Helper.hpp"

using namespace std;
namespace ublas = boost::numeric::ublas;

/*********************************************************************************/
/*                                CLASS FOR TESTSET                              */
/*********************************************************************************/

TestSet::TestSet(const string &directory): m_directory(directory)
{
	// Read in Data from Files
	ReadInData(directory, m_dj, m_bi, m_fi, m_cij);

	// I, J Ranges
	m_locationNumber = m_fi.size();
	m_customerNumber = m_dj.size();
}

void TestSet::ReadInData(
	const string &directory,
	vector<double> &dj,
	vector<double> &bi,
	vector<double> &fi,
	ublas::matrix<double> &cij)
{
	string line = "";
	vector<string> myFiles = { "fi.txt", "bi.txt", "dj.txt" };
	vector <double> cache;
	double vector_value = 0.0;
	string::size_type sz;

	// Read in: d_j, f_i, b_i
	for (string file : myFiles)
	{
		ifstream infile(directory + '/' + file);

		for (string line; getline(infile, line); )
		{
			if (file == "dj.txt")
			{
				if (line.find_last_of(' ') != string::npos)
					vector_value = stod(line.substr(line.find_last_of(' ')).c_str());
				else if (!line.empty())
					vector_value = stod(line, &sz);
				dj.push_back(vector_value);
			}
			else if (file == "fi.txt")
			{
				if (line.find_last_of(' ') != string::npos)
					vector_value = stod(line.substr(line.find_last_of(' ')).c_str());
				else if (!line.empty())
					vector_value = stod(line, &sz);
				fi.push_back(vector_value);
			}
			else if (file == "bi.txt")
			{
				if (line.find_last_of(' ') != string::npos)
					vector_value = stod(line.substr(line.find_last_of(' ')).c_str());
				else if (!line.empty())
					vector_value = stod(line, &sz);
				bi.push_back(vector_value);
			}
		}

		infile.close();
	}

	// Read in C_ij
	ifstream infile(directory + '/' + "cij.txt");
	for (string line; getline(infile, line); )
	{
		if (line.find_last_of(' ') != string::npos)
			vector_value = stod(line.substr(line.find_last_of(' ')).c_str());
		else if (!line.empty())
			vector_value = stod(line, &sz);
		cache.push_back(vector_value);
	}

	cij = makeMatrix(bi.size(), dj.size(), cache);
	cache.clear();
	infile.close();
}


//  (C) Copyright Eric Niebler 2005.
//  Use, modification and distribution are 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)

#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/mean.hpp>

using namespace boost;
using namespace unit_test;
using namespace accumulators;

namespace my
{
    BOOST_PARAMETER_KEYWORD(tag, sum_acc)
}

///////////////////////////////////////////////////////////////////////////////
// test_stat
//
void test_stat()
{
    accumulator_set<int, stats<tag::mean, tag::external<tag::sum, my::tag::sum_acc> > > acc;
    accumulator_set<int, stats<tag::sum> > sum_acc;

    acc(1);
    sum_acc(1);
    BOOST_CHECK_CLOSE(1., mean(acc, my::sum_acc = sum_acc), 1e-5);
    BOOST_CHECK_EQUAL(1u, count(acc));
    BOOST_CHECK_EQUAL(1, sum(sum_acc));

    acc(0);
    sum_acc(0);
    BOOST_CHECK_CLOSE(0.5, mean(acc, my::sum_acc = sum_acc), 1e-5);
    BOOST_CHECK_EQUAL(2u, count(acc));
    BOOST_CHECK_EQUAL(1, sum(acc, my::sum_acc = sum_acc));

    acc(2);
    sum_acc(2);
    BOOST_CHECK_CLOSE(1., mean(acc, my::sum_acc = sum_acc), 1e-5);
    BOOST_CHECK_EQUAL(3u, count(acc));
    BOOST_CHECK_EQUAL(3, sum(acc, my::sum_acc = sum_acc));
}

///////////////////////////////////////////////////////////////////////////////
// test_reference
//
void test_reference()
{
    typedef accumulator_set<int, stats<tag::sum> > sum_acc_type;
    sum_acc_type sum_acc;
    accumulator_set<
        int
      , stats<
            tag::mean
          , tag::external<tag::sum, my::tag::sum_acc>
          , tag::reference<sum_acc_type, my::tag::sum_acc>
        >
    > acc(my::sum_acc = sum_acc);

    acc(1);
    sum_acc(1);
    BOOST_CHECK_CLOSE(1., mean(acc), 1e-5);
    BOOST_CHECK_EQUAL(1u, count(acc));
    BOOST_CHECK_EQUAL(1, sum(sum_acc));

    acc(0);
    sum_acc(0);
    BOOST_CHECK_CLOSE(0.5, mean(acc), 1e-5);
    BOOST_CHECK_EQUAL(2u, count(acc));
    BOOST_CHECK_EQUAL(1, sum(acc));

    acc(2);
    sum_acc(2);
    BOOST_CHECK_CLOSE(1., mean(acc), 1e-5);
    BOOST_CHECK_EQUAL(3u, count(acc));
    BOOST_CHECK_EQUAL(3, sum(acc));
}

///////////////////////////////////////////////////////////////////////////////
// test_reference2
//
void test_reference2()
{
    typedef accumulator_set<int, stats<tag::sum> > sum_acc_type;
    sum_acc_type sum_acc;
    accumulator_set<
        int
      , stats<
            tag::mean
          , tag::external<tag::sum, my::tag::sum_acc, sum_acc_type>
        >
    > acc(my::sum_acc = sum_acc);

    acc(1);
    sum_acc(1);
    BOOST_CHECK_CLOSE(1., mean(acc), 1e-5);
    BOOST_CHECK_EQUAL(1u, count(acc));
    BOOST_CHECK_EQUAL(1, sum(sum_acc));

    acc(0);
    sum_acc(0);
    BOOST_CHECK_CLOSE(0.5, mean(acc), 1e-5);
    BOOST_CHECK_EQUAL(2u, count(acc));
    BOOST_CHECK_EQUAL(1, sum(acc));

    acc(2);
    sum_acc(2);
    BOOST_CHECK_CLOSE(1., mean(acc), 1e-5);
    BOOST_CHECK_EQUAL(3u, count(acc));
    BOOST_CHECK_EQUAL(3, sum(acc));
}

///////////////////////////////////////////////////////////////////////////////
// init_unit_test_suite
//
test_suite* init_unit_test_suite( int argc, char* argv[] )
{
    test_suite *test = BOOST_TEST_SUITE("external_accumulator test");

    test->add(BOOST_TEST_CASE(&test_stat));
    test->add(BOOST_TEST_CASE(&test_reference));
    test->add(BOOST_TEST_CASE(&test_reference2));

    return test;
}


#include "mPlace.h"
#include "circuit.h"
#include "lefdefIO.h"
#include "partition.h"

#include <boost/icl/interval_map.hpp>
#include <boost/icl/closed_interval.hpp>

using std::cout;
using std::endl;
using std::string;
using std::vector;
using std::pair;
using std::unordered_map;
using std::unordered_set;
using MacroNetlist::Partition;
using namespace MacroNetlist;
using namespace boost::icl;

typedef vector<pair<Partition, Partition>> TwoPartitions;

void CutRoundUp( CircuitInfo& cInfo, double& cutLine, bool isHorizontal ) {
  if( isHorizontal ) {
    int integer = (1.0* cutLine / cInfo.siteSizeX) + 0.5f;
    cutLine = integer * cInfo.siteSizeX;
    cutLine = (cutLine > cInfo.ux)? cInfo.ux : cutLine;
    cutLine = (cutLine < cInfo.lx)? cInfo.lx : cutLine;
  }
  else {
    int integer = (1.0* cutLine / cInfo.siteSizeY) + 0.5f;
    cutLine = integer * cInfo.siteSizeY;
    cutLine = (cutLine > cInfo.uy)? cInfo.uy : cutLine;
    cutLine = (cutLine < cInfo.ly)? cInfo.ly : cutLine;
  }
}

//
void PrintAllSets(FILE* fp, CircuitInfo& cInfo,
    vector< vector<Partition> >& allSets) {
  cout << "Writing sets file... ";
  for(auto& curSet : allSets) {
    fprintf(fp,"BEGIN SET ;\n" );
    fprintf(fp,"  WIDTH %f ;\n", cInfo.ux - cInfo.lx );
    fprintf(fp,"  HEIGHT %f ;\n", cInfo.uy - cInfo.ly );
    for(auto& curSlice : curSet) {
      if( curSlice.macroStor.size() == 0) {
        continue;
      }
      curSlice.PrintSetFormat(fp);
    }
    fprintf(fp,"END SET ;\n");
  }
  fflush(fp);
  cout << "Done!" << endl;
}

vector< vector<Partition> >
PlaceMacros(EnvFile& env, Circuit::Circuit& ckt, MacroCircuit& mckt) {

  //env.Print();

  cout << "PROC: LEF/DEF Parsing... ";
  ckt.Init(env.lefStor, env.def);
  env.design = ckt.defDesignName;
  cout << "Done!" << endl;

  double defScale = ckt.defUnit;
  defiPoints points = ckt.defDieArea.getPoint();
  if( points.numPoints != 2) {
    cout << "ERROR: The Layout must be rectangle..! "<< endl;
    exit(1);
  }

  if( ckt.defRowStor.size() == 0) {
    cout << "ERROR: DEF must contain ROW"<< endl;
    exit(1);
  }

  string siteName(ckt.defRowStor[0].macro());
  auto sitePtr = ckt.lefSiteMap.find( siteName );
  if( sitePtr == ckt.lefSiteMap.end()) {
    cout << "ERROR: SITE " << siteName << " not exists in LEF." << endl;
    exit(1);
  }

  double siteSizeX = ckt.lefSiteStor[sitePtr->second].sizeX();
  double siteSizeY = ckt.lefSiteStor[sitePtr->second].sizeY();

  CircuitInfo cInfo( 1.0*points.x[0]/defScale,
        1.0*points.y[0]/defScale,
        1.0*points.x[1]/defScale,
        1.0*points.y[1]/defScale,
        siteSizeX, siteSizeY );

  cout << endl;

  cout << "DieBBox: (" << cInfo.lx << " " << cInfo.ly << ") - ("
    << cInfo.ux << " " << cInfo.uy << ")" << endl;
//  cout << "Layout Information" << endl;
//  for(int i=0; i<points.numPoints; i++) {
//    cout << 1.0*points.x[i]/defScale << " ";
//    cout << 1.0*points.y[i]/defScale << endl;
//  }

//  cout << "Original Macro List" << endl;

//  MacroCircuit mckt(ckt, env, cInfo);
  mckt.Init(&ckt, &env, &cInfo);

  //  RandomPlace for special needs.
  //  Really not recommended to execute this functioning
  if( env.isRandomPlace == true ) {
    double snapGrid = 0.02f;
    mckt.StubPlacer(snapGrid);
  }

//  mckt.GetMacroStor(ckt);

//  for(auto& curMacro: mckt.macroStor) {
//    curMacro.Dump();
//  }

  bool isHorizontal = (cInfo.ux-cInfo.lx) > (cInfo.uy-cInfo.ly);

  Partition layout(PartClass::ALL,
      cInfo.lx, cInfo.ly, cInfo.ux-cInfo.lx, cInfo.uy-cInfo.ly);
  layout.macroStor = mckt.macroStor;

  cout << "PROC: Begin One Level Partition ... " << endl;
  TwoPartitions oneLevelPart = GetPart(cInfo, layout, isHorizontal);
  cout << "PROC: End One Level Partition" << endl << endl;
  TwoPartitions eastStor, westStor;

  vector< vector<Partition> > allSets;

  // Fill the MacroNetlist for ALL circuits
  unordered_map< PartClass, vector<int>,
    MyHash<PartClass>> globalMacroPartMap;
  UpdateMacroPartMap( mckt, layout, globalMacroPartMap );
  layout.FillNetlistTable( mckt, globalMacroPartMap );
  mckt.UpdateNetlist(layout);

  // push to the outer vector
  vector<Partition> layoutSet;
  layoutSet.push_back(layout);

  // push
  allSets.push_back(layoutSet);

  for(auto& curSet : oneLevelPart ) {
    if( isHorizontal ) {
      cout << "PROC: Begin Horizontal Cuts" << endl;
      CircuitInfo eastInfo(cInfo, curSet.first);
      CircuitInfo westInfo(cInfo, curSet.second);

      cout << "PROC: Begin 1st Second Level Partition" << endl;
      TwoPartitions eastStor = GetPart(eastInfo, curSet.first, !isHorizontal);
      cout << "PROC: End 1st Second Level Partition" << endl << endl;

      cout << "PROC: Begin 2nd Second Level Partition" << endl;
      TwoPartitions westStor = GetPart(westInfo, curSet.second, !isHorizontal);
      cout << "PROC: End 2nd Second Level Partition" << endl << endl;

      // Zero case handling when eastStor = 0
      if( eastStor.size() == 0 && westStor.size() != 0 ) {
        for(size_t i=0; i<westStor.size(); i++) {
          vector<Partition> oneSet;

          // one set is composed of two subblocks
          oneSet.push_back( westStor[i].first );
          oneSet.push_back( westStor[i].second );

          // Fill Macro Netlist
          // update macroPartMap
          unordered_map< PartClass, vector<int>,
            MyHash<PartClass>> macroPartMap;
          for(auto& curSet: oneSet) {
            UpdateMacroPartMap( mckt, curSet, macroPartMap );
          }

          for(auto& curSet: oneSet) {
            curSet.FillNetlistTable( mckt, macroPartMap );
          }

          allSets.push_back( oneSet );
        }
      }
      // Zero case handling when westStor = 0
      else if( eastStor.size() != 0 && westStor.size() == 0 ) {
        for(size_t i=0; i<eastStor.size(); i++) {
          vector<Partition> oneSet;

          // one set is composed of two subblocks
          oneSet.push_back( eastStor[i].first );
          oneSet.push_back( eastStor[i].second );

          // Fill Macro Netlist
          // update macroPartMap
          unordered_map< PartClass, vector<int>,
            MyHash<PartClass>> macroPartMap;
          for(auto& curSet: oneSet) {
            UpdateMacroPartMap( mckt, curSet, macroPartMap );
          }

          for(auto& curSet: oneSet) {
            curSet.FillNetlistTable( mckt, macroPartMap );
          }

          allSets.push_back( oneSet );
        }
      }
      else {
        // for all possible combinations in partitions
        for(size_t i=0; i<eastStor.size(); i++) {
          for(size_t j=0; j<westStor.size(); j++) {

            vector<Partition> oneSet;

            // one set is composed of four subblocks
            oneSet.push_back( eastStor[i].first );
            oneSet.push_back( eastStor[i].second );
            oneSet.push_back( westStor[j].first );
            oneSet.push_back( westStor[j].second );

            // Fill Macro Netlist
            // update macroPartMap
            unordered_map< PartClass, vector<int>,
              MyHash<PartClass>> macroPartMap;
            for(auto& curSet: oneSet) {
              UpdateMacroPartMap( mckt, curSet, macroPartMap );
            }

            for(auto& curSet: oneSet) {
              curSet.FillNetlistTable( mckt, macroPartMap );
            }

            allSets.push_back( oneSet );
          }
        }
      }
    }
    else {
      cout << "PROC: Vertical Cuts" << endl;
    }
  }
  cout << "INFO: Total Extracted Sets = " << allSets.size() << endl << endl;

  return allSets;

  // Legalize macro on global Structure
//  double snapGrid = 0.002f;

  // TopLevel Macro Location Update
  // For each possible full-layout

  /*
  int setCnt = 0;
  int bestSetIdx = 0;
  double bestWwl = DBL_MIN;
  for(auto& curSet: allSets) {

    // skip for top-layout partition
    if( curSet.size() == 1) {
      continue;
    }

    // For each partitions (four partition)
//    cout << "curSet.size: " << curSet.size() << endl;
    for(auto& curPart : curSet) {
      // Annealing based on ParquetFP Engine
      curPart.DoAnneal();
      // Update mckt frequently
      mckt.UpdateMacroCoordi(curPart);
    }

    // update partitons' macro info
    for(auto& curPart : curSet) {
      curPart.UpdateMacroCoordi(mckt);
    }

    if( env.generateAll ) {
      // update ckt structure
      UpdateCircuitCoordi(env, mckt, ckt);

      // check plotting
      if( env.isPlot ) {
        mckt.Plot(env.output + "_" + std::to_string(setCnt) + ".plt", curSet);
      }

      // top-level layout print
      FILE* fp =
        fopen(string(env.output + "_"
              + std::to_string(setCnt) + ".def").c_str(), "w");

      if( fp == NULL) {
        cout << "ERROR: cannot open " << env.output << " to write output file" << endl;
        exit(1);
      }

      ckt.WriteDef( fp );
      fclose(fp);
    }
    else {
      double curWwl = mckt.GetWeightedWL();
      cout << "Set " << &curSet - &allSets[0] << ": WWL = " << curWwl << endl;

      if( curWwl > bestWwl ) {
        bestWwl = curWwl;
        bestSetIdx = &curSet - &allSets[0];
      }
    }
    setCnt ++;
  }

  // bestset DEF writing
  vector<Partition> bestSet = allSets[bestSetIdx];
  if( !env.generateAll ) {
    for(auto& curPart : bestSet) {
      curPart.UpdateMacroCoordi(mckt);
    }

    UpdateCircuitCoordi(env, mckt, ckt);

    // check plotting
    if( env.isPlot ) {
      mckt.Plot(env.output + "_best.plt", bestSet );
    }

    // top-level layout print
    FILE* fp =
      fopen(string(env.output + "_best.def").c_str(), "w");

    if( fp == NULL) {
      cout << "ERROR: cannot open " << env.output << " to write output file" << endl;
      exit(1);
    }

    ckt.WriteDef( fp );
    fclose(fp);
  }
   */

  /*
  // Writing functions for Sets file
  FILE* fp = fopen(_env.output.c_str(), "w");
  if( fp == NULL) {
    cout << "ERROR: cannot open " << _env.output << " to write output file" << endl;
    exit(1);
  }

  // Generate *.sets file
  PrintAllSets(fp, cInfo, allSets);
  fclose(fp);
  */

  // ParquetFormat print
  // only top-level formats
//  layout.PrintParquetFormat(_env.output);

  cout << "PROC: End TritonMacroPlacer" << endl;

  // All Edges...
//  for(int i=1; i<=numEdge; i++) {
//    sta::Edge* edge = _sta->graph()->edge(i);
//    cout << edge->from(_sta->graph())->name(_sta->network()) <<
//      " -> " << edge->to(_sta->graph())->name(_sta->network()) << endl;
//  }

//  PinSeq pinStor;
//  pinStor.push_back( );

//  for(auto& curSet : allSets) {
//    for(auto& curPart : curSet) {
//      for(auto& curMacro : curPart.macroStor) {
//        cout << curMacro.name << endl;
//        Pin* curPin = _sta->network()->findPin(curMacro.name.c_str());
//      }
//    }
//  }

  /*
  // vertex
  sta::VertexIterator vertIter(_sta->graph());
  while( vertIter.hasNext()) {
    sta::Vertex* vertex = vertIter.next();
//    if( vertex->isRoot() ) {
//     if ( vertex->isCheckClk() || vertex->isGatedClkEnable() || vertex->isRegClk() ) {
//    if( _sta->network()->isTopLevelPort(vertex->pin())) {
//      cout << vertex->name(_sta->network()) << endl;
//    }
  }

  for(int i=1; i<=numEdge; i++) {
    sta::Edge* edge = _sta->graph()->edge(i);
//    cout << edge->from(_sta->graph())->name(_sta->network()) <<
//      " -> " << edge->to(_sta->graph())->name(_sta->network()) << endl;
  }

  LeafInstanceIterator* leafInstanceIter = _sta->network()->leafInstanceIterator();
  while( leafInstanceIter->hasNext() ) {
    Instance* instance = leafInstanceIter->next();
//    cout << _sta->network()->name(instance) << "/";
    InstancePinIterator* instancePinIter = _sta->network()->pinIterator(instance);
    while( instancePinIter->hasNext()) {
      sta::Pin* pin = instancePinIter->next();
      if( _sta->network()->isDriver(pin) ) {
        cout << "D: ";
      }
      else if( _sta->network()->isLoad(pin) ) {
        cout << "L: ";
      }
      cout << _sta->network()->name(pin) << endl ;
    }
    cout <<endl;
  }
  */

//  cout << "top: " << _sta->network()->name(_sta->network()->topInstance()) << endl;
}

// Two partitioning functions:
// first : lower part
// second : upper part
//
// cutLine is sweeping from lower to upper coordinates in x / y
vector<pair<Partition, Partition>> GetPart(
    CircuitInfo &cInfo,
    Partition& partition,
    bool isHorizontal ) {
  cout << "PROC: Begin Partition ... " << endl;
  cout << "INFO: Partitions' #Macro = " << partition.macroStor.size() << endl;

  // Return vector
  vector<pair<Partition, Partition>> ret;

  typedef std::set<int> MacroSetT;
  interval_map<double, MacroSetT> macroMap;

  double maxWidth = DBL_MIN;
  double maxHeight = DBL_MIN;

  // in parent partition, traverse macros
  //
  for(auto& curMacro: partition.macroStor) {
    MacroSetT macroInfo;
    macroInfo.insert( &curMacro - &partition.macroStor[0]);

    macroMap.add(
        make_pair(
          interval<double>::closed(
            (isHorizontal)? curMacro.lx : curMacro.ly,
            (isHorizontal)? curMacro.lx + curMacro.w :
                curMacro.ly + curMacro.h ), macroInfo));
    maxWidth = ( maxWidth < curMacro.w)? curMacro.w: maxWidth;
    maxHeight = ( maxHeight < curMacro.h)? curMacro.h: maxHeight;
  }

  double cutLineLimit = (isHorizontal)? maxWidth * 0.25 : maxHeight * 0.25;

  double prevPushLimit = DBL_MIN;
  bool isFirst = true;
  vector<double> cutLineStor;

  // less than 4
  if( partition.macroStor.size() <= 4 ) {
    for( interval_map<double, MacroSetT>::iterator
        it = macroMap.begin();
        it != macroMap.end(); *it++) {
      interval<double>::type macroInterval = it->first;
      MacroSetT result = it->second;

      if( isFirst ) {
        cutLineStor.push_back( macroInterval.lower() );
        prevPushLimit = macroInterval.lower();
        isFirst = false;
      }
      else if( abs(macroInterval.lower()-prevPushLimit) > cutLineLimit ) {
        cutLineStor.push_back( macroInterval.lower() );
        prevPushLimit = macroInterval.lower();
      }
    }
  }
  // more than 4
  else {
    int hardLimit = int( sqrt( 1.0*partition.macroStor.size()/3.0 ) + 0.5f);
    for(int i=0; i<=hardLimit; i++) {
      cutLineStor.push_back( (isHorizontal)?
          cInfo.lx + 1.0*(cInfo.ux - cInfo.lx)/hardLimit * i :
          cInfo.ly + 1.0*(cInfo.uy - cInfo.ly)/hardLimit * i );
    }
  }
  cout << "INFO: NumCutline = " << cutLineStor.size() << endl;

  // Macro checker array
  // 0 for uninitialize
  // 1 for lower
  // 2 for upper
  // 3 for both
  int* chkArr = new int[partition.macroStor.size()];

  for(auto& cutLine : cutLineStor ) {
    cout << "INFO: CutLine = " << cutLine << endl;
    CutRoundUp(cInfo, cutLine, isHorizontal);
    cout << "INFO: Updated CutLine = " << cutLine << endl;

    // chkArr initialize
    for(size_t i=0; i<partition.macroStor.size(); i++) {
      chkArr[i] = 0;
    }

    bool isImpossible = false;
    for(auto& curMacro : partition.macroStor) {
      int i = &curMacro - &partition.macroStor[0];
      if( isHorizontal ) {
        // lower is possible
        if( curMacro.w <= cutLine ) {
          chkArr[i] += 1;
        }
        // upper is possible
        if ( curMacro.w <= partition.width - cutLine) {
          chkArr[i] += 2;
        }
        // none of them
        if( chkArr[i] == 0 ) {
          isImpossible = true;
          break;
        }
      }
      else {
        // lower is possible
        if( curMacro.h <= cutLine ) {
          chkArr[i] += 1;
        }
        // upper is possible
        if (curMacro.h <= partition.height - cutLine) {
          chkArr[i] += 2;
        }
        // none of
        if( chkArr[i] == 0 ) {
          isImpossible = true;
          break;
        }
      }
    }
    // impossible cuts, then skip
    if( isImpossible ) {
      continue;
    }

    // Fill in the Partitioning information
    PartClass lClass = None, uClass = None;
    if( partition.partClass == ALL ) {
      lClass = (isHorizontal)? W : S;
      uClass = (isHorizontal)? E : N;
    }

    if( partition.partClass == W) {
      lClass = SW;
      uClass = NW;

    }
    if( partition.partClass == E) {
      lClass = SE;
      uClass = NE;
    }

    if( partition.partClass == N) {
      lClass = NW;
      uClass = NE;
    }

    if( partition.partClass == S) {
      lClass = SW;
      uClass = SE;
    }

    Partition lowerPart( lClass,
      partition.lx,
      partition.ly,
      (isHorizontal)? cutLine - partition.lx : partition.width,
      (isHorizontal)? partition.height : cutLine - partition.ly);

    Partition upperPart( uClass,
      (isHorizontal)? cutLine : partition.lx,
      (isHorizontal)? partition.ly :cutLine,
      (isHorizontal)? partition.lx + partition.width - cutLine : partition.width,
      (isHorizontal)? partition.height : partition.ly + partition.height - cutLine);

    // cout << it->first << " " << it->second << endl;
    //
    // Fill in child partitons' macroStor
    for(auto& curMacro : partition.macroStor) {
      int i=&curMacro - &partition.macroStor[0];
      if( chkArr[i] == 1 ) {
        lowerPart.macroStor.push_back(
            Macro( curMacro.name, curMacro.type,
              curMacro.lx, curMacro.ly,
              curMacro.w, curMacro.h,
              curMacro.haloX, curMacro.haloY,
              curMacro.channelX, curMacro.channelY,
              curMacro.ptr, curMacro.instPtr )) ;
      }
      else if( chkArr[i] == 2 ) {
        upperPart.macroStor.push_back(
            Macro( curMacro.name, curMacro.type,
              (isHorizontal)? curMacro.lx-cutLine : curMacro.lx,
              (isHorizontal)? curMacro.ly : curMacro.ly-cutLine,
              curMacro.w, curMacro.h,
              curMacro.haloX, curMacro.haloY,
              curMacro.channelX, curMacro.channelY,
              curMacro.ptr, curMacro.instPtr));
      }
      else if( chkArr[i] == 3 ) {
        double centerPoint =
          (isHorizontal)?
          curMacro.lx + curMacro.w/2.0 :
          curMacro.ly + curMacro.h/2.0;

        if( centerPoint < cutLine ) {
          lowerPart.macroStor.push_back(
              Macro( curMacro.name, curMacro.type,
                curMacro.lx, curMacro.ly,
                curMacro.w, curMacro.h,
                curMacro.haloX, curMacro.haloY,
                curMacro.channelX, curMacro.channelY,
                curMacro.ptr, curMacro.instPtr )) ;

        }
        else {
          upperPart.macroStor.push_back(
              Macro( curMacro.name, curMacro.type,
                (isHorizontal)? curMacro.lx-cutLine : curMacro.lx,
                (isHorizontal)? curMacro.ly : curMacro.ly-cutLine,
                curMacro.w, curMacro.h,
                curMacro.haloX, curMacro.haloY,
                curMacro.channelX, curMacro.channelY,
                curMacro.ptr, curMacro.instPtr));
        }
      }
    }

    double lowerArea = lowerPart.width * lowerPart.height;
    double upperArea = upperPart.width * upperPart.height;

    double upperMacroArea = 0.0f;
    double lowerMacroArea = 0.0f;

    for(auto& curMacro : upperPart.macroStor) {
      upperMacroArea += curMacro.w * curMacro.h;
    }
    for(auto& curMacro : lowerPart.macroStor) {
      lowerMacroArea += curMacro.w * curMacro.h;
    }
//    cout << upperMacroArea << " " << upperArea << endl;
//    cout << lowerMacroArea << " " << lowerArea << endl;
    // impossible partitioning
    if( upperMacroArea > upperArea || lowerMacroArea > lowerArea) {
      cout << "PROC: Impossible partition, continue" << endl;
      continue;
    }

// Previous code
//    if( partition.partClass == N ||
//        partition.partClass == E ||
//        partition.partClass == W ||
//        partition.partClass == S ) {
//      lowerPart.FillNetlistTable();
//      upperPart.FillNetlistTable();
//    }

    pair<Partition, Partition> curPart( lowerPart, upperPart );

    cout << "INFO: NumMacro in LowerPart[" << ret.size() << "] = "
      << lowerPart.macroStor.size() << endl;
    cout << "INFO: NumMacro in UpperPart[" << ret.size() << "] = "
      << upperPart.macroStor.size() << endl;

    ret.push_back( curPart );
  }
  delete[] chkArr;
  cout << "PROC: End Partition " << endl << endl;

  return ret;
}

// using mckt.macroInstMap and partition,
// fill in macroPartMap
//
// macroPartMap will contain
//
// first: macro partition class info
// second: macro candidates.
void UpdateMacroPartMap(
    MacroCircuit& mckt,
    MacroNetlist::Partition& part,
    unordered_map<MacroNetlist::PartClass, vector<int>,
    MyHash<MacroNetlist::PartClass>>& macroPartMap ) {

  auto mpPtr = macroPartMap.find( part.partClass );
  if( mpPtr == macroPartMap.end() ) {
    vector<int> curMacroStor;
    // convert macro Information into macroIdx
    for(auto& curMacro: part.macroStor) {
      auto miPtr = mckt.macroInstMap.find( curMacro.instPtr );
      if( miPtr == mckt.macroInstMap.end() ) {
        cout << "ERROR: macro " << curMacro.name
          << " not exists in macroInstMap: " << curMacro.instPtr << endl;
        exit(1);
      }
      curMacroStor.push_back( miPtr->second) ;
    }
    macroPartMap[ part.partClass ] = curMacroStor;
  }
  else {
    cout << "ERROR: Partition- " << part.partClass
      << " already updated (UpdateMacroPartMap)" << endl;
    exit(1);
  }
}

//
// update ckt class from mckt, env.
//
void UpdateCircuitCoordi(EnvFile& env, MacroCircuit& mckt, Circuit::Circuit& ckt) {
  unordered_set<int> macroIdxMap;
  double defScale = ckt.defUnit;

  // update ckt structure
  for(auto& curMacro : mckt.macroStor) {
    auto cPtr = ckt.defComponentMap.find( curMacro.name );
    if( cPtr == ckt.defComponentMap.end()) {
      cout << "ERROR: Cannot find " << curMacro.name
        << " in defiComponentMap" << endl;
      exit(1);
    }
    int cIdx = cPtr->second;
    macroIdxMap.insert(cIdx);
    int lx = int(curMacro.lx * defScale + 0.5f);
    int ly = int(curMacro.ly * defScale + 0.5f);

    int orient = -1;
    if( ckt.defComponentStor[cIdx].placementStatus() != 0
        && ckt.defComponentStor[cIdx].placementStatus() != DEFI_COMPONENT_UNPLACED ) {
      // Follow original orient
      orient = ckt.defComponentStor[cIdx].placementOrient();
    }
    else {
      // Default is North
      orient = 0;
    }

    if( env.isWestFix ) {
      orient = 1;
    }

    ckt.defComponentStor[cIdx].
      setPlacementLocation(lx, ly, orient);

    ckt.defComponentStor[cIdx].
      setPlacementStatus(DEFI_COMPONENT_FIXED);
  }

  // reset other cells's location not to have bug in other tools
  for(size_t i=0; i<ckt.defComponentStor.size(); i++) {
    // continue for MacroIndex
    if( macroIdxMap.find(i) != macroIdxMap.end() ) {
      continue;
    }

    ckt.defComponentStor[i].setPlacementStatus(0);
    ckt.defComponentStor[i].setPlacementLocation(-1,-1,-1);
  }
}


// __BEGIN_LICENSE__
//  Copyright (c) 2006-2013, United States Government as represented by the
//  Administrator of the National Aeronautics and Space Administration. All
//  rights reserved.
//
//  The NASA Vision Workbench is 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.
// __END_LICENSE__

#include <test/Helpers.h>

#include <vw/Stereo/PreFilter.h>
#include <vw/Stereo/CorrelationView.h>
#include <boost/random/linear_congruential.hpp>

#include <vw/Image.h>

using namespace vw;
using namespace vw::stereo;

template <typename PixelT>
class PyramidViewTest : public ::testing::Test {
protected:
  PyramidViewTest(){}

  typedef PixelT pixel_type;
  typedef ImageView<PixelT> image_type;
  typedef typename PixelChannelType<PixelT>::type channel_type;
  image_type input1, input2;
  Vector2i kernel_size;
  BBox2i search_volume;
  int corr_timeout;
  double seconds_per_op;
  int max_levels;
  Matrix2x2 scale;
  Vector2 translation;

  virtual void SetUp() {
    boost::rand48 gen(10);
    input1 = ChannelRange<channel_type>::max()*uniform_noise_view( gen, 300, 200 );
    scale = Matrix2x2(.9,0,0,.95);
    translation = Vector2(150*.1,100*.05);
    input2 = transform(input1, AffineTransform(scale,translation),
                       ConstantEdgeExtension(), BicubicInterpolation());

    search_volume = BBox2( -1.5 * translation, 1.5*translation );
    kernel_size = Vector2i(7,7);
    corr_timeout = 0;
    seconds_per_op = 0;
    max_levels = 5;

    // These settings should invoke 4 pyramid levels in the Pyramid
    // Correation View.
  }

  template <class ViewT>
  void check_error( ImageViewBase<ViewT> const& input,
                    float correct = 0.9,
                    float attempted = 0.9,
                    std::string const& mesg = "" ) {
    ViewT const& disparity_map = input.impl();
    int64 count_correct = 0;
    int64 count_valid = 0;
    for ( int32 i = 0; i < disparity_map.cols(); ++i )
      for ( int32 j = 0; j < disparity_map.rows(); ++j )
        if ( is_valid( disparity_map(i,j) ) ) {
          count_valid++;
          Vector2 objectf = scale*Vector2(i,j)+translation - Vector2(i,j);
          Vector2i objective( round(objectf[0]), round(objectf[1]) );
          if ( disparity_map(i,j).child() == objective )
            count_correct++;
        }
    EXPECT_GT( float(count_correct)/float(count_valid), correct ) << mesg;
    EXPECT_GT( float(count_valid)/float(disparity_map.cols()*disparity_map.rows()), attempted ) << mesg;
  }
};

typedef PyramidViewTest<PixelGray<uint8> > PyramidViewGRAYU8;
typedef PyramidViewTest<PixelGray<int16> > PyramidViewGRAYI16;
typedef PyramidViewTest<PixelGray<float> > PyramidViewGRAYF32;
typedef PyramidViewTest<uint8>             PyramidViewU8;

TEST_F( PyramidViewGRAYU8, NullPreprocess ) {
  ImageView<PixelMask<Vector2i> > disparity_map =
    pyramid_correlate( input1, input2,
                       constant_view(uint8(255), input1),
                       constant_view(uint8(255), input2),
                       PREFILTER_NONE, 0,
                       search_volume, kernel_size,
                       ABSOLUTE_DIFFERENCE,
                       corr_timeout, seconds_per_op,
                       -1, max_levels );
  ASSERT_EQ( input1.cols(), disparity_map.cols() );
  ASSERT_EQ( input1.rows(), disparity_map.rows() );
  check_error( disparity_map, .909, .9985, "Absolute Difference" );

  disparity_map =
    pyramid_correlate( input1, input2,
                       constant_view(uint8(255), input1),
                       constant_view(uint8(255), input2),
                       PREFILTER_NONE, 0,
                       search_volume, kernel_size,
                       ABSOLUTE_DIFFERENCE,
                       corr_timeout, seconds_per_op,
                       2, max_levels );
  ASSERT_EQ( input1.cols(), disparity_map.cols() );
  ASSERT_EQ( input1.rows(), disparity_map.rows() );
  check_error( disparity_map, .91, .990, "Absolute Difference" );

  disparity_map =
    pyramid_correlate( input1, input2,
                       constant_view(uint8(255), input1),
                       constant_view(uint8(255), input2),
                       PREFILTER_NONE, 0,
                       search_volume, kernel_size,
                       SQUARED_DIFFERENCE,
                       corr_timeout, seconds_per_op,
                       -1, max_levels );
  ASSERT_EQ( input1.cols(), disparity_map.cols() );
  ASSERT_EQ( input1.rows(), disparity_map.rows() );
  check_error( disparity_map, .90, .9985, "Squared Difference" );

  disparity_map =
    pyramid_correlate( input1, input2,
                       constant_view(uint8(255), input1),
                       constant_view(uint8(255), input2),
                       PREFILTER_NONE, 0,
                       search_volume, kernel_size,
                       SQUARED_DIFFERENCE,
                       corr_timeout, seconds_per_op,
                       2, max_levels );
  ASSERT_EQ( input1.cols(), disparity_map.cols() );
  ASSERT_EQ( input1.rows(), disparity_map.rows() );
  check_error( disparity_map, .90, .990, "Squared Difference" );

  disparity_map =
    pyramid_correlate( input1, input2,
                       constant_view(uint8(255), input1),
                       constant_view(uint8(255), input2),
                       PREFILTER_NONE, 0,
                       search_volume, kernel_size,
                       CROSS_CORRELATION,
                       corr_timeout, seconds_per_op,
                       -1, max_levels );
  ASSERT_EQ( input1.cols(), disparity_map.cols() );
  ASSERT_EQ( input1.rows(), disparity_map.rows() );
  check_error( disparity_map, .90, .998, "Cross Correlation" );

  disparity_map =
    pyramid_correlate( input1, input2,
                       constant_view(uint8(255), input1),
                       constant_view(uint8(255), input2),
                       PREFILTER_NONE, 0,
                       search_volume, kernel_size,
                       CROSS_CORRELATION,
                       corr_timeout, seconds_per_op,
                       2, max_levels );
  ASSERT_EQ( input1.cols(), disparity_map.cols() );
  ASSERT_EQ( input1.rows(), disparity_map.rows() );
  check_error( disparity_map, .90, .990, "Cross Correlation" );
}

TEST_F( PyramidViewGRAYI16, NullPreprocess ) {
  ImageView<PixelMask<Vector2i> > disparity_map =
    pyramid_correlate( input1, input2,
                       constant_view(uint8(255), input1),
                       constant_view(uint8(255), input2),
                       PREFILTER_NONE, 0,
                       search_volume, kernel_size,
                       ABSOLUTE_DIFFERENCE,
                       corr_timeout, seconds_per_op,
                       -1, max_levels );
  ASSERT_EQ( input1.cols(), disparity_map.cols() );
  ASSERT_EQ( input1.rows(), disparity_map.rows() );
  check_error( disparity_map, .909, .9985, "Absolute Difference" );

  disparity_map =
    pyramid_correlate( input1, input2,
                       constant_view(uint8(255), input1),
                       constant_view(uint8(255), input2),
                       PREFILTER_NONE, 0,
                       search_volume, kernel_size,
                       ABSOLUTE_DIFFERENCE,
                       corr_timeout, seconds_per_op,
                       2, max_levels );
  ASSERT_EQ( input1.cols(), disparity_map.cols() );
  ASSERT_EQ( input1.rows(), disparity_map.rows() );
  check_error( disparity_map, .91, .990, "Absolute Difference" );

  disparity_map =
    pyramid_correlate( input1, input2,
                       constant_view(uint8(255), input1),
                       constant_view(uint8(255), input2),
                       PREFILTER_NONE, 0,
                       search_volume, kernel_size,
                       SQUARED_DIFFERENCE,
                       corr_timeout, seconds_per_op,
                       -1, max_levels );
  ASSERT_EQ( input1.cols(), disparity_map.cols() );
  ASSERT_EQ( input1.rows(), disparity_map.rows() );
  check_error( disparity_map, .90, .9985, "Squared Difference" );

  disparity_map =
    pyramid_correlate( input1, input2,
                       constant_view(uint8(255), input1),
                       constant_view(uint8(255), input2),
                       PREFILTER_NONE, 0,
                       search_volume, kernel_size,
                       SQUARED_DIFFERENCE,
                       corr_timeout, seconds_per_op,
                       2, max_levels );
  ASSERT_EQ( input1.cols(), disparity_map.cols() );
  ASSERT_EQ( input1.rows(), disparity_map.rows() );
  check_error( disparity_map, .90, .990, "Squared Difference" );

  disparity_map =
    pyramid_correlate( input1, input2,
                       constant_view(uint8(255), input1),
                       constant_view(uint8(255), input2),
                       PREFILTER_NONE, 0,
                       search_volume, kernel_size,
                       CROSS_CORRELATION,
                       corr_timeout, seconds_per_op,
                       -1, max_levels );
  ASSERT_EQ( input1.cols(), disparity_map.cols() );
  ASSERT_EQ( input1.rows(), disparity_map.rows() );
  check_error( disparity_map, .87, .99, "Cross Correlation" );

  disparity_map =
    pyramid_correlate( input1, input2,
                       constant_view(uint8(255), input1),
                       constant_view(uint8(255), input2),
                       PREFILTER_NONE, 0,
                       search_volume, kernel_size,
                       CROSS_CORRELATION,
                       corr_timeout, seconds_per_op,
                       2, max_levels );
  ASSERT_EQ( input1.cols(), disparity_map.cols() );
  ASSERT_EQ( input1.rows(), disparity_map.rows() );
  check_error( disparity_map, .87, .99, "Cross Correlation" );
}

TEST_F( PyramidViewGRAYF32, NullPreprocess ) {
  ImageView<PixelMask<Vector2i> > disparity_map =
    pyramid_correlate( input1, input2,
                       constant_view(uint8(255), input1),
                       constant_view(uint8(255), input2),
                       PREFILTER_NONE, 0,
                       search_volume, kernel_size,
                       ABSOLUTE_DIFFERENCE,
                       corr_timeout, seconds_per_op,
                       -1, max_levels );
  ASSERT_EQ( input1.cols(), disparity_map.cols() );
  ASSERT_EQ( input1.rows(), disparity_map.rows() );
  check_error( disparity_map, .909, .9985, "Absolute Difference" );

  disparity_map =
    pyramid_correlate( input1, input2,
                       constant_view(uint8(255), input1),
                       constant_view(uint8(255), input2),
                       PREFILTER_NONE, 0,
                       search_volume, kernel_size,
                       ABSOLUTE_DIFFERENCE,
                       corr_timeout, seconds_per_op,
                       2, max_levels );
  ASSERT_EQ( input1.cols(), disparity_map.cols() );
  ASSERT_EQ( input1.rows(), disparity_map.rows() );
  check_error( disparity_map, .91, .990, "Absolute Difference" );

  disparity_map =
    pyramid_correlate( input1, input2,
                       constant_view(uint8(255), input1),
                       constant_view(uint8(255), input2),
                       PREFILTER_NONE, 0,
                       search_volume, kernel_size,
                       SQUARED_DIFFERENCE,
                       corr_timeout, seconds_per_op,
                       -1, max_levels );
  ASSERT_EQ( input1.cols(), disparity_map.cols() );
  ASSERT_EQ( input1.rows(), disparity_map.rows() );
  check_error( disparity_map, .90, .9985, "Squared Difference" );

  disparity_map =
    pyramid_correlate( input1, input2,
                       constant_view(uint8(255), input1),
                       constant_view(uint8(255), input2),
                       PREFILTER_NONE, 0,
                       search_volume, kernel_size,
                       SQUARED_DIFFERENCE,
                       corr_timeout, seconds_per_op,
                       2, max_levels );
  ASSERT_EQ( input1.cols(), disparity_map.cols() );
  ASSERT_EQ( input1.rows(), disparity_map.rows() );
  check_error( disparity_map, .90, .990, "Squared Difference" );

  disparity_map =
    pyramid_correlate( input1, input2,
                       constant_view(uint8(255), input1),
                       constant_view(uint8(255), input2),
                       PREFILTER_NONE, 0,
                       search_volume, kernel_size,
                       CROSS_CORRELATION,
                       corr_timeout, seconds_per_op,
                       -1, max_levels );
  ASSERT_EQ( input1.cols(), disparity_map.cols() );
  ASSERT_EQ( input1.rows(), disparity_map.rows() );
  check_error( disparity_map, .90, .998, "Cross Correlation" );

  disparity_map =
    pyramid_correlate( input1, input2,
                       constant_view(uint8(255), input1),
                       constant_view(uint8(255), input2),
                       PREFILTER_NONE, 0,
                       search_volume, kernel_size,
                       CROSS_CORRELATION,
                       corr_timeout, seconds_per_op,
                       2, max_levels );
  ASSERT_EQ( input1.cols(), disparity_map.cols() );
  ASSERT_EQ( input1.rows(), disparity_map.rows() );
  check_error( disparity_map, .90, .990, "Cross Correlation" );
}

TEST_F( PyramidViewU8, NullPreprocess ) {
  ImageView<PixelMask<Vector2i> > disparity_map =
    pyramid_correlate( input1, input2,
                       constant_view(uint8(255), input1),
                       constant_view(uint8(255), input2),
                       PREFILTER_NONE, 0,
                       search_volume, kernel_size,
                       ABSOLUTE_DIFFERENCE,
                       corr_timeout, seconds_per_op,
                       -1, max_levels );
  ASSERT_EQ( input1.cols(), disparity_map.cols() );
  ASSERT_EQ( input1.rows(), disparity_map.rows() );
  check_error( disparity_map, .909, .9985, "Absolute Difference" );

  disparity_map =
    pyramid_correlate( input1, input2,
                       constant_view(uint8(255), input1),
                       constant_view(uint8(255), input2),
                       PREFILTER_NONE, 0,
                       search_volume, kernel_size,
                       ABSOLUTE_DIFFERENCE,
                       corr_timeout, seconds_per_op,
                       2, max_levels );
  ASSERT_EQ( input1.cols(), disparity_map.cols() );
  ASSERT_EQ( input1.rows(), disparity_map.rows() );
  check_error( disparity_map, .91, .990, "Absolute Difference" );

  disparity_map =
    pyramid_correlate( input1, input2,
                       constant_view(uint8(255), input1),
                       constant_view(uint8(255), input2),
                       PREFILTER_NONE, 0,
                       search_volume, kernel_size,
                       SQUARED_DIFFERENCE,
                       corr_timeout, seconds_per_op,
                       -1, max_levels );
  ASSERT_EQ( input1.cols(), disparity_map.cols() );
  ASSERT_EQ( input1.rows(), disparity_map.rows() );
  check_error( disparity_map, .90, .9985, "Squared Difference" );

  disparity_map =
    pyramid_correlate( input1, input2,
                       constant_view(uint8(255), input1),
                       constant_view(uint8(255), input2),
                       PREFILTER_NONE, 0,
                       search_volume, kernel_size,
                       SQUARED_DIFFERENCE,
                       corr_timeout, seconds_per_op,
                       2, max_levels );
  ASSERT_EQ( input1.cols(), disparity_map.cols() );
  ASSERT_EQ( input1.rows(), disparity_map.rows() );
  check_error( disparity_map, .90, .990, "Squared Difference" );

  disparity_map =
    pyramid_correlate( input1, input2,
                       constant_view(uint8(255), input1),
                       constant_view(uint8(255), input2),
                       PREFILTER_NONE, 0,
                       search_volume, kernel_size,
                       CROSS_CORRELATION,
                       corr_timeout, seconds_per_op,
                       -1, max_levels );
  ASSERT_EQ( input1.cols(), disparity_map.cols() );
  ASSERT_EQ( input1.rows(), disparity_map.rows() );
  check_error( disparity_map, .90, .998, "Cross Correlation" );

  disparity_map =
    pyramid_correlate( input1, input2,
                       constant_view(uint8(255), input1),
                       constant_view(uint8(255), input2),
                       PREFILTER_NONE, 0,
                       search_volume, kernel_size,
                       CROSS_CORRELATION,
                       corr_timeout, seconds_per_op,
                       2, max_levels );
  ASSERT_EQ( input1.cols(), disparity_map.cols() );
  ASSERT_EQ( input1.rows(), disparity_map.rows() );
  check_error( disparity_map, .90, .990, "Cross Correlation" );
}