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

#include <chain.h>
#include <validation.h>
#include <pos.h>
#include <pow.h>
#include <chainparams.h>
#include <primitives/block.h>
#include <wallet/wallet.h>
#include <utiltime.h>
#include <consensus/validation.h>
#include <index/addrindex.h>
#include <antibot/antibot.h>
#include <boost/range/adaptor/reversed.hpp>

double GetPosDifficulty(const CBlockIndex* blockindex) {
	// Floating point number that is a multiple of the minimum difficulty,
	// minimum difficulty = 1.0.
	if (blockindex == nullptr) {
		if (chainActive.Tip() == nullptr) {
			return 1.0;
		}
		else {
			blockindex = GetLastBlockIndex(chainActive.Tip(), 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 GetPoWMHashPS() {
	return 0;
}

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

	CBlockIndex* pindex = chainActive.Tip();
	CBlockIndex* pindexPrevStake = NULL;

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

		pindex = pindex->pprev;
	}

	double result = 0;

	if (nStakesTime) {
		result = dStakeKernelsTriedAvg / nStakesTime;
	}

	result *= STAKE_TIMESTAMP_MASK + 1;

	return result;
}

int64_t GetProofOfStakeReward(int nHeight, int64_t nFees, const Consensus::Params & consensusParams) {
	int halvings = nHeight / consensusParams.nSubsidyHalvingInterval;
	// Force block reward to zero when right shift is undefined.
	if (halvings >= 64) {
		return nFees;
	}

	CAmount nSubsidy = 5 * COIN;

	// Subsidy is cut in half every 210,000 blocks which will occur approximately every 4 years.
	nSubsidy >>= halvings;
	return nSubsidy + nFees;
}

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

bool CheckStake(const std::shared_ptr<CBlock> pblock, std::shared_ptr<CWallet> wallet, CChainParams const & chainparams) {
	arith_uint256 proofHash = arith_uint256(0), hashTarget = arith_uint256(0);
	uint256 hashBlock = pblock->GetHash();

	if (!pblock->IsProofOfStake()) {
		return error("CheckStake() : %s is not a proof-of-stake block", hashBlock.GetHex());
	}

	// verify hash target and signature of coinstake tx
	CDataStream hashProofOfStakeSource(SER_GETHASH, 0);
	if (!CheckProofOfStake(mapBlockIndex[pblock->hashPrevBlock], pblock->vtx[1], pblock->nBits, proofHash, hashProofOfStakeSource, hashTarget, NULL)) {
		return error("CheckStake() : proof-of-stake checking failed (%s)", pblock->hashPrevBlock.GetHex());
	}

	//// debug print
	LogPrintf("=== CheckStake() : new PoS block found hash: %s\n", hashBlock.GetHex());

	// Found a solution
	{
		LOCK(cs_main);
		auto hashBestChain = chainActive.Tip()->GetBlockHash();
		if (pblock->hashPrevBlock != hashBestChain) {
			return error("CheckStake() : generated block is stale");
		}

		GetMainSignals().BlockFound(pblock->GetHash());

	}

	// Process this block the same as if we had received it from another node
	CValidationState state;
	if (!ProcessNewBlock(state, chainparams, pblock, true, /* fReceived */ false, NULL))
	{
		return error("CoinStaker: ProcessNewBlock, block not accepted %s", state.GetRejectReason());
	}

	return true;
}

bool CheckProofOfStake(CBlockIndex* pindexPrev, CTransactionRef const & tx, unsigned int nBits, arith_uint256& hashProofOfStake, CDataStream& hashProofOfStakeSource, arith_uint256& targetProofOfStake, std::vector<CScriptCheck> *pvChecks, bool fCheckSignature) {
	if (!tx->IsCoinStake()) {
		return error("CheckProofOfStake() : called on non-coinstake %s", tx->GetHash().ToString());
	}

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

	CTransactionRef txPrev;
	uint256 hashBlock = uint256();
	if (!GetTransaction(txin.prevout.hash, txPrev, Params().GetConsensus(), hashBlock, true)) {
		return error("CheckProofOfStake() : INFO: read txPrev failed %s", txin.prevout.hash.GetHex());  // previous transaction not in main chain, may occur during initial download
	}

	if (pvChecks) {
		pvChecks->reserve(tx->vin.size());
	}

	if (fCheckSignature) {
		const CTransaction & txn = *tx;
		PrecomputedTransactionData txdata(txn);
		const COutPoint &prevout = tx->vin[0].prevout;
		const Coin* coins = &pcoinsTip->AccessCoin(prevout);
		assert(coins);

		// Verify signature
		CScriptCheck check(coins->out, *tx, 0, SCRIPT_VERIFY_NONE, false, &txdata);
		if (pvChecks) {
			pvChecks->push_back(CScriptCheck());
			check.swap(pvChecks->back());
		}
		else if (!check()) {
			return error("CheckProofOfStake() : script-verify-failed %s", ScriptErrorString(check.GetScriptError()));
		}
	}

	if (mapBlockIndex.count(hashBlock) == 0) {
		return error("CheckProofOfStake() : read block failed"); // unable to read block of previous transaction
	}

	CBlockIndex* pblockindex = mapBlockIndex[hashBlock];

	if (txin.prevout.hash != txPrev->GetHash()) {
		return error("CheckProofOfStake(): Coinstake input does not match previous output %s", txin.prevout.hash.GetHex());
	}

	if (!CheckStakeKernelHash(pindexPrev, nBits, *pblockindex, txPrev, txin.prevout, tx->nTime, hashProofOfStake, hashProofOfStakeSource, targetProofOfStake)) {
		return error("CheckProofOfStake() : INFO: check kernel failed on coinstake %s, hashProof=%s", tx->GetHash().ToString(), hashProofOfStake.ToString()); // may occur during initial download or if behind on block chain sync
	}

	return true;
}

bool CheckKernel(CBlockIndex* pindexPrev, unsigned int nBits, int64_t nTime, const COutPoint& prevout, int64_t* pBlockTime, CWallet* wallet, CDataStream& hashProofOfStakeSource) {
	arith_uint256 hashProofOfStake, targetProofOfStake;

	CTransactionRef txPrev;
	uint256 hashBlock = uint256();
	if (!GetTransaction(prevout.hash, txPrev, Params().GetConsensus(), hashBlock, true)) {
		LogPrintf("CheckKernel : Could not find previous transaction %s\n", prevout.hash.ToString());
		return false;
	}

	if (mapBlockIndex.count(hashBlock) == 0) {
		LogPrintf("CheckKernel : Could not find block of previous transaction %s\n", hashBlock.ToString());
		return false;
	}

	CBlockIndex* pblockindex = mapBlockIndex[hashBlock];

	if (pblockindex->GetBlockTime() + Params().GetConsensus().nStakeMinAge > nTime) {
		return false;
	}

	if (pBlockTime) {
		*pBlockTime = pblockindex->GetBlockTime();
	}

	if (!wallet->mapWallet.count(prevout.hash)) {
		return("CheckProofOfStake(): Couldn't get Tx Index");
	}

	return CheckStakeKernelHash(pindexPrev, nBits, *pblockindex, txPrev,
		prevout, nTime, hashProofOfStake, hashProofOfStakeSource, targetProofOfStake);
}

bool CheckStakeKernelHash(CBlockIndex * pindexPrev, unsigned int nBits, CBlockIndex & blockFrom, CTransactionRef const & txPrev, COutPoint const & prevout, unsigned int nTimeTx, arith_uint256& hashProofOfStake, CDataStream& hashProofOfStakeSource, arith_uint256& targetProofOfStake, bool fPrintProofOfStake) {

	unsigned int nTimeBlockFrom = blockFrom.GetBlockTime();

	if (nTimeTx < txPrev->nTime) {
		LogPrintf(" === ERROR: CheckStakeKernelHash() : nTime violation");
		return error("CheckStakeKernelHash() : nTime violation");
	}

	if (nTimeBlockFrom + Params().GetConsensus().nStakeMinAge > nTimeTx) {
		LogPrintf(" === ERROR: CheckStakeKernelHash() : min age violation");
		return error("CheckStakeKernelHash() : min age violation");
	}

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

	// Weighted target
	int64_t nValueIn = txPrev->vout[prevout.n].nValue;
	arith_uint256 bnWeight = std::min(
		nValueIn, Params().GetConsensus().nStakeMaximumThreshold
	);
	bnTarget *= bnWeight;

	targetProofOfStake = bnTarget;

	uint64_t nStakeModifier = pindexPrev->nStakeModifier;
	int nStakeModifierHeight = pindexPrev->nHeight;
	int64_t nStakeModifierTime = pindexPrev->nTime;

	// Calculate hash
	CDataStream ss(SER_GETHASH, 0);
	ss << nStakeModifier << nTimeBlockFrom << txPrev->nTime << prevout.hash << prevout.n << nTimeTx;
	hashProofOfStakeSource = ss;
	hashProofOfStake = UintToArith256(Hash(ss.begin(), ss.end()));

	if (fPrintProofOfStake)
	{
		//LogPrintf("CheckStakeKernelHash() : using modifier 0x%016x at height=%d timestamp=%s for block from timestamp=%s\n",
		//    nStakeModifier, nStakeModifierHeight,
		//    FormatISO8601DateTime(nStakeModifierTime),
		//    FormatISO8601DateTime(nTimeBlockFrom));
		//LogPrintf("CheckStakeKernelHash() : check modifier=0x%016x nTimeBlockFrom=%u nTimeTxPrev=%u nPrevout=%u nTimeTx=%u hashProof=%s bnTarget=%s nBits=%08x nValueIn=%d bnWeight=%s\n",
		//    nStakeModifier,
		//    nTimeBlockFrom, txPrev->nTime, prevout.n, nTimeTx,
		//    hashProofOfStake.ToString(),bnTarget.ToString(), nBits, nValueIn,bnWeight.ToString());
	}

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

	if (!fPrintProofOfStake)
	{
		LogPrintf("CheckStakeKernelHash() : using modifier 0x%016x at height=%d timestamp=%s for block from timestamp=%s\n",
			nStakeModifier, nStakeModifierHeight,
			FormatISO8601DateTime(nStakeModifierTime),
			FormatISO8601DateTime(nTimeBlockFrom));
		LogPrintf("CheckStakeKernelHash() : pass modifier=0x%016x nTimeBlockFrom=%u nTimeTxPrev=%u nPrevout=%u nTimeTx=%u hashProof=%s\n",
			nStakeModifier,
			nTimeBlockFrom, txPrev->nTime, prevout.n, nTimeTx,
			hashProofOfStake.ToString());
	}

	return true;
}

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

// Stake Modifier (hash modifier of proof-of-stake):
// The purpose of stake modifier is to prevent a txout (coin) owner from
// computing future proof-of-stake generated by this txout at the time
// of transaction confirmation. To meet kernel protocol, the txout
// must hash with a future stake modifier to generate the proof.
// Stake modifier consists of bits each of which is contributed from a
// selected block of a given block group in the past.
// The selection of a block is based on a hash of the block's proof-hash and
// the previous stake modifier.
// Stake modifier is recomputed at a fixed time interval instead of every
// block. This is to make it difficult for an attacker to gain control of
// additional bits in the stake modifier, even after generating a chain of
// blocks.
bool ComputeNextStakeModifier(const CBlockIndex* pindexPrev, uint64_t& nStakeModifier, bool& fGeneratedStakeModifier)
{
	nStakeModifier = 0;
	fGeneratedStakeModifier = false;
	if (!pindexPrev)
	{
		fGeneratedStakeModifier = true;
		return error("ComputeNextStakeModifier(): Could not find pindexPrev");  // genesis block's modifier is 0
	}
	// First find current stake modifier and its generation block time
	// if it's not old enough, return the same stake modifier
	int64_t nModifierTime = 0;
	if (!GetLastStakeModifier(pindexPrev, nStakeModifier, nModifierTime)) {
		return error("ComputeNextStakeModifier: unable to get last modifier");
	}
	//LogPrintf("ComputeNextStakeModifier: prev modifier=0x%016x time=%s\n", nStakeModifier, FormatISO8601DateTime(nModifierTime));
	if (nModifierTime / Params().GetConsensus().nModifierInterval >= pindexPrev->GetBlockTime() / Params().GetConsensus().nModifierInterval) {
		return true;
	}

	// Sort candidate blocks by timestamp
	std::vector<std::pair<int64_t, uint256> > vSortedByTimestamp;
	vSortedByTimestamp.reserve(64 * Params().GetConsensus().nModifierInterval / GetTargetSpacing(pindexPrev->nHeight));
	int64_t nSelectionInterval = GetStakeModifierSelectionInterval();
	int64_t nSelectionIntervalStart = (pindexPrev->GetBlockTime() / Params().GetConsensus().nModifierInterval)
		* Params().GetConsensus().nModifierInterval - nSelectionInterval;
	//LogPrintf("nSelectionInterval = %d nSelectionIntervalStart = %d\n",nSelectionInterval,nSelectionIntervalStart);

	const CBlockIndex* pindex = pindexPrev;
	while (pindex && pindex->GetBlockTime() >= nSelectionIntervalStart)
	{
		vSortedByTimestamp.push_back(std::make_pair(pindex->GetBlockTime(), pindex->GetBlockHash()));
		pindex = pindex->pprev;
	}
	int nHeightFirstCandidate = pindex ? (pindex->nHeight + 1) : 0;
	std::reverse(vSortedByTimestamp.begin(), vSortedByTimestamp.end());
	std::sort(vSortedByTimestamp.begin(), vSortedByTimestamp.end());

	// Select 64 blocks from candidate blocks to generate stake modifier
	uint64_t nStakeModifierNew = 0;
	int64_t nSelectionIntervalStop = nSelectionIntervalStart;
	std::map<uint256, const CBlockIndex*> mapSelectedBlocks;
	for (int nRound = 0; nRound < std::min(64, (int)vSortedByTimestamp.size()); nRound++)
	{
		// add an interval section to the current selection round
		nSelectionIntervalStop += GetStakeModifierSelectionIntervalSection(nRound);
		// select a block from the candidates of current round
		if (!SelectBlockFromCandidates(vSortedByTimestamp, mapSelectedBlocks, nSelectionIntervalStop, nStakeModifier, &pindex)) {
			return error("ComputeNextStakeModifier: unable to select block at round %d", nRound);
		}
		// write the entropy bit of the selected block
		nStakeModifierNew |= (((uint64_t)pindex->GetStakeEntropyBit()) << nRound);
		// add the selected block from candidates to selected list
		mapSelectedBlocks.insert(std::make_pair(pindex->GetBlockHash(), pindex));
		//        LogPrint("stakemodifier", "ComputeNextStakeModifier: selected round %d stop=%s height=%d bit=%d\n", nRound, DateTimeStrFormat("%Y-%m-%d %H:%M:%S", nSelectionIntervalStop), pindex->nHeight, pindex->GetStakeEntropyBit());
	}

	// Print selection map for visualization of the selected blocks
	if (LogAcceptCategory(BCLog::STAKEMODIF)) {
		std::string strSelectionMap = "";
		// '-' indicates proof-of-work blocks not selected
		strSelectionMap.insert(0, pindexPrev->nHeight - nHeightFirstCandidate + 1, '-');
		pindex = pindexPrev;
		while (pindex && pindex->nHeight >= nHeightFirstCandidate)
		{
			// '=' indicates proof-of-stake blocks not selected
			if (pindex->IsProofOfStake()) {
				strSelectionMap.replace(pindex->nHeight - nHeightFirstCandidate, 1, "=");
			}
			pindex = pindex->pprev;
		}
		for (auto & item : mapSelectedBlocks) {
			// 'S' indicates selected proof-of-stake blocks
			// 'W' indicates selected proof-of-work blocks
			strSelectionMap.replace(item.second->nHeight - nHeightFirstCandidate, 1, item.second->IsProofOfStake() ? "S" : "W");
		}
	}

	nStakeModifier = nStakeModifierNew;
	fGeneratedStakeModifier = true;
	return true;
}

// Get the last stake modifier and its generation time from a given block
static bool GetLastStakeModifier(const CBlockIndex* pindex, uint64_t& nStakeModifier, int64_t& nModifierTime)
{
	if (!pindex)
		return error("GetLastStakeModifier: null pindex");
	while (pindex && pindex->pprev && !pindex->GeneratedStakeModifier())
		pindex = pindex->pprev;
	if (!pindex->GeneratedStakeModifier()) {
		nStakeModifier = 1;
		nModifierTime = pindex->GetBlockTime();
		return true;
	}
	nStakeModifier = pindex->nStakeModifier;
	nModifierTime = pindex->GetBlockTime();
	return true;
}

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

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

// select a block from the candidate blocks in vSortedByTimestamp, excluding
// already selected blocks in vSelectedBlocks, and with timestamp up to
// nSelectionIntervalStop.
static bool SelectBlockFromCandidates(std::vector<std::pair<int64_t, uint256> >& vSortedByTimestamp, std::map<uint256, const CBlockIndex*>& mapSelectedBlocks,
	int64_t nSelectionIntervalStop, uint64_t nStakeModifierPrev, const CBlockIndex** pindexSelected)
{
	bool fSelected = false;
	uint256 hashBest = uint256();
	*pindexSelected = (const CBlockIndex*)0;
	for (auto & item : vSortedByTimestamp) {
		if (!mapBlockIndex.count(item.second))
			return error("SelectBlockFromCandidates: failed to find block index for candidate block %s", item.second.ToString());
		const CBlockIndex* pindex = mapBlockIndex[item.second];
		if (fSelected && pindex->GetBlockTime() > nSelectionIntervalStop) {
			break;

		}

		if (mapSelectedBlocks.count(pindex->GetBlockHash()) > 0)
			continue;
		// compute the selection hash by hashing its proof-hash and the
		// previous proof-of-stake modifier
		CDataStream ss(SER_GETHASH, 0);
		ss << ArithToUint256(pindex->hashProof) << nStakeModifierPrev;
		uint256 hashSelection = Hash(ss.begin(), ss.end());

		// the selection hash is divided by 2**32 so that proof-of-stake block
		// is always favored over proof-of-work block. this is to preserve
		// the energy efficiency property
		if (pindex->IsProofOfStake()) {
			hashSelection = ArithToUint256(UintToArith256(hashSelection) >> 32);
		}

		if (fSelected && hashSelection < hashBest) {
			hashBest = hashSelection;
			*pindexSelected = (const CBlockIndex*)pindex;
		}
		else if (!fSelected) {
			fSelected = true;
			hashBest = hashSelection;
			*pindexSelected = (const CBlockIndex*)pindex;
		}
	}
	return fSelected;
}

bool TransactionGetCoinAge(CTransactionRef transaction, uint64_t& nCoinAge) {
	CAmount bnCentSecond = 0;  // coin age in the unit of cent-seconds
	nCoinAge = 0;

	if (transaction->IsCoinBase()) {
		return true;
	}

	for (auto txin : transaction->vin) {
		// First try finding the previous transaction in database
		CTransactionRef txPrev;
		uint256 hashBlock = uint256();

		if (!GetTransaction(txin.prevout.hash, txPrev, Params().GetConsensus(), hashBlock, true))
			continue;  // previous transaction not in main chain

		if (transaction->nTime < txPrev->nTime)
			return false;  // Transaction timestamp violation

		if (mapBlockIndex.count(hashBlock) == 0)
			return false; //Block not found

		CBlockIndex* pblockindex = mapBlockIndex[hashBlock];

		if (pblockindex->nTime + Params().GetConsensus().nStakeMinAge > transaction->nTime)
			continue; // only count coins meeting min age requirement

		int64_t nValueIn = txPrev->vout[txin.prevout.n].nValue;
		bnCentSecond += CAmount(nValueIn) * (transaction->nTime - txPrev->nTime) / CENT;
	}

	CAmount bnCoinDay = ((bnCentSecond * CENT) / COIN) / (24 * 60 * 60);
	nCoinAge = bnCoinDay;

	return true;
}

bool GetRatingRewards(CAmount nCredit, std::vector<CTxOut>& results, CAmount& totalAmount, const CBlockIndex* pindexPrev, CDataStream& hashProofOfStakeSource, const CBlock* block)
{
    const int RATINGS_PAYOUT_MAX = 25;

	std::vector<std::string> vLotteryPost;
	std::vector<std::string> vLotteryComment;

    std::map<std::string, int> allPostRatings;
    std::map<std::string, int> allCommentRatings;

    CBlock blockPrev;
    ReadBlockFromDisk(blockPrev, pindexPrev, Params().GetConsensus());

    // Get all users from prev block for current lottery
    for (const auto& tx : blockPrev.vtx) {
        std::vector<std::string> vasm;
        if (!g_addrindex->FindPocketNetAsmString(tx, vasm)) continue;
        if ((vasm[1] == OR_SCORE || vasm[1] == OR_COMMENT_SCORE) && vasm.size() >= 4) {
            std::vector<unsigned char> _data_hex = ParseHex(vasm[3]);
            std::string _data_str(_data_hex.begin(), _data_hex.end());
            std::vector<std::string> _data;
            boost::split(_data, _data_str, boost::is_any_of("\t "));
            if (_data.size() >= 2) {
                std::string _address = _data[0];
                int _value = std::stoi(_data[1]);

                // For lottery use scores as 4=1 and 5=2 - Scores to posts
                if (vasm[1] == OR_SCORE && (_value == 4 || _value == 5)) {
                    // Get address of score initiator
                    reindexer::Item _score_itm;
                    if (!g_pocketdb->SelectOne( reindexer::Query("Scores").Where("txid", CondEq, tx->GetHash().GetHex()), _score_itm ).ok()) {
                        LogPrintf("--- GetRatingRewards error get score: %s\n", tx->GetHash().GetHex());
                        continue;
                    }

                    reindexer::Item _post_itm;
                    if (!g_pocketdb->SelectOne( reindexer::Query("Posts").Where("txid", CondEq, _score_itm["posttxid"].As<string>()), _post_itm ).ok()) {
                        LogPrintf("--- GetRatingRewards error get post: %s\n", _score_itm["posttxid"].As<string>());
                        continue;
                    }

                    if (g_antibot->AllowModifyReputationOverPost(_score_itm["address"].As<string>(), _post_itm["address"].As<string>(), pindexPrev->nHeight, tx, true)) {
                        if (allPostRatings.find(_address) == allPostRatings.end()) allPostRatings.insert(std::make_pair(_address, 0));
                        allPostRatings[_address] += (_value - 3);
                    }
                }

                // For lottery use scores as 1 and -1 - Scores to comments
                if (vasm[1] == OR_COMMENT_SCORE && (_value == 1)) {
                    // Get address of score initiator
                    reindexer::Item _score_itm;
                    if (!g_pocketdb->SelectOne( reindexer::Query("CommentScores").Where("txid", CondEq, tx->GetHash().GetHex()), _score_itm ).ok()) {
                        LogPrintf("--- GetRatingRewards error get comment score: %s\n", tx->GetHash().GetHex());
                        continue;
                    }

                    reindexer::Item _comment_itm;
                    if (!g_pocketdb->SelectOne( reindexer::Query("Comment").Where("otxid", CondEq, _score_itm["commentid"].As<string>()), _comment_itm ).ok()) {
                        LogPrintf("--- GetRatingRewards error get comment: %s\n", _score_itm["posttxid"].As<string>());
                        continue;
                    }

                    if (g_antibot->AllowModifyReputationOverComment(_score_itm["address"].As<string>(), _comment_itm["address"].As<string>(), pindexPrev->nHeight, tx, true)) {
                        if (allCommentRatings.find(_address) == allCommentRatings.end()) allCommentRatings.insert(std::make_pair(_address, 0));
                        allCommentRatings[_address] += _value;
                    }
                }
            }
        }
    }

    // Sort founded users
    {
        std::vector<std::pair<std::string, std::pair<int, arith_uint256>>> allPostSorted;
        std::vector<std::pair<std::string, std::pair<int, arith_uint256>>> allCommentSorted;

        {
            // Users with scores by post
            for (auto& it : allPostRatings) {
                CDataStream ss(hashProofOfStakeSource);
                ss << it.first;
                arith_uint256 hashSortRating = UintToArith256(Hash(ss.begin(), ss.end())) / it.second;
                allPostSorted.push_back(std::make_pair(it.first, std::make_pair(it.second, hashSortRating)));
            }

            // Users with scores by comment
            for (auto& it : allCommentRatings) {
                CDataStream ss(hashProofOfStakeSource);
                ss << it.first;
                arith_uint256 hashSortRating = UintToArith256(Hash(ss.begin(), ss.end())) / it.second;
                allCommentSorted.push_back(std::make_pair(it.first, std::make_pair(it.second, hashSortRating)));
            }
        }

        // Shrink founded users
        {
            // Users with scores by post
            if (allPostSorted.size() > 0) {
                std::sort(allPostSorted.begin(), allPostSorted.end(), [](auto & a, auto & b) {
                    return a.second.second < b.second.second;
                });

                if (allPostSorted.size() > RATINGS_PAYOUT_MAX) {
                    allPostSorted.resize(RATINGS_PAYOUT_MAX);
                }

                for (auto& it : allPostSorted) {
                    vLotteryPost.push_back(it.first);
                }
            }

            // Users with scores by comment
            if (allCommentSorted.size() > 0) {
                std::sort(allCommentSorted.begin(), allCommentSorted.end(), [](auto & a, auto & b) {
                    return a.second.second < b.second.second;
                });

                if (allCommentSorted.size() > RATINGS_PAYOUT_MAX) {
                    allCommentSorted.resize(RATINGS_PAYOUT_MAX);
                }

                for (auto& it : allCommentSorted) {
                    vLotteryComment.push_back(it.first);
                }
            }
        }
    }

	// Create transactions for all winners
    bool ret = false;
    ret = GenerateOuts(nCredit, results, totalAmount, vLotteryPost, OP_WINNER_POST) || ret;
    ret = GenerateOuts(nCredit, results, totalAmount, vLotteryComment, OP_WINNER_COMMENT) || ret;

    return ret;
}

bool GenerateOuts(CAmount nCredit, std::vector<CTxOut>& results, CAmount& totalAmount, std::vector<std::string> winners, opcodetype op_code_type) {
    if (winners.size() > 0 && winners.size() <= 25) {
        CAmount ratingReward = nCredit * 0.5;
        if (op_code_type == OP_WINNER_COMMENT) ratingReward = ratingReward / 10.0;
        totalAmount += ratingReward;

        int current = 0;
        const int rewardsCount = winners.size();
        CAmount rewardsPool = ratingReward;
        CAmount reward = ratingReward / rewardsCount;

        for (auto addr : boost::adaptors::reverse(winners)) {
            CAmount re;
            if (++current == rewardsCount) {
                re = rewardsPool;
            }
            else {
                rewardsPool = rewardsPool - reward;
                re = reward;
            }

            CTxDestination dest = DecodeDestination(addr);
            CScript scriptPubKey = GetScriptForDestination(dest);
            // TODO (brangr): Include with fork
            // scriptPubKey << op_code_type;
            results.push_back(CTxOut(re, scriptPubKey)); // send to ratings
        }

        return true;
    }

    return false;
}


/*
 *
 *    data: 2019-5-20
 *    auther : Ma Mengyu@National University of Defense Technology
 *    e-mail : mamengyu10@nudt.edu.cn
 *    description :
 *    Hybird-Parallel Visualization Engine
 *    run :
 *
 */

#include <ogrsf_frmts.h>
#include <ogr_p.h>
#include <cpl_conv.h>
#include <cpl_string.h>
#include "Redis.h"
#include <stdio.h>
#include <cstring>
#include <string>
#include <iostream>
#include <sstream>
#include <vector>
#include <dirent.h>
#include <omp.h>
#include <sys/time.h>
#include <regex.h>
#include <math.h>
#include <mpi.h>
#include <stdlib.h>
#include <queue>
#include <utility>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/index/rtree.hpp>
#include <boost/foreach.hpp>
#include <boost/interprocess/managed_mapped_file.hpp>
#include <boost/lambda/lambda.hpp>
#include <png.h>
#include <stdlib.h>

#define MAX_NODE_SIZE	8
#define MAX_TILE_PARAMS 256
#define TILE_SIZE	256
#define L		20037508.34

namespace bg	= boost::geometry;
namespace bgi	= boost::geometry::index;
namespace bgm	= boost::geometry::model;
namespace bi	= boost::interprocess;

using namespace std;

typedef bgm::d2::point_xy<double>									point;
typedef bgm::box<point>											box;
typedef bgm::segment<point>										segment;
typedef boost::tuple<segment, unsigned long, bool>							polygon_segment;
typedef std::pair<box, unsigned long>									polygon_box;
typedef bgi::quadratic<MAX_NODE_SIZE>									params;
typedef bgi::indexable<point>										indexable_point;
typedef bgi::equal_to<point>										equal_to_point;
typedef bi::allocator<point, bi::managed_mapped_file::segment_manager>					allocator_point;
typedef bgi::rtree<point, params, indexable_point, equal_to_point, allocator_point>			rtree_point;
typedef bgi::indexable<segment>										indexable_segment;
typedef bgi::equal_to<segment>										equal_to_segment;
typedef bi::allocator<segment, bi::managed_mapped_file::segment_manager>				allocator_segment;
typedef bgi::rtree<segment, params, indexable_segment, equal_to_segment, allocator_segment>		rtree_segment;
typedef bgi::indexable<polygon_segment>									indexable_psegment;
typedef bgi::equal_to<polygon_segment>									equal_to_psegment;
typedef bi::allocator<polygon_segment, bi::managed_mapped_file::segment_manager>			allocator_psegment;
typedef bgi::rtree<polygon_segment, params, indexable_psegment, equal_to_psegment, allocator_psegment>	rtree_psegment;
typedef bgi::indexable<polygon_box>									indexable_box;
typedef bgi::equal_to<polygon_box>									equal_to_box;
typedef bi::allocator<polygon_box, bi::managed_mapped_file::segment_manager>				allocator_box;
typedef bgi::rtree<polygon_box, params, indexable_box, equal_to_box, allocator_box>			rtree_box;

/**
 * @brief Compare x span of two polygons
 * @param a     Input polygon
 * @param b     Input polygon
 *
 * @return              If the x span of polygon a is smaller than than the x span of polygon b, return true; otherwise, return true.
 */
bool SortPolygon( polygon_box a, polygon_box b )
{
	return( (bg::get<1, 0>( a.first ) - bg::get<0, 0>( a.first ) ) < (bg::get<1, 0>( b.first ) - bg::get<0, 0>( b.first ) ) );
}

/**
 * @brief Split a string into a list where each word is a list item
 * @param argv          Input string
 * @param result[]  Output Word list
 * @param flag          Separator used to split the string
 * @param count         Output Word list length
 *
 */
void GetList( char* argv, char* result[], char* flag, int & count )
{
	char	* string = strdup( argv );
	char	* p;
	int	i = 0;
	while ( (p = strsep( &string, flag ) ) != NULL )
	{
		result[i] = p;
		i++;
	}
	result[i]	= string;
	count		= i;
}

/**
 * @brief Visualize Linestring Objects
 * @param z                     Zoom level of the tile
 * @param x                     x coordinate of the tile
 * @param y                     y coordinate of the tile
 * @param indexPath     Spatial index path
 * @param dataId                Dataset id
 * @param tile_area     Matrix indentifing different regions(e.g. background region, color transition region, the zones of rasterized spatial objects)
 *
 */
void  LineVision( int z, int x, int y, char* indexPath, char *dataId, char *tile_area )
{
	char		* tmp = new char[256];
	double		Rz, R, R1, R2, r, Rz4;
	rtree_segment	* rtree_segment_ptr;
	Rz	= L / (128 << z);       /* Calculate the pixel resolution of zoom level z */
	R	= 2 * Rz;               /* R=2*Rz ,we set the visualizaiton radius N to 2 (pixels) */
	R1	= 1.6464 * Rz;          /* R1=R-sqrt(2)/4*Rz=(2-sqrt(2)/4)*Rz */
	R2	= 2.3536 * Rz;          /* R2=R+sqrt(2)/4*Rz=(2+sqrt(2)/4)*Rz */
	r	= 0.7071 * R1;          /* r=sqrt(2)/2*R1 */
	Rz4	= 0.25 * Rz;            /* Rz4=1/4*Rz */
	sprintf( tmp, "%s%s", indexPath, dataId );
	bi::managed_mapped_file file( bi::open_only, tmp );
	rtree_segment_ptr = file.find<rtree_segment>( "rtree" ).first;
	#pragma omp parallel for num_threads(2)
	for ( int i = 0; i < 256; i++ )
	{
		for ( int j = 0; j < 256; j++ )
		{
			int tile_index = i * 256 + j;
			tile_area[tile_index] = 0;
			/* Calculate the web mercartor coordinate of the pixel */
			double	web_mercartor_x = (256 * x + j + 0.5) * Rz - L;
			double	web_mercartor_y = L - (256 * y + i - 0.5) * Rz;
			/* Query the spatial objects with InnerBox1(if there are lots of spatial objects within the distance R1 from the pixel, we query the spatial objects intersects the InnerBox1) */
			box					InnerBox1( point( web_mercartor_x - r, web_mercartor_y - r ), point( web_mercartor_x + r, web_mercartor_y + r ) );
			rtree_segment::const_query_iterator	it = rtree_segment_ptr->qbegin( bgi::intersects( InnerBox1 ) );
			if ( it != rtree_segment_ptr->qend() )
			{
				tile_area[tile_index] += 4;
			}else  {
				/* Query the spatial objects with OuterBox1(if there are few spatial objects in the neighbor of the pixel, we use the OuterBox1 to filter out the spatial objects which are far from the pixel) */
				box			OuterBox1( point( web_mercartor_x - R1, web_mercartor_y - R1 ), point( web_mercartor_x + R1, web_mercartor_y + R1 ) );
				std::vector<segment>	segment_result1;
				rtree_segment_ptr->query( bgi::intersects( OuterBox1 ) && bgi::nearest( point( web_mercartor_x, web_mercartor_y ), 1 ),
							  std::back_inserter( segment_result1 ) );
				if ( segment_result1.size() > 0 )
				{
					tile_area[tile_index] += 4;
				}else  {
					/* Determine whether the pixel belongs to the color transition regions (calculate the number of sub-pixels that are in the plotting region) */
					box			OuterBox2( point( web_mercartor_x - R2, web_mercartor_y - R2 ), point( web_mercartor_x + R2, web_mercartor_y + R2 ) );
					std::vector<segment>	segment_result2;
					rtree_segment_ptr->query( bgi::intersects( OuterBox2 ) && bgi::nearest( point( web_mercartor_x, web_mercartor_y ), 1 ),
								  std::back_inserter( segment_result2 ) );
					if ( segment_result2.size() > 0 )
					{
						if ( bg::distance( point( web_mercartor_x + Rz4, web_mercartor_y + Rz4 ), segment_result2.front() ) < R )
							tile_area[tile_index] += 1;
						if ( bg::distance( point( web_mercartor_x - Rz4, web_mercartor_y - Rz4 ), segment_result2.front() ) < R )
							tile_area[tile_index] += 1;
						if ( bg::distance( point( web_mercartor_x + Rz4, web_mercartor_y - Rz4 ), segment_result2.front() ) < R )
							tile_area[tile_index] += 1;
						if ( bg::distance( point( web_mercartor_x - Rz4, web_mercartor_y + Rz4 ), segment_result2.front() ) < R )
							tile_area[tile_index] += 1;
					}
				}
			}
		}
	}
}

/**
 * @brief Visualize Point Objects
 * @param z                     Zoom level of the tile
 * @param x                     x coordinate of the tile
 * @param y                     y coordinate of the tile
 * @param indexPath     Spatial index path
 * @param dataId                Dataset id
 * @param tile_area     Matrix indentifing different regions(e.g. background region, color transition region, the zones of rasterized spatial objects)
 *
 */
void  PointVision( int z, int x, int y, char* indexPath, char *dataId, char *tile_area )
{
	char		* tmp = new char[256];
	double		Rz, R, R1, R2, r, Rz4;
	rtree_point	* rtree_point_ptr;
	Rz	= L / (128 << z);       /* Calculate the pixel resolution of zoom level z */
	R	= 2 * Rz;               /* R=2*Rz ,we set the visualizaiton radius N to 2 (pixels) */
	R1	= 1.6464 * Rz;          /* R1=R-sqrt(2)/4*Rz=(2-sqrt(2)/4)*Rz */
	R2	= 2.3536 * Rz;          /* R2=R+sqrt(2)/4*Rz=(2+sqrt(2)/4)*Rz */
	r	= 0.7071 * R1;          /* r=sqrt(2)/2*R1 */
	Rz4	= 0.25 * Rz;            /* Rz4=1/4*Rz */
	sprintf( tmp, "%s%s", indexPath, dataId );
	bi::managed_mapped_file file( bi::open_only, tmp );
	rtree_point_ptr = file.find<rtree_point>( "rtree" ).first;
	#pragma omp parallel for num_threads(2)
	for ( int i = 0; i < 256; i++ )
	{
		for ( int j = 0; j < 256; j++ )
		{
			int tile_index = i * 256 + j;
			tile_area[tile_index] = 0;
			/* Calculate the web mercartor coordinate of the pixel */
			double	web_mercartor_x = (256 * x + j + 0.5) * Rz - L;
			double	web_mercartor_y = L - (256 * y + i - 0.5) * Rz;
			/* Query the spatial objects with InnerBox1(if there are lots of spatial objects within the distance R1 from the pixel, we query the spatial objects intersects the InnerBox1) */
			box					InnerBox1( point( web_mercartor_x - r, web_mercartor_y - r ), point( web_mercartor_x + r, web_mercartor_y + r ) );
			rtree_point::const_query_iterator	it = rtree_point_ptr->qbegin( bgi::intersects( InnerBox1 ) );
			if ( it != rtree_point_ptr->qend() )
			{
				tile_area[tile_index] += 4;
			}else  {
				/* Query the spatial objects with OuterBox1(if there are few spatial objects in the neighbor of the pixel, we use the OuterBox1 to filter out the spatial objects which are far from the pixel) */
				box			OuterBox1( point( web_mercartor_x - R1, web_mercartor_y - R1 ), point( web_mercartor_x + R1, web_mercartor_y + R1 ) );
				std::vector<point>	point_result1;
				rtree_point_ptr->query( bgi::intersects( OuterBox1 ) && bgi::nearest( point( web_mercartor_x, web_mercartor_y ), 1 ),
							std::back_inserter( point_result1 ) );
				if ( point_result1.size() > 0 )
				{
					tile_area[tile_index] += 4;
				}else  {
					/* Determine whether the pixel belongs to the color transition regions (calculate the number of sub-pixels that are in the plotting region) */
					box			OuterBox2( point( web_mercartor_x - R2, web_mercartor_y - R2 ), point( web_mercartor_x + R2, web_mercartor_y + R2 ) );
					std::vector<point>	point_result2;
					rtree_point_ptr->query( bgi::intersects( OuterBox2 ) && bgi::nearest( point( web_mercartor_x, web_mercartor_y ), 1 ),
								std::back_inserter( point_result2 ) );
					if ( point_result2.size() > 0 )
					{
						if ( bg::distance( point( web_mercartor_x + Rz4, web_mercartor_y + Rz4 ), point_result2.front() ) < R )
							tile_area[tile_index] += 1;
						if ( bg::distance( point( web_mercartor_x - Rz4, web_mercartor_y - Rz4 ), point_result2.front() ) < R )
							tile_area[tile_index] += 1;
						if ( bg::distance( point( web_mercartor_x + Rz4, web_mercartor_y - Rz4 ), point_result2.front() ) < R )
							tile_area[tile_index] += 1;
						if ( bg::distance( point( web_mercartor_x - Rz4, web_mercartor_y + Rz4 ), point_result2.front() ) < R )
							tile_area[tile_index] += 1;
					}
				}
			}
		}
	}
}

/**
 * @brief Visualize Pologon Objects
 * @param z                     Zoom level of the tile
 * @param x                     x coordinate of the tile
 * @param y                     y coordinate of the tile
 * @param indexPath     Spatial index path
 * @param dataId                Dataset id
 * @param tile_area     Matrix indentifing different regions(e.g. background region, color transition region, the zones of rasterized spatial objects)
 *
 */
void  PologonVision( int z, int x, int y, char* indexPath, char *dataId, char *tile_area )
{
	char		* tmp = new char[256];
	double		Rz, R, R1, R2, r, Rz4;
	rtree_psegment	* rtree_psegment_ptr;
	rtree_box	* rtree_box_ptr;
	Rz	= L / (128 << z);       /* Calculate the pixel resolution of zoom level z */
	R	= 2 * Rz;               /* R=2*Rz ,we set the visualizaiton radius N to 2 (pixels) */
	R1	= 1.6464 * Rz;          /* R1=R-sqrt(2)/4*Rz=(2-sqrt(2)/4)*Rz */
	R2	= 2.3536 * Rz;          /* R2=R+sqrt(2)/4*Rz=(2+sqrt(2)/4)*Rz */
	r	= 0.7071 * R1;          /* r=sqrt(2)/2*R1 */
	Rz4	= 0.25 * Rz;            /* Rz4=1/4*Rz */
	sprintf( tmp, "%s%s", indexPath, dataId );
	bi::managed_mapped_file file( bi::open_only, tmp );
	rtree_psegment_ptr = file.find<rtree_psegment>( "rtree" ).first;
	sprintf( tmp, "%s%s_mbr", indexPath, dataId );
	bi::managed_mapped_file file_mbr( bi::open_only, tmp );
	rtree_box_ptr = file_mbr.find<rtree_box>( "rtree" ).first;
	#pragma omp parallel for num_threads(2)
	for ( int i = 0; i < 256; i++ )
	{
		for ( int j = 0; j < 256; j++ )
		{
			int tile_index = i * 256 + j;
			tile_area[tile_index] = 0;
			/* Calculate the web mercartor coordinate of the pixel */
			double	web_mercartor_x = (256 * x + j + 0.5) * Rz - L;
			double	web_mercartor_y = L - (256 * y + i - 0.5) * Rz;
			/* Spatial-Index-Based Filling */
			std::vector<polygon_box> box_result;
			rtree_box_ptr->query( bgi::intersects( point( web_mercartor_x, web_mercartor_y ) ), std::back_inserter( box_result ) ); /* Find the candidate polygons */
			sort( box_result.begin(), box_result.end(), SortPolygon );                                                              /* Sort the polygons(Polygon with smaller x span has higher priority) */
			BOOST_FOREACH( polygon_box const & v, box_result )                                                                      /* measure the spatial relationship between the pixel and each candidate polygon one by one until the polygon which contains the pixel is found */
			{
				unsigned long			pID	= v.second;
				box				pBox	= v.first;
				double				minx	= bg::get<0, 0>( pBox );
				double				maxx	= bg::get<1, 0>( pBox );
				int				lcount	= 0;
				std::vector<polygon_segment>	psegment_result;
				if ( web_mercartor_x - minx < maxx - web_mercartor_x )
					rtree_psegment_ptr->query( bgi::intersects( segment( point( minx, web_mercartor_y ), point( web_mercartor_x, web_mercartor_y ) ) ),
								   std::back_inserter( psegment_result ) );
				else
					rtree_psegment_ptr->query( bgi::intersects( segment( point( web_mercartor_x, web_mercartor_y ), point( maxx, web_mercartor_y ) ) ),
								   std::back_inserter( psegment_result ) );
				BOOST_FOREACH( polygon_segment const & sv, psegment_result )
				{
					bool		level	= boost::get<2>( sv );
					unsigned long	pIDtmp	= boost::get<1>( sv );
					if ( level && pID == pIDtmp )
						lcount++;
				}
				if ( lcount % 2 > 0 ) /* pixel is classified as ’inside the polygon’ if the number of crossings is odd, or ’outside’ if it is an even number */
				{
					tile_area[tile_index] = 4;
					break;
				}
			}

			/* Query the spatial objects with InnerBox1(if there are lots of spatial objects within the distance R1 from the pixel, we query the spatial objects intersects the InnerBox1) */
			box					InnerBox1( point( web_mercartor_x - r, web_mercartor_y - r ), point( web_mercartor_x + r, web_mercartor_y + r ) );
			rtree_psegment::const_query_iterator	it = rtree_psegment_ptr->qbegin( bgi::intersects( InnerBox1 ) );
			if ( it != rtree_psegment_ptr->qend() )
			{
				if ( tile_area[tile_index] > 0 )
					tile_area[tile_index] += 4;
				else
					tile_area[tile_index] -= 4;
			}else  {
				/* Query the spatial objects with OuterBox1(if there are few spatial objects in the neighbor of the pixel, we use the OuterBox1 to filter out the spatial objects which are far from the pixel) */
				box				OuterBox1( point( web_mercartor_x - R1, web_mercartor_y - R1 ), point( web_mercartor_x + R1, web_mercartor_y + R1 ) );
				std::vector<polygon_segment>	segment_result1;
				rtree_psegment_ptr->query( bgi::intersects( OuterBox1 ) && bgi::nearest( point( web_mercartor_x, web_mercartor_y ), 1 ),
							   std::back_inserter( segment_result1 ) );
				if ( segment_result1.size() > 0 )
				{
					if ( tile_area[tile_index] > 0 )
						tile_area[tile_index] += 4;
					else
						tile_area[tile_index] -= 4;
				}else  {
					/* Determine whether the pixel belongs to the color transition regions (calculate the number of sub-pixels that are in the plotting region) */
					box				OuterBox2( point( web_mercartor_x - R2, web_mercartor_y - R2 ), point( web_mercartor_x + R2, web_mercartor_y + R2 ) );
					std::vector<polygon_segment>	segment_result2;
					rtree_psegment_ptr->query( bgi::intersects( OuterBox2 ) && bgi::nearest( point( web_mercartor_x, web_mercartor_y ), 1 ),
								   std::back_inserter( segment_result2 ) );
					if ( segment_result2.size() > 0 )
					{
						segment pSegment = boost::get<0>( segment_result2.front() );
						if ( tile_area[tile_index] > 0 )
						{
							if ( bg::distance( point( web_mercartor_x + Rz4, web_mercartor_y + Rz4 ), pSegment ) < R )
								tile_area[tile_index] += 1;
							if ( bg::distance( point( web_mercartor_x - Rz4, web_mercartor_y - Rz4 ), pSegment ) < R )
								tile_area[tile_index] += 1;
							if ( bg::distance( point( web_mercartor_x + Rz4, web_mercartor_y - Rz4 ), pSegment ) < R )
								tile_area[tile_index] += 1;
							if ( bg::distance( point( web_mercartor_x - Rz4, web_mercartor_y + Rz4 ), pSegment ) < R )
								tile_area[tile_index] += 1;
						}else{
							if ( bg::distance( point( web_mercartor_x + Rz4, web_mercartor_y + Rz4 ), pSegment ) < R )
								tile_area[tile_index] -= 1;
							if ( bg::distance( point( web_mercartor_x - Rz4, web_mercartor_y - Rz4 ), pSegment ) < R )
								tile_area[tile_index] -= 1;
							if ( bg::distance( point( web_mercartor_x + Rz4, web_mercartor_y - Rz4 ), pSegment ) < R )
								tile_area[tile_index] -= 1;
							if ( bg::distance( point( web_mercartor_x - Rz4, web_mercartor_y + Rz4 ), pSegment ) < R )
								tile_area[tile_index] -= 1;
						}
					}
				}
			}
		}
	}
}

/**
 * @brief HiVision visualization engine main function
 * @param nArgc         Input parameters copunt
 * @param papszArgv Input startup parameters
 *
 */
int main( int nArgc, char ** papszArgv )
{
	int myId, numProcs;
	MPI_Init( &nArgc, &papszArgv );
	MPI_Comm_rank( MPI_COMM_WORLD, &myId );
	MPI_Comm_size( MPI_COMM_WORLD, &numProcs );
	double	t1, t2;
	char	* indexPath	= papszArgv[1];         /* Path to store the spatial indexes */
	char	* redisHost	= papszArgv[2];         /* Host IP of Redis */
	int	redisPort	= atoi( papszArgv[3] ); /* Host Port of Redis */
	CPLSetConfigOption( "GDAL_FILENAME_IS_UTF8", "NO" );
	CPLSetConfigOption( "SHAPE_ENCODING", "UTF-8" );
	/* Connect Redis */
	Redis *redis = new Redis();
	if ( !redis->connect( redisHost, redisPort ) )
	{
		printf( "connect redis error!\n" );
		exit( 0 );
		MPI_Finalize();
	}
	char	*tile_area	= (char *) malloc( TILE_SIZE * TILE_SIZE );
	char	*buffer_tmp	= (char *) malloc( TILE_SIZE * TILE_SIZE );
	char	* task		= new char[256];
	char	* tile_params[MAX_TILE_PARAMS];
	int	count = 0;
	int	x, y, z;
	if ( myId == 0 )
	{
		printf( "indexPath:%s\n", indexPath );
		printf( "Service Start. cores:%d\n", numProcs );
	}
	while ( 1 )
	{
		/* Get tasks from Task Pool */
		sprintf( task, "%s", redis->brpop( "HiVisiontasklist" ).c_str() );
		try{
			if ( strlen( task ) > 0 )
			{
				t1 = MPI_Wtime();
				/* Parse the parameters */
				GetList( task, tile_params, (char *) "/", count );
				z	= atoi( tile_params[1] );
				x	= atoi( tile_params[2] );
				y	= atoi( tile_params[3] );
				if ( tile_params[0][0] == 'l' )                         /* Linestring visualizaiton */
					LineVision( z, x, y, indexPath, tile_params[0], tile_area );
				else if ( tile_params[0][0] == 'p' )                    /* Point visualizaiton */
					PointVision( z, x, y, indexPath, tile_params[0], tile_area );
				else if ( tile_params[0][0] == 'a' )                    /* Polygon visulization */
					PologonVision( z, x, y, indexPath, tile_params[0], tile_area );
				redis->zset( task, tile_area, TILE_SIZE * TILE_SIZE );  /* Write the result to the Result Pool */
				while ( !redis->zget( task, buffer_tmp ) )
					redis->zset( task, tile_area, TILE_SIZE * TILE_SIZE );
				redis->expire( task, "1000" );                          /* Set expire time to 1000s(expired results will be cleaned up if memory usage reaches the upper limit) */
				redis->pub( "HiVisiontiles", task );                    /* Send task completion message */
				t2 = MPI_Wtime();
				printf( "tile-%s-%d-%f\n", task, myId, t2 - t1 );
			}
		}
		catch ( ... )
		{
			printf( "Error task %s \n", task );
		}
	}
	MPI_Finalize();
}


// Copyright (c) FRC Team 3512. All Rights Reserved.

#pragma once

#include <limits>
#include <vector>

#include <Eigen/Core>
#include <frc/ADIS16470_IMU.h>
#include <frc/AnalogInput.h>
#include <frc/Encoder.h>
#include <frc/controller/ImplicitModelFollower.h>
#include <frc/controller/ProfiledPIDController.h>
#include <frc/controller/SimpleMotorFeedforward.h>
#include <frc/drive/DifferentialDrive.h>
#include <frc/estimator/AngleStatistics.h>
#include <frc/estimator/KalmanFilter.h>
#include <frc/estimator/KalmanFilterLatencyCompensator.h>
#include <frc/filter/LinearFilter.h>
#include <frc/geometry/Pose2d.h>
#include <frc/geometry/Translation2d.h>
#include <frc/kinematics/DifferentialDriveOdometry.h>
#include <frc/motorcontrol/MotorControllerGroup.h>
#include <frc/simulation/ADIS16470_IMUSim.h>
#include <frc/simulation/AnalogInputSim.h>
#include <frc/simulation/DifferentialDrivetrainSim.h>
#include <frc/simulation/EncoderSim.h>
#include <frc/smartdashboard/Field2d.h>
#include <frc/smartdashboard/FieldObject2d.h>
#include <frc/system/plant/LinearSystemId.h>
#include <frc/trajectory/TrajectoryConfig.h>
#include <frc/trajectory/TrapezoidProfile.h>
#include <networktables/NetworkTableEntry.h>
#include <photonlib/PhotonCamera.h>
#include <rev/CANSparkMax.h>
#include <units/acceleration.h>
#include <units/angle.h>
#include <units/angular_velocity.h>
#include <units/current.h>
#include <units/length.h>
#include <units/time.h>
#include <units/velocity.h>

#include "Constants.hpp"
#include "HWConfig.hpp"
#include "NetworkTableUtil.hpp"
#include "Vision.hpp"
#include "controllers/DrivetrainController.hpp"
#include "subsystems/ControlledSubsystemBase.hpp"

namespace frc3512 {

/**
 * The drivetrain subsystem.
 *
 * The drivetrain uses a Kalman Filter for encoder position and velocity
 * estimation and the DifferentialDriveOdometry class for pose estimation.
 */
class Drivetrain : public ControlledSubsystemBase<7, 2, 5> {
public:
    /// The drivetrain length.
    static constexpr units::meter_t kLength = 0.5851068_m;

    /**
     * Distance from middle of robot to intake.
     */
    static constexpr units::meter_t kMiddleOfRobotToIntake = 0.656_m;

    /**
     * Producer-consumer queue for global pose measurements from Vision
     * subsystem.
     */
    wpi::static_circular_buffer<Vision::GlobalMeasurements, 8> visionQueue;

    Drivetrain();

    Drivetrain(const Drivetrain&) = delete;
    Drivetrain& operator=(const Drivetrain&) = delete;

    /**
     * Returns the drivetrain's reference pose.
     */
    frc::Pose2d GetReferencePose() const;

    /**
     * Returns the drivetrain's pose estimate.
     */
    frc::Pose2d GetPose() const;

    /**
     * Returns gyro's heading measurement in the global coordinate frame.
     */
    units::radian_t GetAngle() const;

    /**
     * Returns left encoder displacement.
     */
    units::meter_t GetLeftPosition() const;

    /**
     * Returns right encoder displacement.
     */
    units::meter_t GetRightPosition() const;

    /**
     * Returns left encoder velocity.
     */
    units::meters_per_second_t GetLeftVelocity() const;

    /**
     * Returns right encoder velocity.
     */
    units::meters_per_second_t GetRightVelocity() const;

    /**
     * Returns longitudinal acceleration from IMU.
     */
    units::meters_per_second_squared_t GetAccelerationX() const;

    /**
     * Returns lateral acceleration from IMU.
     */
    units::meters_per_second_squared_t GetAccelerationY() const;

    /**
     * Resets all sensors and controller.
     */
    void Reset(const frc::Pose2d& initialPose = frc::Pose2d());

    /**
     * Adds a trajectory with the given waypoints.
     *
     * This can be called more than once to create a queue of trajectories.
     * Closed-loop control will be enabled to track the first trajectory.
     *
     * @param start    Starting pose.
     * @param interior Intermediate waypoints excluding heading.
     * @param end      Ending pose.
     */
    void AddTrajectory(const frc::Pose2d& start,
                       const std::vector<frc::Translation2d>& interior,
                       const frc::Pose2d& end);

    /**
     * Adds a trajectory with the given waypoints.
     *
     * This can be called more than once to create a queue of trajectories.
     * Closed-loop control will be enabled to track the first trajectory.
     *
     * @param start    Starting pose.
     * @param interior Intermediate waypoints excluding heading.
     * @param end      Ending pose.
     * @param config   TrajectoryConfig for this trajectory. This can include
     *                 constraints on the trajectory dynamics. If adding custom
     *                 constraints, it is recommended to start with the config
     *                 returned by MakeTrajectoryConfig() so differential drive
     *                 dynamics constraints are included automatically.
     */
    void AddTrajectory(const frc::Pose2d& start,
                       const std::vector<frc::Translation2d>& interior,
                       const frc::Pose2d& end,
                       const frc::TrajectoryConfig& config);

    /**
     * Adds a trajectory with the given waypoints.
     *
     * This can be called more than once to create a queue of trajectories.
     * Closed-loop control will be enabled to track the first trajectory.
     *
     * @param waypoints Waypoints.
     */
    void AddTrajectory(const std::vector<frc::Pose2d>& waypoints);

    /**
     * Adds a trajectory with the given waypoints.
     *
     * This can be called more than once to create a queue of trajectories.
     * Closed-loop control will be enabled to track the first trajectory.
     *
     * @param waypoints Waypoints.
     * @param config    TrajectoryConfig for this trajectory. This can include
     *                  constraints on the trajectory dynamics. If adding custom
     *                  constraints, it is recommended to start with the config
     *                  returned by MakeTrajectoryConfig() so differential drive
     *                  dynamics constraints are included automatically.
     */
    void AddTrajectory(const std::vector<frc::Pose2d>& waypoints,
                       const frc::TrajectoryConfig& config);

    /**
     * Adds a trajectory with a pre-determined trajectory.
     *
     * This can be called more than once to create a queue of trajectories.
     * Closed-loop control will be enabled to track the first trajectory.
     *
     * @param trajectory Trajectory.
     * @param config    TrajectoryConfig for this trajectory. This can include
     *                  constraints on the trajectory dynamics. If adding custom
     *                  constraints, it is recommended to start with the config
     *                  returned by MakeTrajectoryConfig() so differential drive
     *                  dynamics constraints are included automatically.
     */
    void AddTrajectory(
        const frc::Trajectory trajectory,
        const frc::TrajectoryConfig& config = MakeTrajectoryConfig());

    /**
     * Sets a new heading goal for the drivetrain to achieve.
     *
     * @param heading The heading you want to the drivetrain to achieve (in
     * radians)
     */
    void SetHeadingGoal(const units::radian_t heading);

    /**
     * Returns whether or not a new heading goal is set.
     */
    bool HasHeadingGoal() const;

    /**
     * Aborts turning in place.
     */
    void AbortTurnInPlace();

    /**
     * Returns a TrajectoryConfig containing a differential drive dynamics
     * constraint with the start and end velocities set to zero.
     */
    static frc::TrajectoryConfig MakeTrajectoryConfig();

    /**
     * Returns a TrajectoryConfig containing a differential drive dynamics
     * constraint and the specified start and end velocities.
     *
     * @param startVelocity The start velocity of the trajectory.
     * @param endVelocity   The end velocity of the trajectory.
     */
    static frc::TrajectoryConfig MakeTrajectoryConfig(
        units::meters_per_second_t startVelocity,
        units::meters_per_second_t endVelocity);

    /**
     * Returns whether the drivetrain controller is at the goal waypoint.
     */
    bool AtGoal() const;

    /**
     * Returns whether the drivetrain is at the goal heading.
     */
    bool AtHeading();

    /**
     * Returns the current heading state. Used for setting points after
     * turn-in-places
     */
    units::radian_t GetHeading();

    /**
     * Sets the tolerance of the turn-in-place ProfiledPID.
     *
     * @param headingTolerance The tolerance on the heading.
     * @param velocityTolerance The tolerance on the velocity.
     */
    void SetTurningTolerance(units::radian_t headingTolerance,
                             units::radians_per_second_t velocityTolerance =
                                 units::radians_per_second_t{
                                     std::numeric_limits<double>::infinity()});

    /**
     * Sets the constraint of the turn-in-place ProfiledPID.
     *
     * @param constraint the new constraint of the controller.
     */
    void SetTurningConstraints(
        frc::TrapezoidProfile<units::radian>::Constraints constraint);

    /**
     * Returns the drivetrain state estimate.
     */
    const Eigen::Vector<double, 7>& GetStates();

    /**
     * Returns the drivetrain inputs.
     */
    const Eigen::Vector<double, 2>& GetInputs() const;

    /**
     * Returns current drawn in simulation.
     */
    units::ampere_t GetCurrentDraw() const;

    /**
     * Returns sim pose.
     */
    frc::Pose2d GetSimPose() const;

    /**
     * Returns the yaw from vision
     */
    units::radian_t GetVisionYaw();

    /**
     * Aims the drivetrain using vision.
     */
    void AimWithVision();

    /**
     * Disengage aiming with vision.
     */
    void DisengageVisionAim();

    /**
     * Returns whether or not the robot is aiming with vision.
     */
    bool IsVisionAiming() const;

    /**
     * Returns whether or not the robot is looking at the center of the vision
     * target.
     */
    bool AtVisionTarget() const;

    /**
     * Returns whether or not the robot is moving.
     */
    bool IsStationary();

    void DisabledInit() override;

    void AutonomousInit() override;

    void TeleopInit() override;

    void TestInit() override;

    void RobotPeriodic() override;

    void TeleopPeriodic() override;

    void TestPeriodic() override;

    void ControllerPeriodic() override;

private:
    static constexpr double kTurningP = 3.0;
    static constexpr double kTurningI = 0.0;
    static constexpr double kTurningD = 0.0;

    static const Eigen::Matrix<double, 2, 2> kGlobalR;

    static frc::LinearSystem<2, 2, 2> kPlant;

    rev::CANSparkMax m_leftLeader{HWConfig::Drivetrain::kLeftMotorLeaderID,
                                  rev::CANSparkMax::MotorType::kBrushless};
    rev::CANSparkMax m_leftFollower{HWConfig::Drivetrain::kLeftMotorFollowerID,
                                    rev::CANSparkMax::MotorType::kBrushless};
    frc::MotorControllerGroup m_leftGrbx{m_leftLeader, m_leftFollower};

    rev::CANSparkMax m_rightLeader{HWConfig::Drivetrain::kRightMotorLeaderID,
                                   rev::CANSparkMax::MotorType::kBrushless};
    rev::CANSparkMax m_rightFollower{
        HWConfig::Drivetrain::kRightMotorFollowerID,
        rev::CANSparkMax::MotorType::kBrushless};
    frc::MotorControllerGroup m_rightGrbx{m_rightLeader, m_rightFollower};

    frc::Encoder m_leftEncoder{HWConfig::Drivetrain::kLeftEncoderA,
                               HWConfig::Drivetrain::kLeftEncoderB};

    frc::Encoder m_rightEncoder{HWConfig::Drivetrain::kRightEncoderA,
                                HWConfig::Drivetrain::kRightEncoderB};

    frc::ADIS16470_IMU m_imu;
    units::radian_t m_headingOffset = 0_rad;

    units::meter_t m_leftPos;
    units::meter_t m_lastLeftPos;
    units::meter_t m_rightPos;
    units::meter_t m_lastRightPos;
    units::second_t m_time = frc::Timer::GetFPGATimestamp();
    units::second_t m_lastTime = m_time - Constants::kControllerPeriod;

    units::meters_per_second_t m_leftVelocity;
    units::meters_per_second_t m_rightVelocity;
    // Filters out encoder quantization noise
    frc::LinearFilter<units::meters_per_second_t> m_leftVelocityFilter =
        frc::LinearFilter<units::meters_per_second_t>::MovingAverage(4);
    frc::LinearFilter<units::meters_per_second_t> m_rightVelocityFilter =
        frc::LinearFilter<units::meters_per_second_t>::MovingAverage(4);

    frc::KalmanFilter<2, 2, 2> m_velocityObserver{
        kPlant,
        {0.25, 0.25},
        {DrivetrainController::kDpP / Constants::kControllerPeriod.value(),
         DrivetrainController::kDpP / Constants::kControllerPeriod.value()},
        Constants::kControllerPeriod};

    frc::DifferentialDriveOdometry m_observer{frc::Rotation2d(), frc::Pose2d()};
    Eigen::Vector<double, 7> m_xHat = Eigen::Vector<double, 7>::Zero();

    DrivetrainController m_controller;
    Eigen::Vector<double, 2> m_u = Eigen::Vector<double, 2>::Zero();

    frc3512::Vision vision;

    frc::Timer m_visionTimer;
    bool m_aimWithVision = false;
    bool m_atVisionTarget = false;

    frc::TrapezoidProfile<units::radian>::Constraints m_turningConstraints{
        6_rad_per_s, 3.3_rad_per_s_sq};
    frc::ProfiledPIDController<units::radian> m_turningPID{
        kTurningP, kTurningI, kTurningD, m_turningConstraints,
        Constants::kControllerPeriod};
    bool m_hasNewHeading = false;
    frc::SimpleMotorFeedforward<units::radian> m_turningFeedforward{
        0.17964_V, 2.6447_V / 1_rad_per_s};
    frc2::PIDController m_aimPID{kTurningP, kTurningI, kTurningD};

    frc::DifferentialDrive m_visionAim{m_leftGrbx, m_rightGrbx};
    frc2::PIDController m_visionController{1.5, kTurningI, 0.25};

    frc::LinearSystem<2, 2, 2> m_imfRef =
        frc::LinearSystemId::IdentifyDrivetrainSystem(
            DrivetrainController::kLinearV, DrivetrainController::kLinearA,
            DrivetrainController::kAngularV, DrivetrainController::kAngularA);
    frc::ImplicitModelFollower<2, 2> m_imf{kPlant, m_imfRef, 20_ms};

    // Simulation variables
    frc::sim::DifferentialDrivetrainSim m_drivetrainSim{
        DrivetrainController::GetPlant(), DrivetrainController::kWidth,
        frc::DCMotor::NEO(2), DrivetrainController::kDriveGearRatio,
        DrivetrainController::kWheelRadius};
    frc::sim::EncoderSim m_leftEncoderSim{m_leftEncoder};
    frc::sim::EncoderSim m_rightEncoderSim{m_rightEncoder};
    frc::sim::ADIS16470_IMUSim m_imuSim{m_imu};
    frc::Field2d m_field;

    nt::NetworkTableEntry m_headingGoalEntry = NetworkTableUtil::MakeBoolEntry(
        "/Diagnostics/Drivetrain/Outputs/Goal Heading Achieved");

    nt::NetworkTableEntry m_yawControllerEntry =
        NetworkTableUtil::MakeDoubleEntry(
            "/Diagnostics/Drivetrain/Outputs/Controller Yaw Value");

    nt::NetworkTableEntry m_rangeControllerEntry =
        NetworkTableUtil::MakeDoubleEntry(
            "/Diagnostics/Drivetrain/Outputs/Controller Range Value");

    nt::NetworkTableEntry m_hasHeadingGoalEntry =
        NetworkTableUtil::MakeBoolEntry(
            "/Diagnostics/Drivetrain/Outputs/Has New Goal Heading");

    nt::NetworkTableEntry m_currHeadingEntry =
        NetworkTableUtil::MakeDoubleEntry(
            "/Diagnostics/Drivetrain/Outputs/Current Heading");

    nt::NetworkTableEntry m_headingGoalValueEntry =
        NetworkTableUtil::MakeDoubleEntry(
            "/Diagnostics/Drivetrain/Outputs/Heading Goal");

    nt::NetworkTableEntry m_rotationEntry =
        NetworkTableUtil::MakeDoubleEntry("/Diagnostics/Drivetrain/Rotation");
    nt::NetworkTableEntry m_isStationaryEntry = NetworkTableUtil::MakeBoolEntry(
        "/Diagnostics/Drivetrain/Outputs/Is Stationary");

    /**
     * Set drivetrain motors to brake mode, which the feedback controllers
     * expect.
     */
    void SetBrakeMode();

    /**
     * Set drivetrain motors to coast mode so the robot is easier to push when
     * it's disabled.
     */
    void SetCoastMode();
};

}  // namespace frc3512


//==================================================================================================
/*!
    @file

    @Copyright 2016 Numscale SAS
    @copyright 2016 J.T.Lapreste

    Distributed under the Boost Software License, Version 1.0.
    (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_X86_SSE2_SIMD_FUNCTION_INBTRUE_HPP_INCLUDED
#define BOOST_SIMD_ARCH_X86_SSE2_SIMD_FUNCTION_INBTRUE_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/meta/as_logical.hpp>
#include <boost/simd/function/simd/genmask.hpp>
#include <boost/simd/function/simd/popcnt.hpp>
#include <boost/simd/meta/cardinal_of.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd =  boost::dispatch;
  namespace bs =  boost::simd;
  BOOST_DISPATCH_OVERLOAD ( inbtrue_
                          , (typename A0)
                          , bs::sse2_
                          , bs::pack_<bd::integer_<A0>, bs::sse_>
                         )
  {
    BOOST_FORCEINLINE std::size_t operator() ( const A0 & a0) const BOOST_NOEXCEPT
    {
       using i8type = pack<int8_t, 16>;
       i8type tmp = bitwise_cast<i8type>(genmask(a0));
       return  bs::popcnt(_mm_movemask_epi8(tmp)) * bs::cardinal_of<A0>::value >> 4;
//      return 0;
    }
  };
  BOOST_DISPATCH_OVERLOAD ( inbtrue_
                          , (typename A0)
                          , bs::sse2_
                          , bs::pack_<bs::logical_<A0>, bs::sse_>
                         )
  {
    BOOST_FORCEINLINE std::size_t operator() ( const A0 & a0 ) const BOOST_NOEXCEPT
    {
      using i8type = pack<int8_t, 16>;
      i8type tmp = bitwise_cast<i8type>(genmask(a0));
      return  bs::popcnt(_mm_movemask_epi8(tmp)) * bs::cardinal_of<A0>::value >> 4;
    }
  };
  BOOST_DISPATCH_OVERLOAD ( inbtrue_
                          , (typename A0)
                          , bs::sse2_
                          , bs::pack_<bd::double_<A0>, bs::sse_>
                         )
  {
    BOOST_FORCEINLINE std::size_t operator() ( const A0 & a0) const BOOST_NOEXCEPT
    {
      int32_t  r = _mm_movemask_pd(genmask(a0));
      return   (r&1)+(r>>1);
    }
  };

} } }

#endif


#define BOOST_TEST_MODULE "test_read_implicit_membrane_potential"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif
#include <mjolnir/input/read_external_potential.hpp>
#include <tuple>

using test_types = std::tuple<double, float>;

constexpr inline float  tolerance_value(float)  noexcept {return 1e-4;}
constexpr inline double tolerance_value(double) noexcept {return 1e-8;}

template<typename Real>
decltype(boost::test_tools::tolerance(std::declval<Real>()))
tolerance() {return boost::test_tools::tolerance(tolerance_value(Real()));}

BOOST_AUTO_TEST_CASE_TEMPLATE(read_implicit_membrane_noenv, T, test_types)
{
    mjolnir::LoggerManager::set_default_logger("test_read_implicit_membrane.log");

    using real_type = T;
    {
        using namespace toml::literals;
        const auto v = u8R"(
            interaction    = "Distance"
            potential      = "ExcludedVolumeWall"
            shape.name     = "AxisAlignedPlane"
            shape.axis     = "-X"
            shape.position = 1.0
            shape.margin   = 0.5
            thickness      = 3.14
            interaction_magnitude = 6.28
            bend                  = 9.42
            parameters = [
                {index = 0, hydrophobicity = 2.0},
                {index = 1, hydrophobicity = 3.0},
                {index = 5, hydrophobicity = 4.0},
            ]
        )"_toml;

        const auto g = mjolnir::read_implicit_membrane_potential<real_type>(v);

        BOOST_TEST(g.participants().size() == 3u);
        BOOST_TEST(g.participants().at(0)  == 0u);
        BOOST_TEST(g.participants().at(1)  == 1u);
        BOOST_TEST(g.participants().at(2)  == 5u);
        BOOST_TEST(g.parameters().at(0)    == real_type(2.0       ), tolerance<real_type>());
        BOOST_TEST(g.parameters().at(1)    == real_type(3.0       ), tolerance<real_type>());
        BOOST_TEST(g.parameters().at(5)    == real_type(4.0       ), tolerance<real_type>());
        BOOST_TEST(g.half_thick()          == real_type(3.14 * 0.5), tolerance<real_type>());
        BOOST_TEST(g.k()                   == real_type(6.28      ), tolerance<real_type>());
        BOOST_TEST(g.bend()                == real_type(9.42      ), tolerance<real_type>());
        BOOST_TEST(g.cutoff()              == real_type(4.0       ), tolerance<real_type>());
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(read_implicit_membrane_env, T, test_types)
{
    mjolnir::LoggerManager::set_default_logger("test_read_implicit_membrane.log");

    using real_type = T;
    {
        using namespace toml::literals;
        const auto v = u8R"(
            interaction    = "Distance"
            potential      = "ExcludedVolumeWall"
            shape.name     = "AxisAlignedPlane"
            shape.axis     = "-X"
            shape.position = 1.0
            shape.margin   = 0.5
            thickness      = 3.14
            cutoff         = 5.0
            interaction_magnitude = 6.28
            bend                  = 9.42
            env.hydrophilic =   0.0
            env.hydrophobic = 100.0
            parameters = [
                {index = 0, hydrophobicity = "hydrophilic"},
                {index = 1, hydrophobicity = "hydrophobic"},
                {index = 5, hydrophobicity = "hydrophobic"},
            ]
        )"_toml;

        const auto g = mjolnir::read_implicit_membrane_potential<real_type>(v);

        BOOST_TEST(g.participants().size() == 3u);
        BOOST_TEST(g.participants().at(0)  == 0u);
        BOOST_TEST(g.participants().at(1)  == 1u);
        BOOST_TEST(g.participants().at(2)  == 5u);
        BOOST_TEST(g.parameters().at(0)    == real_type(  0.0     ), tolerance<real_type>());
        BOOST_TEST(g.parameters().at(1)    == real_type(100.0     ), tolerance<real_type>());
        BOOST_TEST(g.parameters().at(5)    == real_type(100.0     ), tolerance<real_type>());
        BOOST_TEST(g.half_thick()          == real_type(3.14 * 0.5), tolerance<real_type>());
        BOOST_TEST(g.k()                   == real_type(6.28      ), tolerance<real_type>());
        BOOST_TEST(g.bend()                == real_type(9.42      ), tolerance<real_type>());
        BOOST_TEST(g.cutoff()              == real_type(5.0       ), tolerance<real_type>());
    }
}


// Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>

// Version: 1.0
// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl

// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.

// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// Lesser General Public License for more details.

// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

//Based on the svd of the KDL-0.2 library by Erwin Aertbelien
#ifndef SVD_EIGEN_HH_HPP
#define SVD_EIGEN_HH_HPP

#include <Eigen/Core>
#include <algorithm>

using namespace Eigen;

namespace KDL
{
    inline double PYTHAG(double a,double b) {
        double at,bt,ct;
        at = fabs(a);
        bt = fabs(b);
        if (at > bt ) {
            ct=bt/at;
            return at*sqrt(1.0+ct*ct);
        } else {
            if (bt==0)
                return 0.0;
            else {
                ct=at/bt;
                return bt*sqrt(1.0+ct*ct);
            }
        }
    }

    inline double SIGN(double a,double b) {
        return ((b) >= 0.0 ? fabs(a) : -fabs(a));
    }

    /**
     * svd calculation of eigen matrices
     *
     * @param A matrix<double>(mxn)
     * @param U matrix<double>(mxn)
     * @param S vector<double> n
     * @param V matrix<double>(nxn)
     * @param tmp vector<double> n
     * @param maxiter defaults to 150
     *
     * @return -2 if maxiter exceeded, 0 otherwise
     */
    int svd_eigen_HH(const MatrixXd& A,MatrixXd& U,VectorXd& S,MatrixXd& V,VectorXd& tmp,int maxiter=150,double epsilon=1e-300);
}
#endif


/*
 *
 * Copyright (c) Kresimir Fresl 2002
 *
 * Permission to copy, modify, use and distribute this software
 * for any non-commercial or commercial purpose is granted provided
 * that this license appear on all copies of the software source code.
 *
 * Author assumes no responsibility whatsoever for its use and makes
 * no guarantees about its quality, correctness or reliability.
 *
 * Author acknowledges the support of the Faculty of Civil Engineering,
 * University of Zagreb, Croatia.
 *
 */

#ifndef BOOST_NUMERIC_BINDINGS_CBLAS_HPP
#define BOOST_NUMERIC_BINDINGS_CBLAS_HPP

#include <boost/numeric/bindings/atlas/cblas1.hpp>
#include <boost/numeric/bindings/atlas/cblas2.hpp>
#include <boost/numeric/bindings/atlas/cblas3.hpp>

#endif


#include "headers/RayTracing.hpp"
#include "headers/Object.hpp"
#include "headers/Ambient.hpp"
#include "headers/Light.hpp"
#include "headers/Reader.hpp"
#include "headers/NoArvore.hpp"
#include "headers/NVertex.hpp"
#include "headers/Writer.hpp"
#include <vector>
#include <cstdlib>
#include <armadillo>
#include <iostream>

using namespace arma;
using namespace std;

int RayTracing::inicio(const char *config, const char *saida) {

    NoArvore * No = NULL;
    vector<Object *> objetos;
    vector<Light> luz;
    NVertex normal_boladona;
    Ambient ambiente;
    mat keye;
    int cont_tri=0;
    unsigned int porc;

    Reader(config, objetos, luz, ambiente, keye, cont_tri);

    if (objetos.size() == 0){
        cout << "Nenhum objeto encontrado" << endl;
        return 1;
    }

    else{
        if (objetos.size() >= 100) porc=objetos.size() * 0.003;
        else porc=5;

        normal_boladona.N(objetos, cont_tri);
        No = NoArvore::construirArvore(objetos, porc);
        Writer(saida, luz, keye, ambiente, No);
    }

    cout << "Triângulo: " << cont_tri << endl;
    cout << "Esfera: " << objetos.size()-cont_tri << endl;

  return 0;
}


#include <imgui-SFML.h>
#include <imgui.h>

#include <Eigen/Core>
#include <SFML/Graphics/CircleShape.hpp>
#include <SFML/Graphics/RenderWindow.hpp>
#include <SFML/System/Clock.hpp>
#include <SFML/Window/Event.hpp>
#include <iostream>
#include <memory>
#include <opencv2/opencv.hpp>
#include <random>

//#include <SDL2/SDL.h>

#include "coin-flipper.hpp"
#include "foo.hpp"

class MyRng final : public Rng {
 public:
  MyRng() : rng(std::make_unique<std::mt19937>(dev())) {}

  double generate(double min, double max) override;

 private:
  std::random_device dev;
  std::unique_ptr<std::mt19937> rng;
};

double MyRng::generate(const double min, const double max) {
  std::uniform_real_distribution<> rdist(min, max);
  return rdist(*rng);
}

int main() {
  // SDL_Window *window = NULL;

  Eigen::MatrixXd m(2, 2);
  m(0, 0) = 3;
  m(1, 0) = 2.5;
  m(0, 1) = -1;
  m(1, 1) = m(1, 0) + m(0, 1);
  std::cout << m << std::endl;

  std::cout << "do stuff\n";

  const auto x = 4;
  std::cout << x << "\n";
  std::cout << independentMethod(x) << "\n";
  std::cout << Foo::bar(x) << "\n";

  MyRng myGenerator;

  // Create a game
  const CoinFlipper game(&myGenerator);

  // Start playing
  const auto flip = game.flipCoin();

  const auto result = (flip == CoinFlipper::Result::Heads)
                          ? std::string("Heads")
                          : std::string("Tails");

  std::cout << "Result: " << result << "\n";

  // OpenCV

  // initialize a 120X350 matrix of black pixels:
  // cv::Mat output = cv::Mat::zeros(120, 350, CV_8UC3);

  // write text on the matrix:
  // putText(output, "Hello World :)", cvPoint(15, 70), cv::FONT_HERSHEY_PLAIN,
  // 3, cvScalar(0, 255, 0), 4);

  // cv::imshow("test", output);
  // cv::waitKey(0);

  return 0;
}


/* CirKit: A circuit toolkit
 * Copyright (C) 2009-2015  University of Bremen
 * Copyright (C) 2015-2017  EPFL
 *
 * 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 "add_stg.hpp"

#include <numeric>

#include <boost/random/detail/integer_log2.hpp>

#include <core/utils/range_utils.hpp>
#include <reversible/functions/add_gates.hpp>
#include <reversible/target_tags.hpp>
#include <reversible/utils/permutation.hpp>

namespace cirkit
{

/******************************************************************************
 * Types                                                                      *
 ******************************************************************************/

/******************************************************************************
 * Private functions                                                          *
 ******************************************************************************/

/******************************************************************************
 * Public functions                                                           *
 ******************************************************************************/

void add_stg_as_other( circuit& circ, const tt& func, const tt& func_real, const std::vector<unsigned>& line_map )
{
  const auto num_vars = tt_num_vars( func );

  std::vector<kitty::detail::spectral_operation> trans;
  const auto func_norm = kitty::exact_spectral_canonization( to_kitty( func ),
                                                             [&trans]( const std::vector<kitty::detail::spectral_operation>& ops ) {
                                                               std::copy( ops.begin(), ops.end(), std::back_inserter( trans ) );
                                                             } );
  const auto func_real_norm = kitty::exact_spectral_canonization( to_kitty( func_real ),
                                                                  [&trans]( const std::vector<kitty::detail::spectral_operation>& ops ) {
                                                                    std::copy( ops.rbegin(), ops.rend(), std::back_inserter( trans ) );
                                                                  } );

  assert( func_norm == func_real_norm );

  std::vector<unsigned> line( num_vars + 1u );
  if ( !line_map.empty() )
  {
    std::copy( line_map.begin(), line_map.end(), line.begin() );
  }
  else
  {
    std::iota( line.begin(), line.end(), 0u );
  }

  auto index = circ.num_gates();

  /* we defer the adding of NOT gates to the end, but NOT gates are just stored
     and used to adjust the polarities of the controls.
     We also try to merge an output inversion into the control line of a disjoint
     transformation. */
  boost::dynamic_bitset<> not_mask( num_vars + 1 );

  /* we also defer adding SWAP gates to the end.  The swaps are stored in a permutation
     that is used when other gates are added.  Finally the SWAP gates are computed by
     decomposing the permutation into transpositions. */
  permutation_t perm = identity_permutation( num_vars );

  for ( const auto& t : trans )
  {
    switch ( t._kind )
    {
    default:
      assert( false );
    case kitty::detail::spectral_operation::kind::permutation:
      std::swap( perm[boost::integer_log2( t._var1 )], perm[boost::integer_log2( t._var2 )] );
      break;
    case kitty::detail::spectral_operation::kind::input_negation:
      not_mask.flip( perm[boost::integer_log2( t._var1 )] );
      break;
    case kitty::detail::spectral_operation::kind::output_negation:
      not_mask.flip( num_vars );
      break;
    case kitty::detail::spectral_operation::kind::spectral_translation:
    {
      const auto v1 = perm[boost::integer_log2( t._var1 )];
      const auto v2 = perm[boost::integer_log2( t._var2 )];
      insert_cnot( circ, index, make_var( line[v2], !not_mask.test( v2 ) ), line[v1] );
      insert_cnot( circ, index, make_var( line[v2], !not_mask.test( v2 ) ), line[v1] );
      ++index;
    }
    break;
    case kitty::detail::spectral_operation::kind::disjoint_translation:
    { /* disjoint transformation of output */
      const auto v1 = perm[boost::integer_log2( t._var1 )];

      auto pol = !not_mask.test( v1 );
      if ( not_mask.test( num_vars ) )
      {
        not_mask.reset( num_vars );
        pol = !pol;
      }
      insert_cnot( circ, index, make_var( line[v1], pol ), line[num_vars] );
    }
    break;
    }
  }

  /* add SWAPS */
  const auto perm_inv = permutation_invert( perm );
  for ( const auto& trans : permutation_to_transpositions( perm_inv ) )
  {
    insert_fredkin( circ, index, {}, line[trans.first], line[trans.second] );
    insert_fredkin( circ, index, {}, line[trans.first], line[trans.second] );
    ++index;
  }

  /* add inverters from mask */
  for ( auto i = 0u; i < num_vars; ++i )
  {
    if ( not_mask.test( i ) )
    {
      insert_not( circ, index, line[perm_inv[i]] );
      insert_not( circ, index, line[perm_inv[i]] );
      ++index;
    }
  }
  if ( not_mask.test( num_vars ) )
  {
    insert_not( circ, index, line[num_vars] );
  }

  /* add middle gate */
  auto& g = circ.insert_gate( index );
  g.set_type( stg_tag( func_real ) );
  for ( auto i = 0u; i < num_vars; ++i )
  {
    g.add_control( make_var( line[i], true ) );
  }
  g.add_target( line[num_vars] );
}
}

// Local Variables:
// c-basic-offset: 2
// eval: (c-set-offset 'substatement-open 0)
// eval: (c-set-offset 'innamespace 0)
// End:


#include <ros/ros.h>
#include <ros/package.h>
#include <fstream>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/Image.h>
#include <image_geometry/pinhole_camera_model.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <yaml-cpp/yaml.h>
#include <boost/filesystem.hpp>
#include "geometry_msgs/Point.h"
#include "ground_projection/GetGroundCoord.h"
#include "ground_projection/GetImageCoord.h"
#include "ground_projection/EstimateHomography.h"
#include "duckietown_msgs/Pixel.h"
#include "duckietown_msgs/Vector2D.h"
#include "duckietown_msgs/Segment.h"
#include "duckietown_msgs/SegmentList.h"
#include <std_srvs/Empty.h>

namespace enc = sensor_msgs::image_encodings;

#ifdef HAVE_NEW_YAMLCPP
template<typename T>
void operator >> (const YAML::Node& node, T& i)
{
  i = node.as<T>();
}
#endif

class GroundProjection
{
  ros::NodeHandle nh_;
  ros::ServiceServer service_homog_;
  ros::ServiceServer service_gnd_coord_;
  ros::ServiceServer service_img_coord_;
  ros::Subscriber sub_lineseglist_;
  ros::Publisher pub_lineseglist_;
  sensor_msgs::CameraInfo::ConstPtr camera_info_;
  image_geometry::PinholeCameraModel pcm_;
  cv::Mat H_, Hinv_;
  bool rectified_input_;

  std::string node_name_;

  std::string config_name_;
  std::string config_file_name_;
  std::string h_file_;

public:
  GroundProjection()
  : nh_("~")
  {

    node_name_ = ros::this_node::getName();

    // load from homography yaml file
    nh_.param<std::string>("config",config_name_,"baseline");
    nh_.param<std::string>("config_file_name",config_file_name_,"test");
    h_file_ = ros::package::getPath("duckietown") + "/config/" + config_name_ + "/calibration/camera_extrinsic/" + config_file_name_ + ".yaml";
    std::ifstream fin(h_file_.c_str());
    if (!fin.good())
    {
      ROS_WARN_STREAM("Can't find homography file: " << h_file_ << " Using default calibration instead.");
      h_file_ = ros::package::getPath("duckietown") + "/config/" + config_name_ + "/calibration/camera_extrinsic/test.yaml";
      // h_file_ = get_package_filename("package://ground_projection/homography/default.yaml");
    }
    ROS_INFO("load from homography yaml file [%s].", h_file_.c_str());

    std::vector<float> h;
    h.resize(9);
    read_homography_yaml(h_file_, h);

    // update homography matrix (H_)
    H_.create(3, 3, CV_32F);
    for(int r=0; r<3; r++)
    {
      for(int c=0; c<3; c++)
      {
        H_.at<float>(r, c) = h[r*3+c];
      }
    }
    // estimate inv(H_)
    Hinv_ = H_.inv();

    nh_.param<bool>("rectified_input", rectified_input_, false);

    std::string camera_info_topic = nh_.resolveName("camera_info");
    ROS_INFO("[%s] Waiting for message on topic: %s", node_name_.c_str(),camera_info_topic.c_str());
    camera_info_ = ros::topic::waitForMessage<sensor_msgs::CameraInfo>(camera_info_topic, nh_);
    ROS_INFO("[%s] Got camera_info", node_name_.c_str());

    pcm_.fromCameraInfo(camera_info_);

    // Publisher
    pub_lineseglist_ = nh_.advertise<duckietown_msgs::SegmentList>("lineseglist_out", 1);
    ROS_INFO("@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@");
    // Ready the services
    service_homog_ = nh_.advertiseService("estimate_homography", &GroundProjection::estimate_homography_cb, this);
    ROS_INFO("estimate_homography is ready.");
    service_gnd_coord_ = nh_.advertiseService("get_ground_coordinate", &GroundProjection::get_ground_coordinate_cb, this);
    ROS_INFO("get_ground_coordinate is ready.");
    service_img_coord_  = nh_.advertiseService("get_image_coordinate", &GroundProjection::get_image_coordinate_cb, this);
    ROS_INFO("get_image_coordinate is ready.");

    // Subscriber
    sub_lineseglist_ = nh_.subscribe("lineseglist_in", 1, &GroundProjection::lineseglist_cb, this);
  }

  ~GroundProjection()
  {
  }

private:
  duckietown_msgs::Pixel vector2pixel(const duckietown_msgs::Vector2D& vec)
  {
    float w = static_cast<float>(camera_info_->width);
    float h = static_cast<float>(camera_info_->height);

    duckietown_msgs::Pixel pixel;

    pixel.u = static_cast<int>(w * vec.x);
    pixel.v = static_cast<int>(h * vec.y);

    // boundary check (might be redundant, but just in case)
    if(pixel.u < 0)     pixel.u = 0;
    if(pixel.u > w-1)   pixel.u = w-1;
    if(pixel.v < 0)     pixel.v = 0;
    if(pixel.v > h-1)   pixel.v = h-1;

    return pixel;
  }

  duckietown_msgs::Vector2D pixel2vector(const duckietown_msgs::Pixel pixel)
  {
    float w = static_cast<float>(camera_info_->width);
    float h = static_cast<float>(camera_info_->height);

    duckietown_msgs::Vector2D vec;

    vec.x = static_cast<float>(pixel.u)/w;
    vec.y = static_cast<float>(pixel.v)/h;

    return vec;
  }

  void getRectifiedImage(const sensor_msgs::Image::ConstPtr img_msg, cv::Mat &mat_rect)
  {
    // Convert to cv::Mat
    // cv_bridge::CvImagePtr cv_img = cv_bridge::toCvCopy(img_msg);
    cv_bridge::CvImagePtr cv_img = cv_bridge::toCvCopy(img_msg,enc::MONO8);
    pcm_.rectifyImage(cv_img->image,mat_rect);
  }

  void image2ground(const duckietown_msgs::Vector2D& vec, geometry_msgs::Point& point)
  {
    duckietown_msgs::Pixel pixel = vector2pixel(vec);
    image2ground(pixel, point);
  }

  void ground2image(const geometry_msgs::Point& point, duckietown_msgs::Vector2D& vec)
  {
    duckietown_msgs::Pixel pixel;
    ground2image(point, pixel);
    vec = pixel2vector(pixel);
  }

  void image2ground(const duckietown_msgs::Pixel& pixel, geometry_msgs::Point& point)
  {
    cv::Point3f pt_img(static_cast<float>(pixel.u), static_cast<float>(pixel.v), 1.f);

    if(!rectified_input_)
    {
      // point-wise undistortion
      cv::Point2d pt_undistorted = pcm_.rectifyPoint(
        cv::Point2d(static_cast<double>(pt_img.x), static_cast<double>(pt_img.y))
      );

      pt_img.x = static_cast<float>(pt_undistorted.x);
      pt_img.y = static_cast<float>(pt_undistorted.y);
    }

    cv::Mat pt_gnd_ = H_ * cv::Mat(pt_img);
    cv::Point3f pt_gnd(pt_gnd_);

    point.x = pt_gnd.x/pt_gnd.z;
    point.y = pt_gnd.y/pt_gnd.z;
    point.z = 0.f;
  }

  void ground2image(const geometry_msgs::Point& point, duckietown_msgs::Pixel& pixel)
  {
    cv::Point3f pt_gnd(static_cast<float>(point.x), static_cast<float>(point.y), 1.f);

    cv::Mat pt_img_ = Hinv_ * cv::Mat(pt_gnd);
    cv::Point3f pt_img(pt_img_);

    pt_img.x /= pt_img.z;
    pt_img.y /= pt_img.z;

    if(!rectified_input_)
    {
      // get distorted image coordinate (pt_distorted) from undistorted one (pt_img)
      cv::Point2d pt_distorted = pcm_.unrectifyPoint(
        cv::Point2d(static_cast<double>(pt_img.x), static_cast<double>(pt_img.y))
      );

      pixel.u = static_cast<int>(pt_distorted.x);
      pixel.v = static_cast<int>(pt_distorted.y);
    }
    else
    {
      pixel.u = static_cast<int>(pt_img.x);
      pixel.v = static_cast<int>(pt_img.y);
    }
  }

  void lineseglist_cb(const duckietown_msgs::SegmentList::ConstPtr& msg)
  {
    duckietown_msgs::SegmentList msg_new = *msg;
    for(int i=0; i<msg_new.segments.size(); i++)
    {
      // each line segment is composed of two end points [0:1]
      image2ground(msg_new.segments[i].pixels_normalized[0], msg_new.segments[i].points[0]);
      image2ground(msg_new.segments[i].pixels_normalized[1], msg_new.segments[i].points[1]);
    }
    cout>>"yes";
    pub_lineseglist_.publish(msg_new);
  }

  bool get_ground_coordinate_cb(ground_projection::GetGroundCoord::Request &req,
                                ground_projection::GetGroundCoord::Response &res)
  {
    image2ground(req.normalized_uv, res.gp);

    ROS_INFO("normalized image coord (norm u=%f, norm v=%f), ground coord (x=%f, y=%f, z=%f)", req.normalized_uv.x, req.normalized_uv.y, res.gp.x, res.gp.y, res.gp.z);
    return true;
  }

  bool get_image_coordinate_cb(ground_projection::GetImageCoord::Request &req,
                                ground_projection::GetImageCoord::Response &res)
  {
    ground2image(req.gp, res.normalized_uv);

    ROS_INFO("ground coord (x=%f, y=%f, z=%f), normalized image coord (norm u=%f, norm v=%f)", req.gp.x, req.gp.y, req.gp.z, res.normalized_uv.x, res.normalized_uv.y);
    return true;
  }

  bool estimate_homography_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
  {
    ROS_INFO("Estimating homography...");
    ROS_INFO_STREAM("Waiting from raw image on topic: " << nh_.resolveName("cali_image"));
    sensor_msgs::Image::ConstPtr img_msg = ros::topic::waitForMessage<sensor_msgs::Image>("cali_image", nh_);
    ROS_INFO_STREAM("Received raw image on topic: " << nh_.resolveName("cali_image"));

    cv::Mat mat_rect;
    cv::Mat mat_homography;
    getRectifiedImage(img_msg,mat_rect);
    ROS_INFO("Raw image rectifed.");

    if(estimate_homography(mat_rect,mat_homography)){
      ROS_INFO("Homography estimated.");
    }
    else{
      ROS_WARN("Homography estimation failed.");
      return false;
    }

    mat_homography.copyTo(H_);
    std::vector<float> vec_homography(9,0);
    // Fill homography vector
    for(int r=0; r<3; r++)
    {
      for(int c=0; c<3; c++)
      {
        vec_homography[r*3+c] = mat_homography.at<float>(r, c);
      }
    }
    // Write to yaml file
    std::string wrtie_file_path = ros::package::getPath("duckietown") + "/config/" + config_name_ + "/calibration/camera_extrinsic" + ros::this_node::getNamespace() + ".yaml";
    ROS_INFO("Homography yaml file [%s].", wrtie_file_path.c_str());
    write_homography_yaml(wrtie_file_path, vec_homography);
    return true;
  }

  bool estimate_homography(cv::Mat& Ir, cv::Mat& H)
  {
    // estimate homography
    int board_w, board_h;
    float square_size;
    nh_.param<int>("board_w", board_w, 7);
    nh_.param<int>("board_h", board_h, 5);
    nh_.param<float>("square_size", square_size, 0.031f);

    int board_n = board_w * board_h;

    cv::Size board_size(board_w, board_h);
    std::vector<cv::Point2f> corners_;
    bool found = findChessboardCorners(Ir, board_size, corners_);
    if(found)
    {
      cornerSubPix(Ir, corners_, cv::Size(11, 11), cv::Size(-1, -1),
        cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));
    }
    else
    {
      ROS_ERROR("Couldn't find a checkerboard");
      return false;
    }

    std::vector<cv::Point2f> pts_gnd_, pts_img_;
    pts_gnd_.resize(board_w*board_h);
    pts_img_.resize(board_w*board_h);

    float x_offset, y_offset;
    nh_.param<float>("x_offset", x_offset, 0.191f);
    nh_.param<float>("y_offset", y_offset, -0.093f);
    cv::Point2f offset = cv::Point2f(x_offset, y_offset);

    cv::Point2f corner_lr = corners_[0]; // lower-right point
    cv::Point2f corner_ur  = corners_[(board_h-1)*(board_w)]; // upper-right point
    cv::Point2f corner_ul  = corners_[(board_h)*(board_w)-1]; // upper-left point

    // std::cout << "corner_lr: " << corner_lr.x << ", " << corner_lr.y << std::endl;
    // std::cout << "corner_ur: " << corner_ur.x << ", " << corner_ur.y << std::endl;
    // std::cout << "corner_ul: " << corner_ul.x << ", " << corner_ul.y << std::endl;

    bool h_flipped = false, v_flipped = false;
    if(corner_ul.x > corner_ur.x)
    {
      // rows are horizontally flipped
      h_flipped = true;
    }

    if(corner_lr.y < corner_ur.y)
    {
      // cols are vertically flipped
      v_flipped = true;
    }

    for(int r=0; r<board_h; r++)
    {
      for(int c=0; c<board_w; c++)
      {
        pts_gnd_[r*(board_w)+c] = cv::Point2f(float(r)*square_size, float(c)*square_size) + offset;
        pts_img_[r*(board_w)+c] = corners_[(v_flipped ? board_h - 1 - r : r)*(board_w)+(h_flipped ? board_w - 1 - c : c)];
      }
    }

    H = cv::findHomography(pts_img_, pts_gnd_, CV_RANSAC);
    H.convertTo(H, CV_32F);

    return true;
  }

  bool read_homography_yaml(const std::string& h_fname,
                            std::vector<float>& h)
  {
    std::ifstream fin(h_fname.c_str());
    if (!fin.good())
    {
      ROS_INFO("Unable to open homography file [%s]", h_fname.c_str());
      return false;
    }
    // bool success = readCalibrationYml(fin, camera_name, cam_info);
    bool success = read_homography_yaml_(fin, h);

    if (!success)
      ROS_ERROR("Failed to parse homography from file [%s]", h_fname.c_str());
    return success;
  }

  bool read_homography_yaml_(std::istream& fin, std::vector<float>& h)
  {
    try
    {
#ifdef HAVE_NEW_YAMLCPP
      YAML::Node doc = YAML::Load(fin);
#else
      YAML::Parser parser(fin);
      if (!parser)
      {
        ROS_ERROR("Unable to create YAML parser for homography");
        return false;
      }
      YAML::Node doc;
      parser.GetNextDocument(doc);
#endif
      assert(h.size() == 9);

      const YAML::Node& h_data = doc["homography"];
      for(int i = 0; i < 9; ++i)
        h_data[i] >> h[i];

      return true;
    }
    catch (YAML::Exception& e) {
      ROS_ERROR("Exception parsing YAML homography:\n%s", e.what());
      return false;
    }
  }

  bool write_homography_yaml(const std::string& h_fname,
                             const std::vector<float>& h)
  {
    boost::filesystem::path dir(boost::filesystem::path(h_fname).parent_path());
    if(!dir.empty() && !boost::filesystem::exists(dir) &&
      !boost::filesystem::create_directories(dir))
    {
      ROS_ERROR("Unable to create directory for homography file [%s]", dir.c_str());
    }

    std::ofstream out(h_fname.c_str());
    if(!out.is_open())
    {
      ROS_ERROR("Unable to open homography file [%s] for writing", h_fname.c_str());
      return false;
    }

    YAML::Emitter emitter;
    emitter << YAML::BeginMap;

    emitter << YAML::Key << "homography" << YAML::Value << YAML::Flow;

    emitter << YAML::BeginSeq;
    for (int i = 0; i < h.size(); i++)
      emitter << h[i];
    emitter << YAML::EndSeq;

    emitter << YAML::EndMap;

    out << emitter.c_str();
    return true;
  }

  std::string get_package_filename(const std::string &url)
  {
    ROS_DEBUG_STREAM("homography URL: " << url);

    // Scan URL from after "package://" until next '/' and extract
    // package name.
    size_t prefix_len = std::string("package://").length();
    size_t rest = url.find('/', prefix_len);
    std::string package(url.substr(prefix_len, rest - prefix_len));

    // Look up the ROS package path name.
    std::string pkgPath(ros::package::getPath(package));
    if(pkgPath.empty())
    {
      ROS_WARN_STREAM("unknown package: " << package << " (ignored)");
      return pkgPath;
    }
    else
    {
      // Construct file name from package location and remainder of URL.
      return pkgPath + url.substr(rest);
    }
  }

};

int main(int argc, char **argv)
{
  ros::init(argc, argv, "ground_projection");

  GroundProjection gp;

  ros::spin();

  return 0;
}