//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_OPERATOR_FUNCTIONS_SIMD_SSE_AVX_IF_ELSE_HPP_INCLUDED
#define BOOST_SIMD_OPERATOR_FUNCTIONS_SIMD_SSE_AVX_IF_ELSE_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_AVX_SUPPORT

#include <boost/simd/operator/functions/if_else.hpp>
#include <boost/simd/include/functions/simd/bitwise_cast.hpp>
#include <boost/simd/include/functions/simd/genmask.hpp>
#include <boost/dispatch/meta/as_floating.hpp>
#include <boost/dispatch/attributes.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( if_else_
                                    , boost::simd::tag::avx_
                                    , (A0)(A1)
                                    , ((simd_<logical_<A0>, boost::simd::tag::avx_>))
                                      ((simd_<single_<A1> , boost::simd::tag::avx_>))
                                      ((simd_<single_<A1> , boost::simd::tag::avx_>))
                                    )
  {
    typedef A1 result_type;

    BOOST_FORCEINLINE
    result_type operator()(A0 const& a0,A1 const& a1,A1 const& a2) const
    {
      return _mm256_blendv_ps(a2, a1, bitwise_cast<A1>(genmask(a0)));
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( if_else_
                                    , boost::simd::tag::avx_
                                    , (A0)(A1)
                                    , ((simd_<logical_<A0>, boost::simd::tag::avx_>))
                                      ((simd_<double_<A1> , boost::simd::tag::avx_>))
                                      ((simd_<double_<A1> , boost::simd::tag::avx_>))
                                    )
  {
    typedef A1 result_type;

    BOOST_FORCEINLINE
    result_type operator()(A0 const& a0,A1 const& a1,A1 const& a2) const
    {
      return _mm256_blendv_pd(a2, a1, bitwise_cast<A1>(genmask(a0)));
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( if_else_
                                    , boost::simd::tag::avx_
                                    , (A0)(A1)
                                    , ((simd_<logical_<A0>, boost::simd::tag::avx_>))
                                      ((simd_<floating_sized_<A1>, boost::simd::tag::avx_>))
                                      ((simd_<floating_sized_<A1>, boost::simd::tag::avx_>))
                                    )
  {
    typedef A1 result_type;

    BOOST_FORCEINLINE
    result_type operator()(A0 const& a0,A1 const& a1,A1 const& a2) const
    {
      typedef typename dispatch::meta::as_floating<A1>::type ctype;

      return bitwise_cast<A1> ( if_else ( a0
                                        , bitwise_cast<ctype>(a1)
                                        , bitwise_cast<ctype>(a2)
                                        )
                              );
    }
  };
} } }

#endif
#endif


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

#include "main.h"
#include "init.h"
#include "net.h"
#include "wallet.h"
#include "walletdb.h"
#include "bitcoinrpc.h"
#include "alert.h"
#include "enigma/enigmaann.h"
#include "base58.h"
#include <boost/algorithm/string.hpp>

using namespace json_spirit;
using namespace std;

extern CCloakingEncryptionKey* CurrentPosaKey;
extern int64 nNodeStartTime;
extern void TxToJSON(const CTransaction& tx, const uint256 hashBlock, json_spirit::Object& entry);
//extern bool static ProcessMessage(CNode* pfrom, string strCommand, CDataStream& vRecv);

extern CBitcoinAddress GetAccountAddress(string strAccount, bool bForceNew);

double GetDifficulty(const CBlockIndex* blockindex)
{
    // Floating point number that is a multiple of the minimum difficulty,
    // minimum difficulty = 1.0.
    if (blockindex == NULL)
    {
        if (pindexBest == NULL)
            return 1.0;
        else
            blockindex = GetLastBlockIndex(pindexBest, false);
    }

    int nShift = (blockindex->nBits >> 24) & 0xff;

    double dDiff =
        (double)0x0000ffff / (double)(blockindex->nBits & 0x00ffffff);

    while (nShift < 29)
    {
        dDiff *= 256.0;
        nShift++;
    }
    while (nShift > 29)
    {
        dDiff /= 256.0;
        nShift--;
    }

    return dDiff;
}

double GetPoSKernelPS()
{
    int nPoSInterval = 72;
    double dStakeKernelsTriedAvg = 0;
    int nStakesHandled = 0, nStakesTime = 0;

    CBlockIndex* pindex = pindexBest;;
    CBlockIndex* pindexPrevStake = NULL;

    while (pindex && nStakesHandled < nPoSInterval)
    {
        if (pindex->IsProofOfStake())
        {
            dStakeKernelsTriedAvg += GetDifficulty(pindex) * 4294967296.0;
            nStakesTime += pindexPrevStake ? (pindexPrevStake->nTime - pindex->nTime) : 0;
            pindexPrevStake = pindex;
            nStakesHandled++;
        }

        pindex = pindex->pprev;
    }

    return nStakesTime ? dStakeKernelsTriedAvg / nStakesTime : 0;
}

Object blockToJSON(const CBlock& block, const CBlockIndex* blockindex, bool fPrintTransactionDetail)
{
    Object result;
    result.push_back(Pair("hash", block.GetHash().GetHex()));
    CMerkleTx txGen(block.vtx[0]);
    txGen.SetMerkleBranch(&block);
    result.push_back(Pair("confirmations", (int)txGen.GetDepthInMainChain()));
    result.push_back(Pair("size", (int)::GetSerializeSize(block, SER_NETWORK, PROTOCOL_VERSION)));
    result.push_back(Pair("height", blockindex->nHeight));
    result.push_back(Pair("version", block.nVersion));
    result.push_back(Pair("merkleroot", block.hashMerkleRoot.GetHex()));
    result.push_back(Pair("mint", ValueFromAmount(blockindex->nMint)));
    result.push_back(Pair("time", (boost::int64_t)block.GetBlockTime()));
    result.push_back(Pair("nonce", (boost::uint64_t)block.nNonce));
    result.push_back(Pair("bits", HexBits(block.nBits)));
    result.push_back(Pair("difficulty", GetDifficulty(blockindex)));

    if (blockindex->pprev)
        result.push_back(Pair("previousblockhash", blockindex->pprev->GetBlockHash().GetHex()));
    if (blockindex->pnext)
        result.push_back(Pair("nextblockhash", blockindex->pnext->GetBlockHash().GetHex()));

    result.push_back(Pair("flags", strprintf("%s%s", blockindex->IsProofOfStake()? "proof-of-stake" : "proof-of-work", blockindex->GeneratedStakeModifier()? " stake-modifier": "")));
    result.push_back(Pair("proofhash", blockindex->IsProofOfStake()? blockindex->hashProofOfStake.GetHex() : blockindex->GetBlockHash().GetHex()));
    result.push_back(Pair("entropybit", (int)blockindex->GetStakeEntropyBit()));
    result.push_back(Pair("modifier", strprintf("%016" PRI64x, blockindex->nStakeModifier)));
    result.push_back(Pair("modifierchecksum", strprintf("%08x", blockindex->nStakeModifierChecksum)));
    Array txinfo;
    BOOST_FOREACH (const CTransaction& tx, block.vtx)
    {
        if (fPrintTransactionDetail)
        {
            Object entry;

            entry.push_back(Pair("txid", tx.GetHash().GetHex()));
            TxToJSON(tx, 0, entry);

            txinfo.push_back(entry);
        }
        else
            txinfo.push_back(tx.GetHash().GetHex());
    }

    result.push_back(Pair("tx", txinfo));
    result.push_back(Pair("signature", HexStr(block.vchBlockSig.begin(), block.vchBlockSig.end())));

    return result;
}

Value getblockcount(const Array& params, bool fHelp)
{
    if (fHelp || params.size() != 0)
        throw runtime_error(
            "getblockcount\n"
            "Returns the number of blocks in the longest block chain.");

    return nBestHeight;
}

Value getdifficulty(const Array& params, bool fHelp)
{
    if (fHelp || params.size() != 0)
        throw runtime_error(
            "getdifficulty\n"
            "Returns the difficulty as a multiple of the minimum difficulty.");

    Object obj;
    obj.push_back(Pair("proof-of-work",        GetDifficulty()));
    obj.push_back(Pair("proof-of-stake",       GetDifficulty(GetLastBlockIndex(pindexBest, true))));
    obj.push_back(Pair("search-interval",      (int)nLastCoinStakeSearchInterval));
    return obj;
}

Value settxfee(const Array& params, bool fHelp)
{
    if (fHelp || params.size() < 1 || params.size() > 1 || AmountFromValue(params[0]) < MIN_TX_FEE)
        throw runtime_error(
            "settxfee <amount>\n"
            "<amount> is a real and is rounded to the nearest 0.01");

    nTransactionFee = AmountFromValue(params[0]);
    nTransactionFee = (nTransactionFee / CENT) * CENT;  // round to cent

    return true;
}

Value getrawmempool(const Array& params, bool fHelp)
{
    if (fHelp || params.size() != 0)
        throw runtime_error(
            "getrawmempool\n"
            "Returns all transaction ids in memory pool.");

    vector<uint256> vtxid;
    mempool.queryHashes(vtxid);

    Array a;
    BOOST_FOREACH(const uint256& hash, vtxid)
        a.push_back(hash.ToString());

    return a;
}

Value getblockhash(const Array& params, bool fHelp)
{
    if (fHelp || params.size() != 1)
        throw runtime_error(
            "getblockhash <index>\n"
            "Returns hash of block in best-block-chain at <index>.");

    int nHeight = params[0].get_int();
    if (nHeight < 0 || nHeight > nBestHeight)
        throw runtime_error("Block number out of range.");

    CBlockIndex* pblockindex = FindBlockByHeight(nHeight);
    return pblockindex->phashBlock->GetHex();
}

Value getblock(const Array& params, bool fHelp)
{
    if (fHelp || params.size() < 1 || params.size() > 2)
        throw runtime_error(
            "getblock <hash> [txinfo]\n"
            "txinfo optional to print more detailed tx info\n"
            "Returns details of a block with given block-hash.");

    std::string strHash = params[0].get_str();
    uint256 hash(strHash);

    if (mapBlockIndex.count(hash) == 0)
        throw JSONRPCError(RPC_INVALID_ADDRESS_OR_KEY, "Block not found");

    CBlock block;
    CBlockIndex* pblockindex = mapBlockIndex[hash];
    block.ReadFromDisk(pblockindex, true);

    return blockToJSON(block, pblockindex, params.size() > 1 ? params[1].get_bool() : false);
}

Value getblockbynumber(const Array& params, bool fHelp)
{
    if (fHelp || params.size() < 1 || params.size() > 2)
        throw runtime_error(
            "getblock <number> [txinfo]\n"
            "txinfo optional to print more detailed tx info\n"
            "Returns details of a block with given block-number.");

    int nHeight = params[0].get_int();
    if (nHeight < 0 || nHeight > nBestHeight)
        throw runtime_error("Block number out of range.");

    CBlock block;
    CBlockIndex* pblockindex = mapBlockIndex[hashBestChain];
    while (pblockindex->nHeight > nHeight)
        pblockindex = pblockindex->pprev;

    uint256 hash = *pblockindex->phashBlock;

    pblockindex = mapBlockIndex[hash];
    block.ReadFromDisk(pblockindex, true);

    return blockToJSON(block, pblockindex, params.size() > 1 ? params[1].get_bool() : false);
}

// ppcoin: get information of sync-checkpoint
Value getcheckpoint(const Array& params, bool fHelp)
{
    if (fHelp || params.size() != 0)
        throw runtime_error(
            "getcheckpoint\n"
            "Show info of synchronized checkpoint.\n");

    Object result;
    CBlockIndex* pindexCheckpoint;

    result.push_back(Pair("synccheckpoint", Checkpoints::hashSyncCheckpoint.ToString().c_str()));
    pindexCheckpoint = mapBlockIndex[Checkpoints::hashSyncCheckpoint];
    result.push_back(Pair("height", pindexCheckpoint->nHeight));
    result.push_back(Pair("timestamp", DateTimeStrFormat(pindexCheckpoint->GetBlockTime()).c_str()));
    if (mapArgs.count("-checkpointkey"))
        result.push_back(Pair("checkpointmaster", true));

    return result;
}

Value dumpenigmap(const Array& params, bool fHelp)
{
    if (fHelp)
        throw runtime_error(
            "dumpenigmap\n");

    Object result;

    BOOST_FOREACH(const uint256 hash, pwalletMain->setEnigmaProcessed)
    {
	result.push_back(Pair("hash", hash.GetHex()));
    }

    return result;
}

Value enstr(const Array& params, bool fHelp)
{
    if (fHelp || params.size() < 1)
        throw runtime_error(
            "enstr <loot>\n");

    Object result;

    string strTxtIn = params[0].get_str();
    string strMarker = "70352205";
    std::stringstream strConcat;
    strConcat << strTxtIn << strMarker;

    string strTxt = strConcat.str();
    std::vector<CScript> hashes;

    std::stringstream strDecode;
    for(unsigned int i=0; i<=strTxt.length()/40; i++)
    {
        if((unsigned int)i*40 == strTxt.length())
	    break;

	string strPiece = strTxt.substr(i*40,40);
        while(strPiece.length() < 40)
        {
            strPiece = strPiece + "0";
        }
	result.push_back(Pair("piece", strPiece));

        uint160 data(strPiece);
	CScript dataScript;
        dataScript << OP_DUP << OP_HASH160 << data << OP_EQUALVERIFY << OP_CHECKSIG;
	result.push_back(Pair("hash", data.GetHex()));
        result.push_back(Pair("CScript", dataScript.ToString()));
	result.push_back(Pair("CScript datastring", dataScript.ToDataString()));
        uint160 datahash(dataScript.ToDataString());
        result.push_back(Pair("datastring hash", datahash.GetHex()));

	std::stringstream strExtract;
        strExtract << dataScript.ToDataString();
        string strExtracted = strExtract.str();
        std::stringstream strReversed;

        for(int i=strExtracted.length()-2;i>=0;i=i-2)
        {
	    strReversed << strExtracted.substr(i,2);
        }

	strDecode << strReversed.str();
        result.push_back(Pair("Extracted", strReversed.str()));
    }

    string strDecoded = strDecode.str();
    string strOutput = strDecoded.substr(0, strDecoded.find(strMarker));
    result.push_back(Pair("Decoded", strOutput));

    CEnigmaTransaction enigmaTx;
    CDataStream ssTx(ParseHex(strOutput), SER_NETWORK, PROTOCOL_VERSION); // ssTx(SER_NETWORK, PROTOCOL_VERSION);
    //ssTx << ParseHex(strOutput); //ParseHex(strOutput);
    ssTx >> enigmaTx;

    result.push_back(Pair("ENIGMA TX", enigmaTx.ToString()));
    return result;
}

Value destr(const Array& params, bool fHelp)
{
    if (fHelp || params.size() < 1)
        throw runtime_error(
            "destr <loot>\n");

    Object result;

    return result;
}

Value hexposa(const Array& params, bool fHelp)
{
    if (fHelp || params.size() < 1 || params.size() > 3)
        throw runtime_error(
            "hexenigma <amount> <realdestinationaddress> <phase>\n");

    // TODO: this looks b0rked/iffy - investigate!
    Object result;
    int hop = 0; //params[2].get_int();
    string strAddress = "mkgwGsCLCWDpFKXWWTiDn4xcvzDicHapDt";// params[1].get_str();

    // Amount
    int64 nAmount = AmountFromValue(2.12345); //AmountFromValue(params[0]);

    if (nAmount < MIN_TXOUT_AMOUNT)
        throw JSONRPCError(-101, "Send amount too small");

    CBitcoinAddress address(strAddress);
    if (!address.IsValid())
        throw JSONRPCError(RPC_INVALID_ADDRESS_OR_KEY, "Invalid CloakCoin address");

    result.push_back(Pair("addr", strAddress));
    //result.push_back(Pair("amount", nAmount.ToString()));
    result.push_back(Pair("hop", hop));

    CScript posaScriptPubKey;
    posaScriptPubKey.SetDestination(address.Get());

    // Create enigma transaction object
    CEnigmaTransaction enigmaTx;
    CEnigmaTxOut enigmaTxOut;

    enigmaTxOut.nHop = hop;
    enigmaTxOut.nValue = nAmount;
    enigmaTxOut.scriptPubKey = posaScriptPubKey;

    enigmaTx.vout.push_back(enigmaTxOut);
    // Serialize the enigma transaction object to a string
    CDataStream ssTx(SER_NETWORK, PROTOCOL_VERSION);
    ssTx << enigmaTx;

    // Get the hex formatted string
    string enigmatxstring = HexStr(ssTx.begin(), ssTx.end());

    // populate result
    result.push_back(Pair("enigma", enigmatxstring));
    result.push_back(Pair("ENIGMA TX", enigmaTx.ToString()));
    return result;
}

// send our Enigma details to the Enigma network so other nodes are aware of us.
void broadcastenigma()
{
    if(!fEnigma)
        return;

    if(GetBoolArg("-printenigma"))
        printf("ENIGMA: broadcasting Enigma announcement\n");

    if (!CCloakShield::GetShield()){
        // cloakshield not initialized yet
        return;
    }

    CCloakingEncryptionKey* key = CCloakShield::GetShield()->GetRoutingKey();

    if(GetBoolArg("-printenigma"))
        printf("ENIGMA: Enigma pubkey: %s\n", key->pubkeyHex.c_str());

    // create the ann
    CEnigmaAnnouncement ann(key);

    if(GetBoolArg("-printenigma"))
        printf("ENIGMA: Constructing ann\n");

    nMyEnigmaAddress = ann.pEnigmaSessionKeyHex;

    if(GetBoolArg("-printenigma")){
        printf("ENIGMA: ann: %s\n", ann.GetSigningHash().ToString().c_str());
        printf("ENIGMA: ann process message\n");
    }

    if(GetBoolArg("-printenigma")){
        printf("ENIGMA: relaying to peers.\n");
    }

    mapEnigmaAnns[ann.GetKeyHash()] = ann;

    // Relay
    {
        LOCK(cs_vNodes);
        BOOST_FOREACH(CNode* pnode, vNodes)
        ann.RelayTo(pnode);
    }
}


//===- ${OperatorName}.cc -------------------------------------------------===//
//
//                       The CIM Hardware Simulator Project
//
// See LICENSE.TXT for details.
//
//===----------------------------------------------------------------------===//
#include "${OperatorName}.hh"
#include "diagnostic/msgHandling.hh"
#include <Eigen/Dense>

namespace cimHW {

cimHW${OperatorName}Op::cimHW${OperatorName}Op(
  const std::string &configFile
    ${argument_list}
  )
	: cimHWOp("${OperatorName}")
  , m_cimCU(configFile)
{
  verbose1(opName());
}

void cimHW${OperatorName}Op::simulate()
{
  // copy back to output
  //memcpy(m_output_Y, result.data(), sizeof(numType) * ;
  verbose1("\n");
}

} // namespace cimHW


/**
 * Copyright (c) 2018 Melown Technologies SE
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * *  Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * *  Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include <boost/lexical_cast.hpp>

#include "utility/enum-io.hpp"

#include "jsoncpp/json.hpp"
#include "jsoncpp/as.hpp"

#include "./heightfunction.hpp"

#include "./error.hpp"
#include "./support/python.hpp"

SuperElevation::SuperElevation(const Config &config)
    : config_(config), s_()
{
    if (config_.heightRange.min >= config_.heightRange.max) {
        LOGTHROW(err2, std::logic_error)
            << "Invalid superelevation configuration (height range is "
            << config_.heightRange << ").";
    }

    s_ = ((config_.scaleRange.max - config_.scaleRange.min)
          / (config_.heightRange.max - config_.heightRange.min));
    limits_.min = apply(config_.heightRange.min);
    limits_.max = apply(config_.heightRange.max);
}

bool SuperElevation::Config::changed(const Config &other) const
{
    if (heightRange != other.heightRange) { return true; }
    if (scaleRange != other.scaleRange) { return true; }
    return false;
}

bool SuperElevation::changed(const HeightFunction::pointer &other) const
{
    if (!other) { return true; }
    if (const auto *o = dynamic_cast<const SuperElevation*>(other.get())) {
        // same type compare configs
        return config_.changed(o->config_);
    }
    // different type
    return true;
}

namespace {

UTILITY_GENERATE_ENUM(HeightFunctionType,
    ((superelevation))
)

void parse(SuperElevation::Config &config, const Json::Value &value)
{
    Json::get(config.heightRange.min, value, "heightRange", 0);
    Json::get(config.heightRange.max, value, "heightRange", 1);
    Json::get(config.scaleRange.min, value, "scaleRange", 0);
    Json::get(config.scaleRange.max, value, "scaleRange", 1);
}

void buildJson(Json::Value &value, const SuperElevation::Config &config)
{
    value["function"] = boost::lexical_cast<std::string>
        (HeightFunctionType::superelevation);

    auto &heightRange(value["heightRange"] = Json::arrayValue);
    heightRange.append(config.heightRange.min);
    heightRange.append(config.heightRange.max);

    auto &scaleRange(value["scaleRange"] = Json::arrayValue);
    scaleRange.append(config.scaleRange.min);
    scaleRange.append(config.scaleRange.max);
}

void parse(SuperElevation::Config &config, const boost::python::dict &value)
{
    const auto &heightRange(value["heightRange"]);
    config.heightRange.min
        = boost::python::extract<double>(heightRange[0]);
    config.heightRange.max
        = boost::python::extract<double>(heightRange[1]);

    const auto &scaleRange(value["scaleRange"]);
    config.scaleRange.min
        = boost::python::extract<double>(scaleRange[0]);
    config.scaleRange.max
        = boost::python::extract<double>(scaleRange[1]);
}

HeightFunction::pointer
parseJson(const Json::Value &value, const std::string &key)
{
    if (!value.isMember(key)) { return {}; }

    const auto &content(value[key]);

    HeightFunctionType type{};
    Json::get(type, content, "function");

    switch (type) {
    case HeightFunctionType::superelevation: {
        SuperElevation::Config config;
        parse(config, content);
        return std::make_shared<SuperElevation>(config);
    } break;
    }

    // never reached
    throw;
}

HeightFunction::pointer parsePy(const boost::python::dict &value
                                , const std::string &key)
{
    if (!value.has_key(key)) { return {}; }

    boost::python::dict content(value[key]);

    const auto type(boost::lexical_cast<HeightFunctionType>
                    (py2utf8(content["function"])));

    switch (type) {
    case HeightFunctionType::superelevation: {
        SuperElevation::Config config;
        parse(config, content);
        return std::make_shared<SuperElevation>(config);
    } break;
    }

    // never reached
    throw;
}

} // namespace

HeightFunction::pointer HeightFunction::parse(const boost::any &value
                                              , const std::string &key)
{
    if (const auto *json = boost::any_cast<Json::Value>(&value)) {
        return parseJson(*json, key);
    } else if (const auto *py = boost::any_cast<boost::python::dict>(&value)) {
        return parsePy(*py, key);
    }

    LOGTHROW(err1, Error)
        << "Function: Unsupported configuration from: <"
        << value.type().name() << ">.";
    throw;
}

void SuperElevation::build(boost::any &value) const
{
    if (auto *json = boost::any_cast<Json::Value>(&value)) {
        buildJson(*json, config_);
    } else {
        LOGTHROW(err1, Error)
            << "SuperElevation:: Unsupported serialization into: <"
            << value.type().name() << ">.";
    }
}

bool HeightFunction::changed(const HeightFunction::pointer &l
                             , const HeightFunction::pointer &r)
{
    if (!l) { return bool(r); }
    return l->changed(r);
}


/*
 * ExtractMagnitudeGadget.cpp
 *
 *  Created on: Nov 8, 2011
 *      Author: Michael S. Hansen
 */

#include <bitset>
#include <gadgetron/hoNDArray_math.h>
#include <unordered_map>

#include "ExtractMP2RAGEGadget.h"
#include <boost/math/constants/constants.hpp>

#include "hoNDArray_fileio.h"
#include <gadgetron/mri_core_def.h>

namespace Gadgetron {

    namespace {
        using IMTYPE = ISMRMRD::ISMRMRD_ImageTypes;

        const std::map<IMTYPE, size_t> series_offset{ { IMTYPE::ISMRMRD_IMTYPE_MAGNITUDE, 0 },
            { IMTYPE::ISMRMRD_IMTYPE_REAL, 1000 }, { IMTYPE::ISMRMRD_IMTYPE_IMAG, 2000 },
            { IMTYPE::ISMRMRD_IMTYPE_PHASE, 3000 } };

        const std::map<int, IMTYPE> mask_to_imtype{ { 0, IMTYPE::ISMRMRD_IMTYPE_MAGNITUDE },
            { 1, IMTYPE::ISMRMRD_IMTYPE_REAL }, { 2, IMTYPE::ISMRMRD_IMTYPE_IMAG },
            { 3, IMTYPE::ISMRMRD_IMTYPE_PHASE } };

        template <class FUNCTION>
        hoNDArray<float> extract(const hoNDArray<float>& data, FUNCTION&& extractor) {
            hoNDArray<float> output(data.dimensions());
            std::transform(data.begin(), data.end(), output.begin(), extractor);
            return output;
        }

        hoNDArray<float> extract_image(const hoNDArray<float>& data, IMTYPE imtype, float offset) {
            switch (imtype) {
            case ISMRMRD::ISMRMRD_IMTYPE_MAGNITUDE: return extract(data, [](auto& val) { return std::abs(val); });
            case ISMRMRD::ISMRMRD_IMTYPE_PHASE:
                return extract(data, [](auto& val) { return std::arg(val) + boost::math::constants::pi<float>(); });
            case ISMRMRD::ISMRMRD_IMTYPE_REAL: return extract(data, [&](auto& val) { return std::real(val) + offset; });
            case ISMRMRD::ISMRMRD_IMTYPE_IMAG: return extract(data, [&](auto& val) { return std::imag(val) + offset; });
            default: throw std::runtime_error("Illegal image tpye encounted in extract_image");
            }
        }

    }

    void ExtractMP2RAGEGadget::process(Core::InputChannel<Core::Image<float>>& in, Core::OutputChannel& out) {
        GDEBUG("----------EXTRACT_MP2RAGE_PROCESS----------------\n");

        for (auto image : in) {
            int count =0;
            count = count +1;
            GDEBUG("----------Image in loop = %d----------------\n",count);
            const auto& head = std::get<ISMRMRD::ImageHeader>(image);
            const auto& data = std::get<hoNDArray<float>>(image);
            const auto& meta = std::get<2>(image);

            ISMRMRD::MetaContainer meta_copy = meta.value_or(ISMRMRD::MetaContainer());

            for (auto imtype : image_types) {
                auto data_copy       = extract_image(data, imtype, real_imag_offset);
                auto head_copy       = head;

                if (head.image_series_index == 5000)
                {
                  GDEBUG("------TI1 IMAGE------\n");
                  meta_copy.set(GADGETRON_IMAGECOMMENT,"Magn_TI1_CS");
                  meta_copy.set(GADGETRON_SEQUENCEDESCRIPTION, "Magn_TI1_CS");
                  meta_copy.set(GADGETRON_IMAGEPROCESSINGHISTORY, "GT");
                }
                else if (head.image_series_index == 6000)
                {
                  GDEBUG("------TI2 IMAGE------\n");
                  meta_copy.set(GADGETRON_IMAGECOMMENT, "Magn_TI2_CS");
                  meta_copy.set(GADGETRON_SEQUENCEDESCRIPTION, "Magn_TI2_CS");
                  meta_copy.set(GADGETRON_IMAGEPROCESSINGHISTORY, "GT");
                }
                else if (head.image_series_index ==7000)
                {
                  GDEBUG("------MP2RAGE IMAGE------\n");
                  meta_copy.set(GADGETRON_IMAGECOMMENT, "MP2RAGE");
                  meta_copy.set(GADGETRON_SEQUENCEDESCRIPTION, "MP2RAGE");
                  meta_copy.set(GADGETRON_IMAGEPROCESSINGHISTORY, "GT");
                }
                else if(head.image_series_index ==8000)
                {
                  GDEBUG("------T1 map------\n");
                  meta_copy.set(GADGETRON_DATA_ROLE, GADGETRON_IMAGE_T1MAP);//
                  meta_copy.set(GADGETRON_IMAGECOMMENT, "T1_CS");
                  meta_copy.set(GADGETRON_SEQUENCEDESCRIPTION, "T1_CS");
                  //metaset(GADGETRON_SEQUENCEDESCRIPTION, GADGETRON_IMAGE_T1MAP);
                  meta_copy.set(GADGETRON_IMAGEPROCESSINGHISTORY, "GT");
                  //metaset(GADGETRON_IMAGE_COLORMAP,  "HotMetal.pal");
                  meta_copy.set(GADGETRON_IMAGE_COLORMAP,  "Perfusion.pal");//
                }
                else
                {
                  GDEBUG("------Undefined------\n");
                  meta_copy.set(GADGETRON_IMAGECOMMENT, "Undefined");
                  meta_copy.set(GADGETRON_SEQUENCEDESCRIPTION, "Undefined");
                  meta_copy.set(GADGETRON_IMAGEPROCESSINGHISTORY, "GT");
                }

                head_copy.data_type  = ISMRMRD::ISMRMRD_FLOAT;
                head_copy.image_type = imtype;
                head_copy.image_series_index += series_offset.at(imtype);
                out.push(head_copy, std::move(data_copy), meta_copy);
            }
        }
    }

    ExtractMP2RAGEGadget::ExtractMP2RAGEGadget(const Core::Context& context, const Core::GadgetProperties& props)
        : Core::ChannelGadget<Core::Image<float>>(context, props) {

        for (int i = 0; i < extract_mask.size(); i++) {
            if (extract_mask[i])
                image_types.insert(mask_to_imtype.at(i));
        }
        if (extract_magnitude)
            image_types.insert(IMTYPE::ISMRMRD_IMTYPE_MAGNITUDE);
        if (extract_real)
            image_types.insert(IMTYPE::ISMRMRD_IMTYPE_REAL);
        if (extract_imag)
            image_types.insert(IMTYPE::ISMRMRD_IMTYPE_IMAG);
        if (extract_phase)
            image_types.insert(IMTYPE::ISMRMRD_IMTYPE_PHASE);

        if (image_types.empty())
            throw std::runtime_error("ExctractGadget: No valid extract functions specified");
    }

    GADGETRON_GADGET_EXPORT(ExtractMP2RAGEGadget)

}


/************************************************************************/
/*                                                                      */
/*       Copyright 2011 by Ullrich Koethe and Michael Hanselmann        */
/*                                                                      */
/*    This file is part of the VIGRA computer vision library.           */
/*    The VIGRA Website is                                              */
/*        http://hci.iwr.uni-heidelberg.de/vigra/                       */
/*    Please direct questions, bug reports, and contributions to        */
/*        ullrich.koethe@iwr.uni-heidelberg.de    or                    */
/*        vigra@informatik.uni-hamburg.de                               */
/*                                                                      */
/*    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 copyrigfht 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.                                   */
/*                                                                      */
/************************************************************************/

#define PY_ARRAY_UNIQUE_SYMBOL vigranumpylearning_PyArray_API
// #define NO_IMPORT_ARRAY

#include <vigra/numpy_array.hxx>
#include <vigra/numpy_array_converters.hxx>
#include <vigra/unsupervised_decomposition.hxx>
#include <set>
#include <cmath>
#include <memory>
#include <boost/python.hpp>

namespace python = boost::python;
namespace vigra
{

template<class U>
python::tuple
pythonPCA(NumpyArray<2,U> features, int nComponents)
{
    vigra_precondition(!features.axistags(),
                       "principleComponents(): feature matrix must not have axistags\n"
                       "(use 'array.view(numpy.ndarray)' to remove them).");

    NumpyArray<2, U> fz(Shape2(features.shape(0), nComponents));
    NumpyArray<2, U> zv(Shape2(nComponents, features.shape(1)));

    {
        PyAllowThreads _pythread;
        principleComponents(features, fz, zv);
    }
    return python::make_tuple(fz, zv);
}

template<class U>
python::tuple
pythonPLSA(NumpyArray<2,U> features,
           int nComponents,
           int nIterations,
           double minGain,
           bool normalize)
{
    vigra_precondition(!features.axistags(),
                       "pLSA(): feature matrix must not have axistags\n"
                       "(use 'array.view(numpy.ndarray)' to remove them).");

    NumpyArray<2, U> fz(Shape2(features.shape(0), nComponents));
    NumpyArray<2, U> zv(Shape2(nComponents, features.shape(1)));

    {
        PyAllowThreads _pythread;
        pLSA(features, fz, zv,
             RandomNumberGenerator<>(),
             PLSAOptions().maximumNumberOfIterations(nIterations)
                          .minimumRelativeGain(minGain)
                          .normalizedComponentWeights(normalize));
    }
    return python::make_tuple(fz, zv);
}

void defineUnsupervised()
{
    using namespace python;

    docstring_options doc_options(true, true, false);

    def("principleComponents", registerConverters(&pythonPCA<double>),
        (arg("features"), arg("nComponents")),
        "\nPerform principle component analysis. \n\n"
        "The imput matrix 'features' must have shape (nFeatures*nSamples). PCA will\n"
        "reduce it to a smaller matrix 'C' with shape (nComponents*nSamples) that \n"
        "preserves as much variance as possible. Specifically, the call::\n\n"
        "    P, C = principleComponents(features, 3)\n\n"
        "returns a projection matrix 'P' with shape (nComponents*nFeatures)\n"
        "such that ``C = numpy.dot(numpy.transpose(P), features)``. Conversely, the\n"
        "matrix  ``f = numpy.dot(P, C)`` is the best possible rank-nComponents\n"
        "approximation to the matrix 'features' under the least-squares criterion.\n\n"
        "See principleComponents_ in the C++ documentation for more detailed\ninformation.\n\n");

    PLSAOptions options;

    def("pLSA", registerConverters(&pythonPLSA<double>),
        (arg("features"), arg("nComponents"), arg("nIterations") = options.max_iterations,
         arg("minGain") = options.min_rel_gain, arg("normalize") = options.normalized_component_weights),
        "\nPerform probabilistic latent semantic analysis. \n\n"
        "The imput matrix 'features' must have shape (nFeatures*nSamples). PCA will\n"
        "reduce it to a smaller matrix 'C' with shape (nComponents*nSamples) that \n"
        "preserves as much information as possible. Specifically, the call::\n\n"
        "    P, C = pLSA(features, 3)\n\n"
        "returns a projection matrix 'P' with shape (nComponents*nFeatures)\n"
        "such that the matrix ``f = numpy.dot(P, C)`` is a rank-nComponents matrix\n"
        "that approximates the matrix 'features' well under the pLSA criterion.\n"
        "Note that the result of pLSA() is not unique, since the algorithm uses random\n"
        "initialization.\n\n"
        "See pLSA_ in the C++ documentation for more detailed\ninformation.\n\n");
}

void defineRandomForest();
void defineRandomForestOld();

} // namespace vigra

using namespace vigra;
using namespace boost::python;

BOOST_PYTHON_MODULE_INIT(learning)
{
    import_vigranumpy();
    defineUnsupervised();
    defineRandomForest();
    defineRandomForestOld();
}


#include "../src/History.h"
#include "../src/KDOPBroadPhase.h"
#include "../src/Mesh.h"
#include "../src/SeparatingPlaneNarrowPhase.h"
#include "../src/CTCDNarrowPhase.h"
#include <iostream>
#include <fstream>
#include <vector>
#include "Distance.h"
#include <Eigen/Core>
#include "CTCD.h"

using namespace std;
using namespace Eigen;

void loadMesh(const char *filename, VectorXd &verts, Matrix3Xi &faces)
{
	ifstream ifs(filename);
	if(!ifs)
		return;

	vector<double> pos;
	vector<int> faceidx;

	while(true)
	{
		char c;
		ifs >> c;
		if(!ifs)
			break;
		if(c == 'v')
		{
			for(int i=0; i<3; i++)
			{
				double p;
				ifs >> p;
				pos.push_back(p);
			}
		}
		if(c == 'f')
		{
			for(int i=0; i<3; i++)
			{
				int vert;
				ifs >> vert;
				faceidx.push_back(vert);
			}
		}
	}

	int nverts = pos.size()/3;
	int nfaces = faceidx.size()/3;

	verts.resize(3*nverts);
	for(int i=0; i<3*nverts; i++)
		verts[i] = pos[i];

	faces.resize(3, nfaces);
	for(int i=0; i<nfaces; i++)
		for(int j=0; j<3; j++)
			faces.coeffRef(j, i) = faceidx[3*i+j]-1;
}

int main(int argc, char *argv[])
{
	if(argc != 3)
	{
		std::cerr << "Usage: AlecTest (initial mesh) (final mesh)" << std::endl;
		return -1;
	}
	VectorXd initialv;
	Matrix3Xi faces;
	loadMesh(argv[1], initialv, faces);
	int numcoarseverts = initialv.size()/3;
	std::cout << "Loaded initial mesh with " << numcoarseverts << " vertices and " << faces.cols() << " faces" << std::endl;

	VectorXd finalv;
	loadMesh(argv[2], finalv, faces);
	std::cout << "Loaded final mesh with " << finalv.size()/3 << " vertices and " << faces.cols() << " faces" << std::endl;

  // Build data structures: mesh and history (trajectories)
  Mesh m;
  m.faces = faces;
  // These are needed but ignored
  m.vertices = initialv;
  History h(initialv);
  h.finishHistory( finalv);

  // Broadphase to collect candidate vertex-face and edge-edge collisions
  const double outerEta = 1e-8;
  std::set<VertexFaceStencil> can_vfs;
  std::set<EdgeEdgeStencil> can_ees;
  std::set<std::pair<VertexFaceStencil,double> > can_vfs_eta;
  std::set<std::pair<EdgeEdgeStencil,double> > can_ees_eta;
  // No fixed vertices
  const std::set<int> fixedVerts;
  KDOPBroadPhase().findCollisionCandidates(h,m,outerEta,can_vfs,can_ees,fixedVerts);
  // Narrowphase
  const double innerEta = 1e-8;
  for(std::set<VertexFaceStencil>::iterator it = can_vfs.begin(); it != can_vfs.end(); ++it)
  {
    can_vfs_eta.insert(std::pair<VertexFaceStencil,double>(*it, innerEta));
  }
  for(std::set<EdgeEdgeStencil>::iterator it = can_ees.begin(); it != can_ees.end(); ++it)
  {
    can_ees_eta.insert(std::pair<EdgeEdgeStencil,double>(*it, innerEta));
  }
  std::set<VertexFaceStencil> ctcdvfs;
  {
    std::set<EdgeEdgeStencil> ees;
    CTCDNarrowPhase().findCollisions(h,can_vfs_eta,can_ees_eta,ctcdvfs,ees);
    std::cout<<"CTCDNarrowPhase:"<<std::endl;
    std::cout<<"  "<<ctcdvfs.size()<<" vf collisions"<<std::endl;
    std::cout<<"  "<<ees.size()<<" ee collisions"<<std::endl;
  }
  {
    std::set<VertexFaceStencil> vfs;
    std::set<EdgeEdgeStencil> ees;
    SeparatingPlaneNarrowPhase().findCollisions(h,can_vfs_eta,can_ees_eta,vfs,ees);
    std::cout<<"SeparatingPlaneNarrowPhase:"<<std::endl;
    std::cout<<"  "<<vfs.size()<<" vf collisions"<<std::endl;
    std::cout<<"  "<<ees.size()<<" ee collisions"<<std::endl;

   for(std::set<VertexFaceStencil>::iterator it = ctcdvfs.begin(); it != ctcdvfs.end(); ++it)
	{
		if(vfs.count(*it) == 0)
			cout << it->p << " " << it->q0 << " " << it->q1 << " " << it->q2 << " " << endl;
	}
  }
}


#include "QPCommon.h"
#include <Eigen/StdVector>
// #include <limits>
// #include <cmath>
// #include "drakeUtil.h"

using namespace std;

void parseIntegratorParams(const mxArray *params_obj, IntegratorParams &params) {
  const mxArray *pobj;
  pobj = myGetField(params_obj, "gains");
  Map<VectorXd> gains(mxGetPrSafe(pobj), mxGetNumberOfElements(pobj));
  params.gains = gains;

  pobj = myGetField(params_obj, "clamps");
  Map<VectorXd> clamps(mxGetPrSafe(pobj), mxGetNumberOfElements(pobj));
  params.clamps = clamps;

  pobj = myGetField(params_obj, "eta");
  params.eta = mxGetScalar(pobj);

  return;
}

void parseJointSoftLimits(const mxArray *params_obj, RigidBodyManipulator *r, JointSoftLimitParams *params) {
  int nq = r->num_positions;

  if (mxGetNumberOfElements(params_obj) != nq)
    mexErrMsgTxt("Joint soft limits should be of size nq");

  params->enabled = Matrix<bool, Dynamic, 1>::Zero(nq);
  params->disable_when_body_in_support = VectorXi::Zero(nq);
  params->lb = VectorXd::Zero(nq);
  params->ub = VectorXd::Zero(nq);
  params->kp = VectorXd::Zero(nq);
  params->kd = VectorXd::Zero(nq);
  params->weight = VectorXd::Zero(nq);
  params->k_logistic = VectorXd::Zero(nq);

  for (int i=0; i < nq; i++) {
    params->enabled(i) = static_cast<bool> (mxGetScalar(mxGetFieldSafe(params_obj, i, "enabled")));
    params->disable_when_body_in_support(i) = static_cast<int> (mxGetScalar(mxGetFieldSafe(params_obj, i, "disable_when_body_in_support")));
    params->lb(i) = mxGetScalar(mxGetFieldSafe(params_obj, i, "lb"));
    params->ub(i) = mxGetScalar(mxGetFieldSafe(params_obj, i, "ub"));
    params->kp(i) = mxGetScalar(mxGetFieldSafe(params_obj, i, "kp"));
    params->kd(i) = mxGetScalar(mxGetFieldSafe(params_obj, i, "kd"));
    params->weight(i) = mxGetScalar(mxGetFieldSafe(params_obj, i, "weight"));
    params->k_logistic(i) = mxGetScalar(mxGetFieldSafe(params_obj, i, "k_logistic"));
  }
  return;
}

void parseWholeBodyParams(const mxArray *params_obj, RigidBodyManipulator *r, WholeBodyParams *params) {
  const mxArray *int_obj = myGetField(params_obj, "integrator");
  const mxArray *qddbound_obj = myGetField(params_obj, "qdd_bounds");
  int nq = r->num_positions;
  int nv = r->num_velocities;

  if (mxGetNumberOfElements(myGetField(params_obj, "Kp")) != nq) mexErrMsgTxt("Kp should be of size nq");
  if (mxGetNumberOfElements(myGetField(params_obj, "Kd")) != nq) mexErrMsgTxt("Kd should be of size nq");
  if (mxGetNumberOfElements(myGetField(params_obj, "w_qdd")) != nv) mexErrMsgTxt("w_qdd should be of size nv");
  if (mxGetNumberOfElements(myGetField(int_obj, "gains")) != nq) mexErrMsgTxt("gains should be of size nq");
  if (mxGetNumberOfElements(myGetField(int_obj, "clamps")) != nq) mexErrMsgTxt("clamps should be of size nq");
  if (mxGetNumberOfElements(myGetField(qddbound_obj, "min")) != nv) mexErrMsgTxt("qdd min should be of size nv");
  if (mxGetNumberOfElements(myGetField(qddbound_obj, "max")) != nv) mexErrMsgTxt("qdd max should be of size nv");

  Map<VectorXd>Kp(mxGetPrSafe(myGetField(params_obj, "Kp")), mxGetNumberOfElements(myGetField(params_obj, "Kp")));
  params->Kp = Kp;

  Map<VectorXd>Kd(mxGetPrSafe(myGetField(params_obj, "Kd")), mxGetNumberOfElements(myGetField(params_obj, "Kd")));
  params->Kd = Kd;

  Map<VectorXd>w_qdd(mxGetPrSafe(myGetField(params_obj, "w_qdd")), mxGetNumberOfElements(myGetField(params_obj, "w_qdd")));
  params->w_qdd = w_qdd;

  parseIntegratorParams(int_obj, params->integrator);

  Map<VectorXd>min(mxGetPrSafe(myGetField(qddbound_obj, "min")), mxGetNumberOfElements(myGetField(qddbound_obj, "min")));
  params->qdd_bounds.min = min;

  Map<VectorXd>max(mxGetPrSafe(myGetField(qddbound_obj, "max")), mxGetNumberOfElements(myGetField(qddbound_obj, "max")));
  params->qdd_bounds.max = max;
  return;
}

void parseBodyMotionParams(const mxArray *params_obj, int i, BodyMotionParams *params) {
  const mxArray *pobj;
  const mxArray *bounds_obj = myGetField(params_obj, i, "accel_bounds");

  pobj = myGetField(params_obj, i, "Kp");
  sizecheck(pobj, 6, 1);
  Map<Vector6d>Kp(mxGetPrSafe(pobj));
  params->Kp = Kp;

  pobj = myGetField(params_obj, i, "Kd");
  sizecheck(pobj, 6, 1);
  Map<Vector6d>Kd(mxGetPrSafe(pobj));
  params->Kd = Kd;

  pobj = myGetField(params_obj, i, "weight");
  sizecheck(pobj, 1, 1);
  params->weight = mxGetScalar(pobj);

  pobj = myGetField(bounds_obj, "min");
  sizecheck(pobj, 6, 1);
  Map<Vector6d>min(mxGetPrSafe(pobj));
  params->accel_bounds.min = min;

  pobj = myGetField(bounds_obj, "max");
  sizecheck(pobj, 6, 1);
  Map<Vector6d>max(mxGetPrSafe(pobj));
  params->accel_bounds.max = max;
  return;
}

void parseVRefIntegratorParams(const mxArray *params_obj, VRefIntegratorParams *params) {
  const mxArray *pobj;

  pobj = myGetField(params_obj, "zero_ankles_on_contact");
  if (!mxIsDouble(pobj)) mexErrMsgTxt("zero_ankles_on_contact should be a double (yes, even though it's treated as a logical. sorry...)");
  sizecheck(pobj, 1, 1);
  params->zero_ankles_on_contact = (mxGetScalar(pobj) != 0);

  pobj = myGetField(params_obj, "eta");
  sizecheck(pobj, 1, 1);
  params->eta = mxGetScalar(pobj);

  pobj = myGetField(params_obj, "delta_max");
  sizecheck(pobj, 1, 1);
  params->delta_max = mxGetScalar(pobj);

  return;
}

void parseHardwareGains(const mxArray *params_obj, RigidBodyManipulator *r, AtlasHardwareGains *params) {
  const mxArray *pobj;
  int nu = r->num_velocities - 6;

  pobj = myGetField(params_obj, "k_f_p");
  sizecheck(pobj, nu, 1);
  Map<VectorXd> k_f_p(mxGetPrSafe(pobj), nu);
  params->k_f_p = k_f_p;

  pobj = myGetField(params_obj, "k_q_p");
  sizecheck(pobj, nu, 1);
  Map<VectorXd> k_q_p(mxGetPrSafe(pobj), nu);
  params->k_q_p = k_q_p;

  pobj = myGetField(params_obj, "k_q_i");
  sizecheck(pobj, nu, 1);
  Map<VectorXd> k_q_i(mxGetPrSafe(pobj), nu);
  params->k_q_i = k_q_i;

  pobj = myGetField(params_obj, "k_qd_p");
  sizecheck(pobj, nu, 1);
  Map<VectorXd> k_qd_p(mxGetPrSafe(pobj), nu);
  params->k_qd_p = k_qd_p;

  pobj = myGetField(params_obj, "ff_qd");
  sizecheck(pobj, nu, 1);
  Map<VectorXd> ff_qd(mxGetPrSafe(pobj), nu);
  params->ff_qd = ff_qd;

  pobj = myGetField(params_obj, "ff_f_d");
  sizecheck(pobj, nu, 1);
  Map<VectorXd> ff_f_d(mxGetPrSafe(pobj), nu);
  params->ff_f_d = ff_f_d;

  pobj = myGetField(params_obj, "ff_const");
  sizecheck(pobj, nu, 1);
  Map<VectorXd> ff_const(mxGetPrSafe(pobj), nu);
  params->ff_const = ff_const;

  pobj = myGetField(params_obj, "ff_qd_d");
  sizecheck(pobj, nu, 1);
  Map<VectorXd> ff_qd_d(mxGetPrSafe(pobj), nu);
  params->ff_qd_d = ff_qd_d;
  return;
}

void parseHardwareParams(const mxArray *params_obj, RigidBodyManipulator *r, AtlasHardwareParams *params) {
  const mxArray *pobj;

  parseHardwareGains(myGetField(params_obj, "gains"), r, &(params->gains));

  int nu = r->num_velocities - 6;
  params->joint_is_force_controlled = Matrix<bool, Dynamic, 1>::Zero(nu);
  params->joint_is_position_controlled = Matrix<bool, Dynamic, 1>::Zero(nu);

  pobj = myGetField(params_obj, "joint_is_position_controlled");
  sizecheck(pobj, nu, 1);
  Map<VectorXd>pos_double(mxGetPrSafe(pobj), nu);

  pobj = myGetField(params_obj, "joint_is_force_controlled");
  sizecheck(pobj, nu, 1);
  Map<VectorXd>force_double(mxGetPrSafe(pobj), nu);

  for (int i=0; i < nu; i++) {
    params->joint_is_force_controlled(i) = force_double(i) > 0.5;
    params->joint_is_position_controlled(i) = pos_double(i) > 0.5;
  }
  return;
}

void parseAtlasParams(const mxArray *params_obj, RigidBodyManipulator *r, AtlasParams *params) {
  const mxArray *pobj;

  pobj = myGetProperty(params_obj, "W_kdot");
  sizecheck(pobj, 3, 3);
  Map<Matrix3d>W_kdot(mxGetPrSafe(pobj));
  params->W_kdot = W_kdot;

  pobj = myGetProperty(params_obj, "Kp_ang");
  sizecheck(pobj, 1, 1);
  params->Kp_ang = mxGetScalar(pobj);

  pobj = myGetProperty(params_obj, "w_slack");
  sizecheck(pobj, 1, 1);
  params->w_slack = mxGetScalar(pobj);

  pobj = myGetProperty(params_obj, "slack_limit");
  sizecheck(pobj, 1, 1);
  params->slack_limit = mxGetScalar(pobj);

  pobj = myGetProperty(params_obj, "w_grf");
  sizecheck(pobj, 1, 1);
  params->w_grf = mxGetScalar(pobj);

  pobj = myGetProperty(params_obj, "Kp_accel");
  sizecheck(pobj, 1, 1);
  params->Kp_accel = mxGetScalar(pobj);

  pobj = myGetProperty(params_obj, "contact_threshold");
  sizecheck(pobj, 1, 1);
  params->contact_threshold = mxGetScalar(pobj);

  pobj = myGetProperty(params_obj, "min_knee_angle");
  sizecheck(pobj, 1, 1);
  params->min_knee_angle = mxGetScalar(pobj);

  parseWholeBodyParams(myGetProperty(params_obj, "whole_body"), r, &(params->whole_body));
  parseVRefIntegratorParams(myGetProperty(params_obj, "vref_integrator"), &(params->vref_integrator));
  parseJointSoftLimits(myGetProperty(params_obj, "joint_soft_limits"), r, &(params->joint_soft_limits));

  BodyMotionParams body_motion_params;
  const mxArray *body_motion_obj = myGetProperty(params_obj, "body_motion");
  int num_tracked_bodies = mxGetNumberOfElements(body_motion_obj);
  params->body_motion.resize(num_tracked_bodies);
  for (int i=0; i < num_tracked_bodies; i++) {
    parseBodyMotionParams(body_motion_obj, i, &body_motion_params);
    params->body_motion[i] = body_motion_params;
  }

  parseHardwareParams(myGetProperty(params_obj, "hardware"), r, &(params->hardware));
  return;
}

void parseAtlasParamSets(const mxArray *pobj, RigidBodyManipulator *r, map<string,AtlasParams> *param_sets) {
  int num_fields = mxGetNumberOfFields(pobj);
  if (num_fields == 0) mexErrMsgTxt("could not get any field names from the param_sets object\n");

  AtlasParams params;
  const char* fieldname;
  for (int i=0; i < num_fields; i++) {
    fieldname = mxGetFieldNameByNumber(pobj, i);
    parseAtlasParams(myGetField(pobj, fieldname), r, &params);
    param_sets->insert(pair<string,AtlasParams>(string(fieldname), params));
  }

  return;
}

void parseRobotPropertyCache(const mxArray *rpc_obj, RobotPropertyCache *rpc) {
  const mxArray *pobj;

  pobj = myGetField(myGetField(rpc_obj, "position_indices"), "r_leg_kny");
  Map<VectorXd>r_leg_kny(mxGetPrSafe(pobj), mxGetNumberOfElements(pobj));
  rpc->position_indices.r_leg_kny = r_leg_kny.cast<int>().array() - 1;

  pobj = myGetField(myGetField(rpc_obj, "position_indices"), "l_leg_kny");
  Map<VectorXd>l_leg_kny(mxGetPrSafe(pobj), mxGetNumberOfElements(pobj));
  rpc->position_indices.l_leg_kny = l_leg_kny.cast<int>().array() - 1;

  pobj = myGetField(myGetField(rpc_obj, "position_indices"), "r_leg");
  Map<VectorXd>r_leg(mxGetPrSafe(pobj), mxGetNumberOfElements(pobj));
  rpc->position_indices.r_leg = r_leg.cast<int>().array() - 1;

  pobj = myGetField(myGetField(rpc_obj, "position_indices"), "l_leg");
  Map<VectorXd>l_leg(mxGetPrSafe(pobj), mxGetNumberOfElements(pobj));
  rpc->position_indices.l_leg = l_leg.cast<int>().array() - 1;

  pobj = myGetField(myGetField(rpc_obj, "position_indices"), "r_leg_ak");
  Map<VectorXd>r_leg_ak(mxGetPrSafe(pobj), mxGetNumberOfElements(pobj));
  rpc->position_indices.r_leg_ak = r_leg_ak.cast<int>().array() - 1;

  pobj = myGetField(myGetField(rpc_obj, "position_indices"), "l_leg_ak");
  Map<VectorXd>l_leg_ak(mxGetPrSafe(pobj), mxGetNumberOfElements(pobj));
  rpc->position_indices.l_leg_ak = l_leg_ak.cast<int>().array() - 1;

  rpc->body_ids.r_foot = (int) mxGetScalar(myGetField(myGetField(rpc_obj, "body_ids"), "r_foot")) - 1;
  rpc->body_ids.l_foot = (int) mxGetScalar(myGetField(myGetField(rpc_obj, "body_ids"), "l_foot")) - 1;
  rpc->body_ids.pelvis = (int) mxGetScalar(myGetField(myGetField(rpc_obj, "body_ids"), "pelvis")) - 1;

  pobj = myGetField(rpc_obj, "actuated_indices");
  Map<VectorXd>actuated_indices(mxGetPrSafe(pobj), mxGetNumberOfElements(pobj));
  rpc->actuated_indices = actuated_indices.cast<int>().array() - 1;

  pobj = myGetField(rpc_obj, "num_bodies");
  rpc->num_bodies = (int) mxGetScalar(pobj);

  return;
}

void parseRobotJointNames(const mxArray *input_names, JointNames *joint_names) {
  joint_names->robot = get_strings(myGetField(input_names, "robot"));
  joint_names->drake = get_strings(myGetField(input_names, "drake"));
  return;
}

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
  if (nrhs<1) mexErrMsgTxt("usage: ptr = constructQPDataPointerMex(robot_obj, params_sets, robot_property_cache, B, umin, umax, terrain_map_ptr, gurobi_opts);");

  if (nrhs == 1) {
    // By convention, calling the constructor with just one argument (the pointer) should delete the pointer
    if (isa(prhs[0],"DrakeMexPointer")) {
      destroyDrakeMexPointer<NewQPControllerData*>(prhs[0]);
      return;
    } else {
      mexErrMsgIdAndTxt("Drake:constructQPDataPointerMex:BadInputs", "Expected a DrakeMexPointer (or a subclass)");
    }
  }

  if (nlhs<1) mexErrMsgTxt("take at least one output... please.");

  struct NewQPControllerData* pdata;
  pdata = new struct NewQPControllerData;

  int narg = 0;

  // robot_obj
  pdata->r = (RigidBodyManipulator*) getDrakeMexPointer(prhs[narg]);
  narg++;

  // param_sets
  parseAtlasParamSets(prhs[narg], pdata->r, &(pdata->param_sets));
  narg++;

  // robot_property_cache
  parseRobotPropertyCache(prhs[narg], &(pdata->rpc));
  narg++;

  // B
  pdata->B.resize(mxGetM(prhs[narg]),mxGetN(prhs[narg]));
  memcpy(pdata->B.data(),mxGetPrSafe(prhs[narg]),sizeof(double)*mxGetM(prhs[narg])*mxGetN(prhs[narg]));
  narg++;

  // umin
  int nq = pdata->r->num_positions, nu = pdata->B.cols();
  pdata->umin.resize(nu);
  pdata->umax.resize(nu);
  memcpy(pdata->umin.data(),mxGetPrSafe(prhs[narg++]),sizeof(double)*nu);

  // umax
  memcpy(pdata->umax.data(),mxGetPrSafe(prhs[narg++]),sizeof(double)*nu);

  // terrain_map_ptr
  if (!mxIsNumeric(prhs[narg]) || mxGetNumberOfElements(prhs[narg])!=1) mexErrMsgIdAndTxt("Drake:constructQPDataPointerMex:BadInputs","this should be a map pointer");
  void *ptr = NULL;
  mxClassID cid;
  if (sizeof(ptr) == 4) {
    cid = mxUINT32_CLASS;
  } else if (sizeof(ptr) == 8) {
    cid = mxUINT64_CLASS;
  } else {
    std::cout << "size of ptr: " << sizeof(ptr) << std::endl;
    mexErrMsgTxt("We expect size of pointer to be 4 on a 32-bit machine or 8 on a 64-bit machine");
  }
  if (mxGetClassID(prhs[narg]) != cid) {
    mexErrMsgTxt("map_ptr should be a uint64 (for 64-bit machines) or uint32 (for 32-bit machines)");
  }
  memcpy(&pdata->map_ptr,mxGetData(prhs[narg]),sizeof(ptr));
  if (!pdata->map_ptr) mexWarnMsgTxt("Map ptr is NULL. Assuming flat ground.");
  narg++;

  // default_terrain_height
  pdata->default_terrain_height = mxGetScalar(prhs[narg]);
  narg++;

  // use_fast_qp
  pdata->use_fast_qp = (int) mxGetScalar(prhs[narg]);
  narg++;

  // gurobi_opts
  const mxArray* psolveropts = prhs[narg];
  narg++;

  // input_coordinate_names
  parseRobotJointNames(myGetField(prhs[narg], "input"), &(pdata->input_joint_names));
  pdata->state_coordinate_names = get_strings(myGetField(prhs[narg], "state"));
  narg++;

  // Done parsing inputs

  pdata->B_act.resize(nu,nu);
  pdata->B_act = pdata->B.bottomRows(nu);

  pdata->qdd_lb = VectorXd::Zero(nq).array() - numeric_limits<double>::infinity();
  pdata->qdd_ub = VectorXd::Zero(nq).array() + numeric_limits<double>::infinity();

  //create gurobi environment
  int error = GRBloadenv(&(pdata->env),NULL);
  if (error)
    mexErrMsgTxt("Cannot load gurobi environment.");

  // set solver params (http://www.gurobi.com/documentation/5.5/reference-manual/node798#sec:Parameters)
  int method = (int) mxGetScalar(myGetField(psolveropts,"method"));
  CGE ( GRBsetintparam(pdata->env,"outputflag",0), pdata->env );
  CGE ( GRBsetintparam(pdata->env,"method",method), pdata->env );
  // CGE ( GRBsetintparam(pdata->env,"method",method), pdata->env );
  CGE ( GRBsetintparam(pdata->env,"presolve",0), pdata->env );
  if (method==2) {
    CGE ( GRBsetintparam(pdata->env,"bariterlimit",20), pdata->env );
    CGE ( GRBsetintparam(pdata->env,"barhomogeneous",0), pdata->env );
    CGE ( GRBsetdblparam(pdata->env,"barconvtol",0.0005), pdata->env );
  }

  // preallocate some memory
  pdata->H.resize(nq,nq);
  pdata->H_float.resize(6,nq);
  pdata->H_act.resize(nu,nq);

  pdata->C.resize(nq);
  pdata->C_float.resize(6);
  pdata->C_act.resize(nu);

  pdata->J.resize(3,nq);
  pdata->J_xy.resize(2,nq);
  pdata->Hqp.resize(nq,nq);
  pdata->fqp.resize(nq);
  pdata->Ag.resize(6,nq);
  pdata->Ak.resize(3,nq);

  pdata->state.vbasis_len = 0;
  pdata->state.cbasis_len = 0;
  pdata->state.vbasis = NULL;
  pdata->state.cbasis = NULL;

  pdata->state.t_prev = 0;
  pdata->state.vref_integrator_state = VectorXd::Zero(pdata->r->num_velocities);
  pdata->state.q_integrator_state = VectorXd::Zero(pdata->r->num_positions);
  pdata->state.foot_contact_prev[0] = false;
  pdata->state.foot_contact_prev[1] = false;
  pdata->state.num_active_contact_pts = 0;

  plhs[0] = createDrakeMexPointer((void*) pdata, "NewQPControllerData");

  return;
}


#include <iostream>
#include <armadillo>
#include "fileManager.h"
#include "main.h"
#include "Algorithms.h"

using namespace std;
using namespace arma;

int main(int argc, const char **argv) {

	fileManager fm("Bildschirmtext.txt", "StopWords.txt");
	fm.readFile(40);

	//// Initialize the random generator
	//arma::arma_rng::set_seed_random();

	//// Create a 4x4 random matrix and print it on the screen
	//arma::Mat<double> A = arma::randu(4, 4);
	//std::cout << "A:\n" << A << "\n";

	//// Multiply A with his transpose:
	//std::cout << "A * A.t() =\n";
	//std::cout << A * A.t() << "\n";

	//// Access/Modify rows and columns from the array:
	//A.row(0) = A.row(1) + A.row(3);
	//A.col(3).zeros();
	//std::cout << "add rows 1 and 3, store result in row 0, also fill 4th column with zeros:\n";
	//std::cout << "A:\n" << A << "\n";

	//// Create a new diagonal matrix using the main diagonal of A:
	//arma::Mat<double>B = arma::diagmat(A);
	//std::cout << "B:\n" << B << "\n";

	//// Save matrices A and B:
	//A.save("A_mat.txt", arma::arma_ascii);
	//B.save("B_mat.txt", arma::arma_ascii);

	return 0;

}


//
//  cvx_pose_estimation.cpp
//  LoopClosure
//
//  Created by jimmy on 2016-03-31.
//  Copyright © 2016 jimmy. All rights reserved.
//

#include "cvx_pose_estimation.hpp"
#include <iostream>
#include <Eigen/Geometry>
#include "cvx_calib3d.hpp"

using std::cout;
using std::endl;
using cv::Mat;

struct HypotheseLoss
{
    double loss_;
    Mat rvec_;       // rotation     for 3D --> 2D projection
    Mat tvec_;       // translation  for 3D --> 2D projection
    Mat affine_;     //              for 3D --> 3D camera to world transformation
    vector<unsigned int> inlier_indices_;         // camera coordinate index
    vector<unsigned int> inlier_candidate_world_pts_indices_; // candidate world point index

    // store all inliers from preemptive ransac
    vector<cv::Point3d> camera_pts_;
    vector<cv::Point3d> wld_pts_;

    HypotheseLoss()
    {
        loss_ = INT_MAX;
    }
    HypotheseLoss(const double loss)
    {
        loss_  = loss;
    }

    HypotheseLoss(const HypotheseLoss & other)
    {
        loss_ = other.loss_;
        rvec_ = other.rvec_;
        tvec_ = other.tvec_;
        affine_ = other.affine_;
        inlier_indices_.clear();
        inlier_indices_.resize(other.inlier_indices_.size());
        inlier_candidate_world_pts_indices_.clear();
        inlier_candidate_world_pts_indices_.resize(other.inlier_candidate_world_pts_indices_.size());
        for(int i = 0; i<other.inlier_indices_.size(); i++) {
            inlier_indices_[i] = other.inlier_indices_[i];
        }
        for(int i = 0; i<other.inlier_candidate_world_pts_indices_.size(); i++){
            inlier_candidate_world_pts_indices_[i] = other.inlier_candidate_world_pts_indices_[i];
        }
        if(inlier_candidate_world_pts_indices_.size() != 0){
            assert(inlier_indices_.size() == inlier_candidate_world_pts_indices_.size());
        }

        // copy camera points and world coordinate points
        if(other.camera_pts_.size() > 0)
        {
            camera_pts_.resize(other.camera_pts_.size());
            wld_pts_.resize(other.wld_pts_.size());

            assert(camera_pts_.size() == wld_pts_.size());
            for(int i = 0; i<camera_pts_.size(); i++)
            {
                camera_pts_[i] = other.camera_pts_[i];
                wld_pts_[i] = other.wld_pts_[i];
            }
        }
    }

    bool operator < (const HypotheseLoss & other) const
    {
        return loss_ < other.loss_;
    }

    HypotheseLoss & operator = (const HypotheseLoss & other)
    {
        if (&other == this) {
            return *this;
        }
        loss_ = other.loss_;
        rvec_ = other.rvec_;
        tvec_ = other.tvec_;
        affine_ = other.affine_;
        inlier_indices_.clear();
        inlier_indices_.resize(other.inlier_indices_.size());
        inlier_candidate_world_pts_indices_.clear();
        inlier_candidate_world_pts_indices_.resize(other.inlier_candidate_world_pts_indices_.size());
        for(int i = 0; i<other.inlier_indices_.size(); i++) {
            inlier_indices_[i] = other.inlier_indices_[i];
        }
        for(int i = 0; i<other.inlier_candidate_world_pts_indices_.size(); i++){
            inlier_candidate_world_pts_indices_[i] = other.inlier_candidate_world_pts_indices_[i];
        }
        if(inlier_candidate_world_pts_indices_.size() != 0){
            assert(inlier_indices_.size() == inlier_candidate_world_pts_indices_.size());
        }

        // copy camera points and world coordinate points
        if(other.camera_pts_.size() > 0)
        {
            camera_pts_.resize(other.camera_pts_.size());
            wld_pts_.resize(other.wld_pts_.size());

            assert(camera_pts_.size() == wld_pts_.size());
            for(int i = 0; i<camera_pts_.size(); i++)
            {
                camera_pts_[i] = other.camera_pts_[i];
                wld_pts_[i] = other.wld_pts_[i];
            }
        }

        return *this;
    }
};

bool CvxPoseEstimation::preemptiveRANSAC3DOneToMany(const vector<cv::Point3d> & camera_pts,
                                                    const vector<vector<cv::Point3d>> & candidate_wld_pts,
                                                    const PreemptiveRANSAC3DParameter & param,
                                                    cv::Mat & camera_pose)
{
    assert(camera_pts.size() == candidate_wld_pts.size());
    if (camera_pts.size() < 500) {
        return false;
    }

    const int num_iteration = 2048;
    int K = 1024;
    const int N = (int)camera_pts.size();
    const int B = param.sample_number_;

    vector<cv::Mat > affine_candidate;
    for (int i = 0; i<num_iteration; i++) {

        int k1 = 0;
        int k2 = 0;
        int k3 = 0;
        int k4 = 0;

        do{
            k1 = rand()%N;
            k2 = rand()%N;
            k3 = rand()%N;
            k4 = rand()%N;
        }while (k1 == k2 || k1 == k3 || k1 == k4 ||
                k2 == k3 || k2 == k4 || k3 == k4);

        vector<cv::Point3d> sampled_camera_pts;
        vector<cv::Point3d> sampled_wld_pts;

        sampled_camera_pts.push_back(camera_pts[k1]);
        sampled_camera_pts.push_back(camera_pts[k2]);
        sampled_camera_pts.push_back(camera_pts[k3]);
        sampled_camera_pts.push_back(camera_pts[k4]);

        sampled_wld_pts.push_back(candidate_wld_pts[k1][0]);
        sampled_wld_pts.push_back(candidate_wld_pts[k2][0]);
        sampled_wld_pts.push_back(candidate_wld_pts[k3][0]);
        sampled_wld_pts.push_back(candidate_wld_pts[k4][0]);

        Mat affine;
        CvxCalib3D::KabschTransform(sampled_camera_pts, sampled_wld_pts, affine);
        affine_candidate.push_back(affine);
        if (affine_candidate.size() > K) {
            printf("initialization repeat %d times\n", i);
            break;
        }
    }
    printf("init camera parameter number is %lu\n", affine_candidate.size());

    vector<HypotheseLoss> losses;
    for (int i = 0; i<affine_candidate.size(); i++) {
        HypotheseLoss hyp(0.0);
        hyp.affine_ = affine_candidate[i];
        losses.push_back(hyp);
    }

    double threshold = param.dis_threshold_;
    while (losses.size() > 1) {
        // sample random set
        vector<cv::Point3d> sampled_camera_pts;
        vector< vector<cv::Point3d> > sampled_wld_pts;  // one camera point may have multiple world points correspondences
        vector<int> sampled_indices;
        for (int i =0; i<B; i++) {
            int index = rand()%N;
            sampled_camera_pts.push_back(camera_pts[index]);
            sampled_wld_pts.push_back(candidate_wld_pts[index]);
            sampled_indices.push_back(index);
        }

        // count outliers
        for (int i = 0; i<losses.size(); i++) {
            // evaluate the accuracy by check transformation
            vector<cv::Point3d> transformed_pts;
            CvxCalib3D::rigidTransform(sampled_camera_pts, losses[i].affine_, transformed_pts);

            // check minimum distance from transformed points to world coordiante
            for (int j = 0; j<transformed_pts.size(); j++) {
                double min_dis = threshold * 2;
                int min_index = -1;
                for (int k = 0; k<sampled_wld_pts[j].size(); k++) {
                    cv::Point3d dif = transformed_pts[j] - sampled_wld_pts[j][k];
                    double dis = cv::norm(dif);
                    if (dis < min_dis) {
                        min_dis = dis;
                        min_index = k;
                    }
                } // end of k

                if (min_dis > threshold) {
                    losses[i].loss_ += 1.0;
                }
                else {
                    losses[i].inlier_indices_.push_back(sampled_indices[j]);
                    losses[i].inlier_candidate_world_pts_indices_.push_back(min_index);
                }
            } // end of j
            assert(losses[i].inlier_indices_.size() == losses[i].inlier_candidate_world_pts_indices_.size());
            // printf("inlier number is %lu\n", losses[i].inlier_indices_.size());
        }
        //getchar();

        std::sort(losses.begin(), losses.end());
        losses.resize(losses.size()/2);

        for (int i = 0; i<losses.size(); i++) {
         //   printf("after: loss is %lf\n", losses[i].loss_);
         //   printf("inlier number is %lu\n", losses[i].inlier_indices_.size());
        }
        // printf("\n\n");

        // refine by inliers
        for (int i = 0; i<losses.size(); i++) {
            // number of inliers is larger than minimum configure
            if (losses[i].inlier_indices_.size() > 4) {
                vector<cv::Point3d> inlier_camera_pts;
                vector<cv::Point3d> inlier_wld_pts;
                for (int j = 0; j < losses[i].inlier_indices_.size(); j++) {
                    int index = losses[i].inlier_indices_[j];
                    int wld_index = losses[i].inlier_candidate_world_pts_indices_[j];
                    inlier_camera_pts.push_back(camera_pts[index]);
                    inlier_wld_pts.push_back(candidate_wld_pts[index][wld_index]);
                }
                Mat affine;

                CvxCalib3D::KabschTransform(inlier_camera_pts, inlier_wld_pts, affine);
                losses[i].affine_ = affine;
                losses[i].inlier_indices_.clear();
                losses[i].inlier_candidate_world_pts_indices_.clear();
            }
        }
    }
    assert(losses.size() == 1);

    camera_pose = cv::Mat::eye(4, 4, CV_64F);
    losses[0].affine_.copyTo(camera_pose(cv::Rect(0, 0, 4, 3)));
    //cout<<"camera pose\n"<<camera_pose<<endl;
    return true;
}

Mat CvxPoseEstimation::rotationToEularAngle(const cv::Mat & rot)
{
    assert(rot.rows == 3 && rot.cols == 3);
    assert(rot.type() == CV_64FC1);

    // https://d3cw3dd2w32x2b.cloudfront.net/wp-content/uploads/2012/07/euler-angles.pdf
    double m00 = rot.at<double>(0, 0);
    double m01 = rot.at<double>(0, 1);
    double m02 = rot.at<double>(0, 2);
    double m10 = rot.at<double>(1, 0);
    double m11 = rot.at<double>(1, 1);
    double m12 = rot.at<double>(1, 2);
    double m20 = rot.at<double>(2, 0);
    double m21 = rot.at<double>(2, 1);
    double m22 = rot.at<double>(2, 2);
    double theta1 = atan2(m12, m22);
    double c2 = sqrt(m00 * m00 + m01 * m01);
    double theta2 = atan2(-m02, c2);
    double s1 = sin(theta1);
    double c1 = cos(theta1);
    double theta3 = atan2(s1*m20 - c1 * m10, c1*m11 - s1*m21);

    double scale = 180.0/3.14159;
    theta1 *= scale;
    theta2 *= scale;
    theta3 *= scale;

    Mat eular_angle = cv::Mat::zeros(3, 1, CV_64FC1);
    eular_angle.at<double>(0, 0) = theta1;
    eular_angle.at<double>(1, 0) = theta2;
    eular_angle.at<double>(2, 0) = theta3;
    return eular_angle;
    //printf("Eular angle %lf %lf %lf\n", theta1, theta2, theta3);
}

void CvxPoseEstimation::poseDistance(const cv::Mat & src_pose,
                                     const cv::Mat & dst_pose,
                                     double & angle_distance,
                                     double & euclidean_disance)
{
    // http://chrischoy.github.io/research/measuring-rotation/
    assert(src_pose.type() == CV_64F);
    assert(dst_pose.type() == CV_64F);

    Mat src_R = src_pose(cv::Rect(0, 0, 3, 3));
    Mat dst_R = dst_pose(cv::Rect(0, 0, 3, 3));

    double scale = 180.0/3.14159;

    Mat q1 = CvxPoseEstimation::rotationToQuaternion(src_R);
    Mat q2 = CvxPoseEstimation::rotationToQuaternion(dst_R);
    double val_dot = fabs(q1.dot(q2));

    //double dot = r1.dot(r2);
    //angle_distance = acos(dot) * scale;
    angle_distance = 2.0 * acos(val_dot) * scale;

    euclidean_disance = 0.0;
    double dx = src_pose.at<double>(0, 3) - dst_pose.at<double>(0, 3);
    double dy = src_pose.at<double>(1, 3) - dst_pose.at<double>(1, 3);
    double dz = src_pose.at<double>(2, 3) - dst_pose.at<double>(2, 3);
    euclidean_disance += dx * dx;
    euclidean_disance += dy * dy;
    euclidean_disance += dz * dz;
    euclidean_disance = sqrt(euclidean_disance);
 //   printf("location distance are %f %f %f\n", dx, dy, dz);
}

Mat CvxPoseEstimation::rotationToQuaternion(const cv::Mat & rot)
{
    assert(rot.type() == CV_64FC1);
    assert(rot.rows == 3 && rot.cols == 3);

    Mat ret = cv::Mat::zeros(4, 1, CV_64FC1);

    float r11 = rot.at<double>(0, 0);
    float r12 = rot.at<double>(0, 1);
    float r13 = rot.at<double>(0, 2);
    float r21 = rot.at<double>(1, 0);
    float r22 = rot.at<double>(1, 1);
    float r23 = rot.at<double>(1, 2);
    float r31 = rot.at<double>(2, 0);
    float r32 = rot.at<double>(2, 1);
    float r33 = rot.at<double>(2, 2);

    float q0 = ( r11 + r22 + r33 + 1.0f) / 4.0f;
    float q1 = ( r11 - r22 - r33 + 1.0f) / 4.0f;
    float q2 = (-r11 + r22 - r33 + 1.0f) / 4.0f;
    float q3 = (-r11 - r22 + r33 + 1.0f) / 4.0f;
    if(q0 < 0.0f) q0 = 0.0f;
    if(q1 < 0.0f) q1 = 0.0f;
    if(q2 < 0.0f) q2 = 0.0f;
    if(q3 < 0.0f) q3 = 0.0f;
    q0 = sqrt(q0);
    q1 = sqrt(q1);
    q2 = sqrt(q2);
    q3 = sqrt(q3);
    if(q0 >= q1 && q0 >= q2 && q0 >= q3) {
        q0 *= +1.0f;
        q1 *= CvxPoseEstimation::SIGN(r32 - r23);
        q2 *= CvxPoseEstimation::SIGN(r13 - r31);
        q3 *= CvxPoseEstimation::SIGN(r21 - r12);
    } else if(q1 >= q0 && q1 >= q2 && q1 >= q3) {
        q0 *= CvxPoseEstimation::SIGN(r32 - r23);
        q1 *= +1.0f;
        q2 *= CvxPoseEstimation::SIGN(r21 + r12);
        q3 *= CvxPoseEstimation::SIGN(r13 + r31);
    } else if(q2 >= q0 && q2 >= q1 && q2 >= q3) {
        q0 *= CvxPoseEstimation::SIGN(r13 - r31);
        q1 *= CvxPoseEstimation::SIGN(r21 + r12);
        q2 *= +1.0f;
        q3 *= CvxPoseEstimation::SIGN(r32 + r23);
    } else if(q3 >= q0 && q3 >= q1 && q3 >= q2) {
        q0 *= CvxPoseEstimation::SIGN(r21 - r12);
        q1 *= CvxPoseEstimation::SIGN(r31 + r13);
        q2 *= CvxPoseEstimation::SIGN(r32 + r23);
        q3 *= +1.0f;
    } else {
        printf("q0, q1, q2, q3: %f %f %f %f\n", q0, q1, q2, q3);
        printf("Error: rotation matrix quaternion.\n");
        cout<<"rotation matrix is \n"<<rot<<endl;
        assert(0);
    }
    float r = CvxPoseEstimation::NORM(q0, q1, q2, q3);
    q0 /= r;
    q1 /= r;
    q2 /= r;
    q3 /= r;

    ret.at<double>(0, 0) = q0;
    ret.at<double>(1, 0) = q1;
    ret.at<double>(2, 0) = q2;
    ret.at<double>(3, 0) = q3;
    return ret;
}

Mat CvxPoseEstimation::quaternionToRotation(const cv::Mat & q)
{
    assert(q.type() == CV_64FC1);
    assert(q.rows == 4);
    assert(q.cols == 1);

    double x = q.at<double>(0, 0);
    double y = q.at<double>(1, 0);
    double z = q.at<double>(2, 0);
    double w = q.at<double>(3, 0);

    Eigen::Quaterniond quat(w, x, y, z);

    Eigen::Matrix<double, 3, 3> eig_mat = quat.matrix();
    cv::Mat rot = cv::Mat::zeros(3, 3, CV_64FC1);
    for (int r = 0; r<3; r++) {
        for (int c = 0; c<3; c++) {
            rot.at<double>(r, c) = eig_mat(r, c);
        }
    }
    return rot;
}


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

#include "masternode.h"
#include "keystore.h"
#include <boost/test/unit_test.hpp>

namespace
{
    CMasternode CreateMasternode(CTxIn vin)
    {
        CMasternode mn;
        mn.vin = vin;
        mn.activeState = CMasternode::MASTERNODE_ENABLED;
        return mn;
    }

    void FillBlock(CBlockIndex& block, CBlockIndex* prevBlock, const uint256& hash)
    {
        if (prevBlock)
        {
            block.nHeight = prevBlock->nHeight + 1;
            block.pprev = prevBlock;
        }
        else
        {
            block.nHeight = 0;
        }

        block.phashBlock = &hash;
        block.BuildSkip();
    }

    void FillHash(uint256& hash, const arith_uint256& height)
    {
        hash = ArithToUint256(height);
    }

    void FillBlock(CBlockIndex& block, uint256& hash, CBlockIndex* prevBlock, size_t height)
    {
        FillHash(hash, height);
        FillBlock(block, height ? prevBlock : NULL, hash);

        assert(static_cast<int>(UintToArith256(block.GetBlockHash()).GetLow64()) == block.nHeight);
        assert(block.pprev == NULL || block.nHeight == block.pprev->nHeight + 1);

    }

    struct CalculateScoreFixture
    {
        const CMasternode mn;
        std::vector<uint256> hashes;
        std::vector<CBlockIndex> blocks;

        CalculateScoreFixture()
            : mn(CreateMasternode(CTxIn(COutPoint(ArithToUint256(1), 1 * COIN))))
            , hashes(1000)
            , blocks(1000)
        {
            BuildChain();
        }

        void BuildChain()
        {
            // Build a main chain 1000 blocks long.
            for (size_t i = 0; i < blocks.size(); ++i)
            {
                FillBlock(blocks[i], hashes[i], &blocks[i - 1], i);
            }
            chainActive.SetTip(&blocks.back());
        }

        ~CalculateScoreFixture()
        {
            chainActive = CChain();
        }
    };
}

BOOST_FIXTURE_TEST_SUITE(CalculateScore, CalculateScoreFixture)

    BOOST_AUTO_TEST_CASE(NullTip)
    {
        chainActive = CChain();
        BOOST_CHECK(mn.CalculateScore(1) == arith_uint256());
    }

    BOOST_AUTO_TEST_CASE(NotExistingBlock)
    {
        BOOST_CHECK(mn.CalculateScore(1001) == arith_uint256());
    }

    BOOST_AUTO_TEST_CASE(ScoreChanges)
    {
        CMasternode mn = CreateMasternode(CTxIn(COutPoint(ArithToUint256(1), 1 * COIN)));
        arith_uint256 score = mn.CalculateScore(100);

        // Change masternode vin
        mn.vin = CTxIn(COutPoint(ArithToUint256(2), 1 * COIN));

        // Calculate new score
        arith_uint256 newScore = mn.CalculateScore(100);

        BOOST_CHECK(score != newScore);
    }

    BOOST_AUTO_TEST_CASE(DistributionCheck)
    {
        std::vector<CMasternode> masternodes;
        masternodes.push_back(CreateMasternode(CTxIn(COutPoint(ArithToUint256(1), 1 * COIN))));
        masternodes.push_back(CreateMasternode(CTxIn(COutPoint(ArithToUint256(2), 1 * COIN))));
        masternodes.push_back(CreateMasternode(CTxIn(COutPoint(ArithToUint256(3), 1 * COIN))));
        masternodes.push_back(CreateMasternode(CTxIn(COutPoint(ArithToUint256(4), 1 * COIN))));
        masternodes.push_back(CreateMasternode(CTxIn(COutPoint(ArithToUint256(5), 1 * COIN))));

        // Find winner masternode for all 1000 blocks and
        // make sure all masternodes have the same probability to win
        std::vector<pair<int64_t, CTxIn> > vecMasternodeScores;
        std::map<int, int> winningCount;
        for (int i = 0; i <= chainActive.Height(); ++i)
        {
            arith_uint256 score = 0;
            int index = 0;
            for (size_t j = 0; j < masternodes.size(); ++j)
            {
                arith_uint256 s = masternodes[j].CalculateScore(i);
                if (s > score)
                {
                    score = s;
                    index = j;
                }
            }
            winningCount[index]++;
        }

        std::map<int, int>::iterator it = winningCount.begin();
        const int averageWinCount = chainActive.Height() / masternodes.size();
        for (; it != winningCount.end(); ++it)
        {
            // +- 15%
            BOOST_CHECK(it->second < averageWinCount + 0.15 * averageWinCount);
            BOOST_CHECK(it->second > averageWinCount - 0.15 * averageWinCount);
        }
    }

BOOST_AUTO_TEST_SUITE_END()


/*
Copyright 2009-2021 Nicolas Colombe

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

#pragma once

#include <engine/enginelib.hpp>
#include <core/coredef.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/geometry/index/rtree.hpp>
#include <math/geometrytraits.hpp>
#include <math/aabb2d.hpp>
#include <math/aabb2dpolygon.hpp>
#include <math/segment.hpp>
#include <boost/optional.hpp>

namespace eXl
{
  class EXL_ENGINE_API NavMesh
  {
  public:
    typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS,
      boost::no_property,
      boost::property<boost::edge_weight_t, float> > Graph;

    using BoxIndexEntry = std::pair<AABB2Df, uint32_t>;
    using BoxIndex = boost::geometry::index::rtree<BoxIndexEntry, boost::geometry::index::rstar<16, 4>>;

    struct EXL_ENGINE_API Face
    {
      AABB2Df m_Box;
      Vector<Graph::vertex_descriptor> m_Edges;
      Vector<Segmentf> m_Walls;

      Vector2f GetTreadmillDir(Vector2f const& iPos) const;

      // Left, Bottom, Right, Top
      bool m_HasSide[4];
    };

    struct Edge
    {
      boost::optional<uint32_t> CommonFace(Edge const& iOther) const;

      uint32_t face1;
      uint32_t face2;
      Segmentf segment;
    };

    //typedef UnorderedMap<Graph::vertex_descriptor, Edge> EdgeMap;
    typedef Vector<Edge> EdgeMap;

    struct Component
    {
      BoxIndex m_FacesIdx;
      Vector<Face> m_Faces;

      Graph m_Graph;

      EdgeMap m_FaceEdges;
    };

    static NavMesh MakeFromAABB2DPoly(Vector<AABB2DPolygoni> const& iPolys);

    static NavMesh MakeFromBoxes(Vector<AABB2Di> const& iBoxes);

    Vector<Component> const& GetComponents() const { return m_Components; }

    Graph const& GetGraph(uint32_t iComponent) const
    { return m_Components[iComponent].m_Graph; }
    Vector<Face> const& GetFaces(uint32_t iComponent) const
    { return m_Components[iComponent].m_Faces; }
    EdgeMap const& GetEdges(uint32_t iComponent) const
    { return m_Components[iComponent].m_FaceEdges; }

    struct FoundFace
    {
      bool operator == (FoundFace const& iOther)
      {
        return m_Component == iOther.m_Component && m_Face == iOther.m_Face;
      }
      uint32_t m_Component;
      uint32_t m_Face;
    };

    boost::optional<FoundFace> FindFace(Vector2f const& iPos) const;

    struct Path
    {
      uint32_t m_Component;
      // Stack, first destination is at the back.
      Vector<Graph::vertex_descriptor> m_Edges;
      Vector<Vector2f> m_EdgeDirs;
    };

    boost::optional<Path> FindPath(Vector2f const& iStart, Vector2f const& iEnd) const;

  protected:

    Vector<Component> m_Components;
  };
}