/**
 * @file dex.cpp
 *
 * This file contains DEx logic.
 */

#include <tradelayer/dex.h>

#include <tradelayer/convert.h>
#include <tradelayer/errors.h>
#include <tradelayer/externfns.h>
#include <tradelayer/log.h>
#include <tradelayer/mdex.h>
#include <tradelayer/rules.h>
#include <tradelayer/sp.h>
#include <tradelayer/tradelayer.h>
#include <tradelayer/uint256_extensions.h>

#include <arith_uint256.h>
#include <hash.h>
#include <tinyformat.h>
#include <uint256.h>

#include <stdint.h>
#include <fstream>
#include <map>
#include <set>
#include <string>
#include <utility>
#include <vector>

#include <boost/algorithm/string.hpp>
#include <boost/format.hpp>

std::map<int, std::map<uint32_t,int64_t>> mastercore::MapLTCVolume;
std::map<int, std::map<uint32_t,int64_t>> mastercore::MapTokenVolume;
std::map<uint32_t,std::map<int,std::vector<std::pair<int64_t,int64_t>>>> mastercore::tokenvwap;

namespace mastercore
{

extern CMPSPInfo *_my_sps;
/**
 * Checks, if such a sell offer exists.
 */
bool DEx_offerExists(const std::string& addressSeller, uint32_t propertyId)
{
    std::string key = STR_SELLOFFER_ADDR_PROP_COMBO(addressSeller, propertyId);

    return !(my_offers.find(key) == my_offers.end());
}

/**
 * Retrieves a sell offer.
 *
 * @return The sell offer, or NULL, if no match was found
 */
CMPOffer* DEx_getOffer(const std::string& addressSeller, uint32_t propertyId)
{
    if (msc_debug_dex) PrintToLog("%s(%s, %d)\n", __func__, addressSeller, propertyId);

    std::string key = STR_SELLOFFER_ADDR_PROP_COMBO(addressSeller, propertyId);
    OfferMap::iterator it = my_offers.find(key);

    if (it != my_offers.end()) return &(it->second);

    return static_cast<CMPOffer*>(nullptr);
}

/**
 * Checks, if such an accept order exists.
 */
bool DEx_acceptExists(const std::string& addressSeller, uint32_t propertyId, const std::string& addressBuyer)
{
    std::string key = STR_ACCEPT_ADDR_PROP_ADDR_COMBO(addressSeller, addressBuyer, propertyId);

    return !(my_accepts.find(key) == my_accepts.end());
}

/**
 * Retrieves an accept order.
 *
 * @return The accept order, or NULL, if no match was found
 */
CMPAccept* DEx_getAccept(const std::string& addressSeller, uint32_t propertyId, const std::string& addressBuyer)
{
    if (msc_debug_dex) PrintToLog("%s(%s, %d, %s)\n", __func__, addressSeller, propertyId, addressBuyer);

    std::string key = STR_ACCEPT_ADDR_PROP_ADDR_COMBO(addressSeller, addressBuyer, propertyId);
    AcceptMap::iterator it = my_accepts.find(key);

    if (it != my_accepts.end()) {
      if (msc_debug_dex) PrintToLog("%s(): ORDER FOUND!, getAcceptBlock : %d\n",__func__, (it->second).getAcceptBlock());
      return &(it->second);
    }

    if (msc_debug_dex) PrintToLog("%s(): accept order not found!\n",__func__);

    return static_cast<CMPAccept*>(nullptr);
}

/**
 * Determines the amount of litecoins desired, in case it needs to be recalculated.
 *
 * TODO: don't expose it!
 * @return The amount of litecoins desired
 */
int64_t calculateDesiredLTC(const int64_t amountOffered, const int64_t amountDesired, const int64_t amountAvailable)
{
    if (amountOffered == 0) {
        return 0; // divide by null protection
    }

    arith_uint256 amountOffered256 = ConvertTo256(amountOffered);
    arith_uint256 amountDesired256 = ConvertTo256(amountDesired);
    arith_uint256 amountAvailable256 = ConvertTo256(amountAvailable);

    amountDesired256 = DivideAndRoundUp((amountDesired256 * amountAvailable256), amountOffered256);

    return ConvertTo64(amountDesired256);
}

/**
 * Creates a new sell offer.
 *
 * @return 0 if everything is OK
 */
int DEx_offerCreate(const std::string& addressSeller, uint32_t propertyId, int64_t amountOffered, int block, int64_t amountDesired, int64_t minAcceptFee, uint8_t paymentWindow, const uint256& txid, uint64_t* nAmended)
{
    int rc = DEX_ERROR_SELLOFFER;

    // sanity checks
    // shold not be removed, because it may be used when updating/destroying an offer
    if (paymentWindow == 0) {
        return (DEX_ERROR_SELLOFFER -101);
    }
    if (amountDesired == 0) {
        return (DEX_ERROR_SELLOFFER -102);
    }

    if (propertyId == TL_PROPERTY_VESTING) {
        return (DEX_ERROR_SELLOFFER -103);
    }

    if (DEx_getOffer(addressSeller, propertyId)) {
        return (DEX_ERROR_SELLOFFER -10); // offer already exists
    }

    const std::string key = STR_SELLOFFER_ADDR_PROP_COMBO(addressSeller, propertyId);
    if (msc_debug_dex) PrintToLog("%s(%s|%s), nValue=%d)\n", __func__, addressSeller, key, amountOffered);

    const int64_t balanceReallyAvailable = getMPbalance(addressSeller, propertyId, BALANCE);

    if (amountOffered > balanceReallyAvailable) {
        PrintToLog("%s: rejected: sender %s has insufficient balance of property %d [%s < %s]\n", __func__,
                        addressSeller, propertyId, FormatDivisibleMP(balanceReallyAvailable), FormatDivisibleMP(amountOffered));
        return (DEX_ERROR_SELLOFFER -25);
      }

    // -------------------------------------------------------------------------

    if (amountOffered > 0) {
        assert(update_tally_map(addressSeller, propertyId, -amountOffered, BALANCE));
        assert(update_tally_map(addressSeller, propertyId, amountOffered, SELLOFFER_RESERVE));
        CMPOffer sellOffer(block, amountOffered, propertyId, amountDesired, minAcceptFee, paymentWindow, txid,0, 2);
        my_offers.insert(std::make_pair(key, sellOffer));

        rc = 0;
    }

    return rc;
}

int DEx_BuyOfferCreate(const std::string& addressMaker, uint32_t propertyId, int64_t amountOffered, int block, int64_t price, int64_t minAcceptFee, uint8_t paymentWindow, const uint256& txid, uint64_t* nAmended)
{
    // bool ready = false;
    int rc = DEX_ERROR_SELLOFFER;

    // sanity checks
    if (paymentWindow == 0)
    {
        if (msc_debug_dex) PrintToLog("DEX_ERROR_SELLOFFER\n");
        return (DEX_ERROR_SELLOFFER -101);
    }

    if (price == 0)
    {
        if (msc_debug_dex) PrintToLog("DEX_ERROR_SELLOFFER\n");
        return (DEX_ERROR_SELLOFFER -101);
    }

    if (DEx_getOffer(addressMaker, propertyId))
    {
        if (msc_debug_dex) PrintToLog("DEX_ERROR_SELLOFFER\n");
        return (DEX_ERROR_SELLOFFER -10); // offer already exists
    }

    const std::string key = STR_SELLOFFER_ADDR_PROP_COMBO(addressMaker, propertyId);
    if (msc_debug_dex) PrintToLog("%s(%s|%s), nValue=%d)\n", __func__, addressMaker, key, amountOffered);

    // ------------------------------------------------------------------------
    // On this part we need to put in reserve synth Litecoins.
    LOCK(cs_tally);

    arith_uint256 sumValues;
    uint32_t nextSPID = _my_sps->peekNextSPID();

    for (uint32_t propertyId = 1; propertyId < nextSPID; propertyId++)
    {
        CMPSPInfo::Entry sp;
        if (_my_sps->getSP(propertyId, sp))
        {
            if (msc_debug_dex) PrintToLog("Property Id: %d\n",propertyId);
            if (!sp.isContract())
	              continue;

            int64_t longs = 0;
            int64_t shorts = 0;

            const int64_t balance = getMPbalance(addressMaker, propertyId, CONTRACT_BALANCE);

            (balance > 0) ? longs = balance : shorts = balance;

            if (msc_debug_dex) PrintToLog("%s(): longs: %d, shorts: %d, notional: %d\n",__func__, longs, shorts, sp.notional_size);

            // price of one sLTC in ALLs
            int64_t pair = getPairMarketPrice("sLTC", "ALL");

            if (msc_debug_dex) PrintToLog("pair: %d\n", pair);

            const int64_t side = (longs >= 0 && shorts == 0) ? longs : (longs == 0 && shorts >= 0) ? shorts : 0;
            arith_uint256 sumNum = (ConvertTo256(side) * ConvertTo256(sp.notional_size)) * ConvertTo256(pair);
            sumValues += DivideAndRoundUp(sumNum, COIN);
	          if (sumValues > ConvertTo256(price))
            {
                // price = price of entire order   .
	              if (msc_debug_dex) PrintToLog("You can buy tokens now\n");
	              // ready = true;
	              break;
            }
        }
    }

    // NOTE: check conditions to buy tokens using litecoin
    // if (ready)
    if (true)
    {
        CMPOffer sellOffer(block, amountOffered, propertyId, price, minAcceptFee, paymentWindow, txid, 0, 1);
        my_offers.insert(std::make_pair(key, sellOffer));
        rc = 0;
    } else {
        if (msc_debug_dex) PrintToLog("You can't buy tokens, you need more position value\n");
        rc = -1;
    }

    return rc;
}

/**
 * Destroys a sell offer.
 *
 * The remaining amount reserved for the offer is returned to the available balance.
 *
 * @return 0 if the offer was destroyed
 */
int DEx_offerDestroy(const std::string& addressSeller, uint32_t propertyId)
{
    if (!DEx_offerExists(addressSeller, propertyId)) {
        return (DEX_ERROR_SELLOFFER -11); // offer does not exist
    }

    const int64_t amountReserved = getMPbalance(addressSeller, propertyId, SELLOFFER_RESERVE);

    // return the remaining reserved amount back to the seller
    if (amountReserved > 0) {
        assert(update_tally_map(addressSeller, propertyId, -amountReserved, SELLOFFER_RESERVE));
        assert(update_tally_map(addressSeller, propertyId, amountReserved, BALANCE));
    }

    // delete the offer
    const std::string key = STR_SELLOFFER_ADDR_PROP_COMBO(addressSeller, propertyId);
    OfferMap::iterator it = my_offers.find(key);
    if (it != my_offers.end()) my_offers.erase(it);

    if (msc_debug_dex) PrintToLog("%s(%s|%s)\n", __func__, addressSeller, key);

    return 0;
}

/**
 * Updates a sell offer.
 *
 * The old offer is destroyed, and replaced by the updated offer.
 *
 * @return 0 if everything is OK
 */
int DEx_offerUpdate(const std::string& addressSeller, uint32_t propertyId, int64_t amountOffered, int block, int64_t amountDesired, int64_t minAcceptFee, uint8_t paymentWindow, const uint256& txid, uint64_t* nAmended)
{
    if (msc_debug_dex) PrintToLog("%s(%s, %d)\n", __func__, addressSeller, propertyId);

    if (!DEx_offerExists(addressSeller, propertyId)) {
        return (DEX_ERROR_SELLOFFER -12); // offer does not exist
    }

    int rc = DEx_offerDestroy(addressSeller, propertyId);

    if (rc == 0) {
        // if the old offer was destroyed successfully, try to create a new one
        rc = DEx_offerCreate(addressSeller, propertyId, amountOffered, block, amountDesired, minAcceptFee, paymentWindow, txid, nAmended);
    }

    return rc;
}

/**
 * Creates a new accept order.
 *
 * @return 0 if everything is OK
 */
int DEx_acceptCreate(const std::string& addressTaker, const std::string& addressMaker, uint32_t propertyId, int64_t amountAccepted, int block, int64_t feePaid, uint64_t* nAmended)
{
    int rc = DEX_ERROR_ACCEPT -10;
    const std::string keySellOffer = STR_SELLOFFER_ADDR_PROP_COMBO(addressMaker, propertyId);
    const std::string keyAcceptOrder = STR_ACCEPT_ADDR_PROP_ADDR_COMBO(addressMaker, addressTaker, propertyId);

    OfferMap::const_iterator my_it = my_offers.find(keySellOffer);

    if (my_it == my_offers.end()) {
        PrintToLog("%s(): rejected: no matching sell offer for accept order found\n", __func__);
        return (DEX_ERROR_ACCEPT -15);
    }

    const CMPOffer& offer = my_it->second;

    if (msc_debug_dex) PrintToLog("%s(): found a matching sell offer [seller: %s, buyer: %s, property: %d)\n", __func__,
                    addressMaker, addressTaker, propertyId);

    // the older accept is the valid one: do not accept any new ones!
    if (DEx_acceptExists(addressMaker, propertyId, addressTaker)) {
        PrintToLog("%s(): rejected: an accept order from this same maker for this same offer already exists\n", __func__);
        return DEX_ERROR_ACCEPT -205;
    }

    // ensure the correct LTC fee was paid in this acceptance message
    if (msc_debug_dex) PrintToLog("%s() Checking: feePaid: %d, offer.getMinFee(): %d\n",__func__, feePaid, offer.getMinFee());

    if (feePaid < offer.getMinFee()) {
        PrintToLog("%s(): rejected: transaction fee too small [%d < %d]\n", __func__, feePaid, offer.getMinFee());
        return DEX_ERROR_ACCEPT -105;
    }

    if (offer.getOption() == 1)
    {   // if market maker is buying tokens

        if (msc_debug_dex)
        {
            PrintToLog("getProperty: %d\n",offer.getProperty());
            PrintToLog("getOfferAmountOriginal: %d\n",offer.getOfferAmountOriginal());
            PrintToLog("getLTCDesiredOriginal: %d\n",offer.getLTCDesiredOriginal());
        }

        if (amountAccepted > offer.getOfferAmountOriginal()) {
            amountAccepted = offer.getOfferAmountOriginal();
        }

        int64_t amountInBalance = getMPbalance(addressTaker, propertyId, BALANCE);

        if (msc_debug_dex) PrintToLog("%s(): amountInBalance: %d, amountAccepted: %d\n",__func__, amountInBalance, amountAccepted);

        if (amountInBalance == 0){
            PrintToLog("%s(): rejected: amount of token in seller  is zero \n",__func__);
            return rc;
        }

        if (amountInBalance >= amountAccepted) {
            assert(update_tally_map(addressTaker, propertyId, -amountAccepted, BALANCE));
            assert(update_tally_map(addressTaker, propertyId, amountAccepted, ACCEPT_RESERVE));
        } else {
            if (msc_debug_dex) PrintToLog("amountInBalance < amountAccepted ???\n");
        }

        CMPAccept acceptOffer(amountAccepted, block, offer.getBlockTimeLimit(), offer.getProperty(), offer.getOfferAmountOriginal(), offer.getLTCDesiredOriginal(), offer.getHash());
        my_accepts.insert(std::make_pair(keyAcceptOrder, acceptOffer));

        return 0;
    }

    int64_t amountReserved = 0;
    int64_t amountRemainingForSale = getMPbalance(addressMaker, propertyId, SELLOFFER_RESERVE);

    // ensure the buyer only can reserve the amount that is still available
    if (amountRemainingForSale >= amountAccepted) {
        amountReserved = amountAccepted;
    } else {
        amountReserved = amountRemainingForSale;
        if (msc_debug_dex) PrintToLog("%s(): buyer wants to reserve %d tokens, but only %d tokens are available\n", __func__, amountAccepted, amountRemainingForSale);
    }

    if (amountReserved > 0) {
        assert(update_tally_map(addressMaker, propertyId, -amountReserved, SELLOFFER_RESERVE));
        assert(update_tally_map(addressMaker, propertyId, amountReserved, ACCEPT_RESERVE));

        CMPAccept acceptOffer(amountReserved, block, offer.getBlockTimeLimit(), offer.getProperty(), offer.getOfferAmountOriginal(), offer.getLTCDesiredOriginal(), offer.getHash());
        my_accepts.insert(std::make_pair(keyAcceptOrder, acceptOffer));

        rc = 0;
    }

    if (nAmended) *nAmended = amountReserved;

    return rc;
}

/**
 * Finalizes an accepted offer.
 *
 * This function is called by handler_block() for each accepted offer that has
 * expired. This function is also called when the purchase has been completed,
 * and the buyer bought everything that was reserved.
 *
 * @return 0 if everything is OK
 */
int DEx_acceptDestroy(const std::string& addressBuyer, const std::string& addressSeller, uint32_t propertyid, bool fForceErase)
{
    int rc = DEX_ERROR_ACCEPT -20;
    CMPOffer* p_offer = DEx_getOffer(addressSeller, propertyid);
    CMPAccept* p_accept = DEx_getAccept(addressSeller, propertyid, addressBuyer);
    bool fReturnToMoney;

    if (!p_accept) return rc; // sanity check

    // if the offer is gone, ACCEPT_RESERVE should go back to seller's BALANCE
    // otherwise move the previously accepted amount back to SELLOFFER_RESERVE
    if (!p_offer) fReturnToMoney = true;

    PrintToLog("%s(): finalize trade [offer=%s, accept=%s]\n", __func__, p_offer->getHash().GetHex(), p_accept->getHash().GetHex());

    // offer exists, determine whether it's the original offer or some random new one
    fReturnToMoney = (p_offer->getHash() == p_accept->getHash()) ? false : true;

    const int64_t amountRemaining = p_accept->getAcceptAmountRemaining();

    if (amountRemaining > 0) {
        TallyType typ = (fReturnToMoney) ? BALANCE : SELLOFFER_RESERVE;
        assert(update_tally_map(addressSeller, propertyid, -amountRemaining, ACCEPT_RESERVE));
        assert(update_tally_map(addressSeller, propertyid, amountRemaining, typ));

    }

    // can only erase when is NOT called from an iterator loop
    if (fForceErase) {
        std::string key = STR_ACCEPT_ADDR_PROP_ADDR_COMBO(addressSeller, addressBuyer, propertyid);
        AcceptMap::iterator it = my_accepts.find(key);

        if (my_accepts.end() != it) {
            my_accepts.erase(it);
        }
    }

    return 0;
}

/**
 * Determines the purchased amount of tokens.
 *
 * The amount is rounded up, which may be in favor of the buyer, to avoid small
 * leftovers of 1 willet. This is not exploitable due to transaction fees.
 *
 * The desired amount must not be zero, and the purchased amount should be checked
 * against the actual amount that is still left for sale.
 *
 * @return The amount of tokens purchased
 */
int64_t calculateDExPurchase(const int64_t amountOffered, const int64_t amountDesired, const int64_t amountPaid)
{
    // conversion
    arith_uint256 amountOffered256 = ConvertTo256(amountOffered);
    arith_uint256 amountDesired256 = ConvertTo256(amountDesired);
    arith_uint256 amountPaid256 = ConvertTo256(amountPaid);

    // actual calculation; round up
    arith_uint256 amountPurchased256 = DivideAndRoundUp((amountPaid256 * amountOffered256), amountDesired256);

    // convert back to int64_t
    return ConvertTo64(amountPurchased256);
}

/**
 * Handles incoming LTC payment for the offer in tradelayer.cpp
 */
int DEx_payment(const uint256& txid, unsigned int vout, const std::string& addressSeller, const std::string& addressBuyer, int64_t amountPaid, int block, uint64_t* nAmended)
{
    if (msc_debug_dex) PrintToLog("%s(%s, %s)\n", __func__, addressSeller, addressBuyer);
    int rc = DEX_ERROR_PAYMENT;
    uint32_t propertyId;

    CMPAccept* p_accept = nullptr;

    // logic here: we look only into main properties if there's some match
    for (propertyId = 1; propertyId < _my_sps->peekNextSPID(); propertyId++)
    {
        CMPSPInfo::Entry sp;
        if (msc_debug_dex) PrintToLog("propertyId: %d\n",propertyId);
        if (!_my_sps->getSP(propertyId, sp)) continue;

        // seller market maker?
        p_accept = DEx_getAccept(addressSeller, propertyId, addressBuyer);

        if (p_accept)
        {
            if (msc_debug_dex) PrintToLog("Found seller market maker!\n");
            break;
        }

        // buyer market maker?
        if (IsFeatureActivated(FEATURE_DEX_BUY, block))
        {
            p_accept = DEx_getAccept(addressBuyer, propertyId, addressSeller);

            if (p_accept)
            {
                if (msc_debug_dex) PrintToLog("Found buyer market maker!\n");
                break;
            }

        }
    }

    if (!p_accept && msc_debug_dex)
    {
       PrintToLog("%s(): ERROR: there must be an active accept order for this payment, seller: %s, buyer: %s\n",__func__, addressSeller, addressBuyer);
       return (DEX_ERROR_PAYMENT -1);
    }

    // -------------------------------------------------------------------------
    const int64_t amountDesired = p_accept->getLTCDesiredOriginal();
    const int64_t amountOffered = p_accept->getOfferAmountOriginal();

    if (msc_debug_dex) PrintToLog("%s(): amountDesired : %d, amountOffered : %d\n",__func__, amountDesired, amountOffered);

    // divide by 0 protection
    if (0 == amountDesired && msc_debug_dex)
    {
        PrintToLog("%s(): ERROR: desired amount of accept order is zero", __func__);
        return (DEX_ERROR_PAYMENT -2);
    }

   /**
    * Plain integer math is used to determine
    * the purchased amount. The purchased amount is rounded up, which may be
    * in favor of the buyer, to avoid small leftovers of 1 willet.
    *
    * This is not exploitable due to transaction fees.
    */
     int64_t amountPurchased = mastercore::calculateDExPurchase(amountOffered, amountDesired, amountPaid);

    // -------------------------------------------------------------------------

    // adding LTC volume added by this property
    PrintToLog("%s(): block: %d, propertyId: %d. amountPaid (LTC): %d\n",__func__, block, propertyId, amountPaid);
    MapLTCVolume[block][propertyId] += amountPaid;

    const arith_uint256 amountDesired256  = ConvertTo256(amountDesired);
    const arith_uint256 amountOffered256 = ConvertTo256(amountOffered);
    const arith_uint256 unitPrice256 = (ConvertTo256(COIN) * amountDesired256) / amountOffered256;

    const int64_t unitPrice = (isPropertyDivisible(propertyId)) ? ConvertTo64(unitPrice256) : ConvertTo64(unitPrice256) / COIN;

    // adding last price
    lastPrice[propertyId] = unitPrice;

    // adding numerator of vwap
    tokenvwap[propertyId][block].push_back(std::make_pair(unitPrice, amountPurchased));

    // saving DEx token volume
    MapTokenVolume[block][propertyId] += amountPurchased;

    // adding Last token/ ltc price
    rational_t market_pricetokens_LTC(amountPurchased, amountPaid);
    int64_t market_p_tokens_LTC = mastercore::RationalToInt64(market_pricetokens_LTC);
    market_priceMap[LTC][propertyId] = market_p_tokens_LTC;

    if(msc_debug_dex) PrintToLog("%s(): amountPaid for propertyId : %d,  inside MapLTCVolume: %d, market_p_tokens_LTC : %d\n", __func__, propertyId, amountPaid, market_p_tokens_LTC);

    // -------------------------------------------------------------------------
    if (msc_debug_dex) PrintToLog("amountPurchased: %d\n",amountPurchased);
    const int64_t amountRemaining = p_accept->getAcceptAmountRemaining(); // actual amount desired, in the Accept

    if (msc_debug_dex) PrintToLog(
    "%s(): LTC desired: %s, offered amount: %s, amount to purchase: %s, amount remaining: %s\n", __func__,
    FormatDivisibleMP(amountDesired), FormatDivisibleMP(amountOffered),
    FormatDivisibleMP(amountPurchased), FormatDivisibleMP(amountRemaining));

    // if units_purchased is greater than what's in the Accept, the buyer gets only what's in the Accept
    if (amountRemaining < amountPurchased)
    {
        amountPurchased = amountRemaining;
        if (nAmended) *nAmended = amountPurchased;
    }

    if (msc_debug_dex) PrintToLog("amountPurchased: %d",amountPurchased);

    if (amountPurchased > 0)
    {
        if(msc_debug_dex) PrintToLog("%s(): seller %s offered %s %s for %s LTC\n", __func__,
	      addressSeller, FormatDivisibleMP(amountOffered), strMPProperty(propertyId), FormatDivisibleMP(amountDesired));

        if(msc_debug_dex) PrintToLog("%s: buyer %s pays %s LTC to purchase %s %s\n", __func__,
	      addressBuyer, FormatDivisibleMP(amountPaid), FormatDivisibleMP(amountPurchased), strMPProperty(propertyId));

        assert(update_tally_map(addressSeller, propertyId, -amountPurchased, ACCEPT_RESERVE));
        assert(update_tally_map(addressBuyer, propertyId, amountPurchased, BALANCE));

        if(msc_debug_dex) PrintToLog("AmountPurchased : %d\n",amountPurchased);

        bool valid = true;
        p_txlistdb->recordPaymentTX(txid, valid, block, vout, propertyId, amountPurchased, amountPaid, addressBuyer, addressSeller);

        rc = 0;
        if(msc_debug_dex) PrintToLog("#######################################################\n");
    }

    // reduce the amount of units still desired by the buyer and if 0 destroy the Accept order
    if (p_accept->reduceAcceptAmountRemaining_andIsZero(amountPurchased))
    {
        if(msc_debug_dex) PrintToLog("p_accept->reduceAcceptAmountRemaining_andIsZero true\n");

        const int64_t reserveSell = getMPbalance(addressSeller, propertyId, SELLOFFER_RESERVE);
        const int64_t reserveAccept = getMPbalance(addressSeller, propertyId, ACCEPT_RESERVE);

        if(msc_debug_dex) PrintToLog("reserveSell: %d, reserveAccept: %d\n",reserveSell, reserveAccept);

        DEx_acceptDestroy(addressBuyer, addressSeller, propertyId, true);

        // delete the Offer object if there is nothing in its Reserves -- everything got puchased and paid for
        if (0 == reserveSell && 0 == reserveAccept)
        {
            if(msc_debug_dex) PrintToLog("0 == reserveSell && 0 == reserveAccept true\n");
            DEx_offerDestroy(addressSeller, propertyId);
        }

        rc = 0;
    }

    return rc;
}

unsigned int eraseExpiredAccepts(int blockNow)
{
    unsigned int how_many_erased = 0;
    AcceptMap::iterator it = my_accepts.begin();

    while (my_accepts.end() != it) {
        const CMPAccept& acceptOrder = it->second;

        int blocksSinceAccept = blockNow - acceptOrder.getAcceptBlock();
        int blocksPaymentWindow = static_cast<int>(acceptOrder.getBlockTimeLimit());

        if (blocksSinceAccept >= blocksPaymentWindow) {
            PrintToLog("%s(): erasing at block: %d, order confirmed at block: %d, payment window: %d\n",
                    __func__, blockNow, acceptOrder.getAcceptBlock(), acceptOrder.getBlockTimeLimit());

            // extract the seller, buyer and property from the key
            std::vector<std::string> vstr(3);
            boost::split(vstr, it->first, boost::is_any_of("-+"), boost::token_compress_on);
            const std::string& addressSeller = vstr[0];
            uint32_t propertyId = atoi(vstr[1]);
            const std::string& addressBuyer = vstr[2];

            DEx_acceptDestroy(addressBuyer, addressSeller, propertyId);

            my_accepts.erase(it++);

            ++how_many_erased;

        } else it++;
    }

    return how_many_erased;
}

} // namespace mastercore


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

#include <boost/simd/arithmetic/functions/max.hpp>
#include <boost/simd/include/functions/simd/is_unord.hpp>
#include <boost/simd/include/functions/simd/if_else.hpp>
#include <boost/simd/include/functions/simd/slice.hpp>
#include <boost/simd/include/functions/simd/combine.hpp>
#include <boost/simd/sdk/simd/meta/retarget.hpp>
#include <boost/simd/sdk/config/compiler.hpp>
#include <boost/simd/sdk/config.hpp>
#include <boost/dispatch/attributes.hpp>

namespace boost { namespace simd { namespace ext
{

  BOOST_DISPATCH_IMPLEMENT          ( max_
                                    , boost::simd::tag::avx_
                                    , (A0)
                                    , ((simd_<double_<A0>,boost::simd::tag::avx_>))
                                      ((simd_<double_<A0>,boost::simd::tag::avx_>))
                                     )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
#if !defined(BOOST_SIMD_NO_NANS) && defined(BOOST_SIMD_COMPILER_GCC)
      // workaround for GCC bug #57057
      return if_else(is_unord(a0, a1), a1, A0(_mm256_max_pd(a0, a1)));
#else
      return _mm256_max_pd(a0,a1);
#endif
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( max_
                                    , boost::simd::tag::avx_
                                    , (A0)
                                    , ((simd_<single_<A0>,boost::simd::tag::avx_>))
                                      ((simd_<single_<A0>,boost::simd::tag::avx_>))
                                    )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
#if !defined(BOOST_SIMD_NO_NANS) && defined(BOOST_SIMD_COMPILER_GCC)
      // workaround for GCC bug #57057
      return if_else(is_unord(a0, a1), a1, A0(_mm256_max_ps(a0, a1)));
#else
      return _mm256_max_ps(a0,a1);
#endif
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( max_
                                    , boost::simd::tag::avx_
                                    , (A0)
                                    , ((simd_<integer_<A0>,boost::simd::tag::avx_>))
                                      ((simd_<integer_<A0>,boost::simd::tag::avx_>))
                                    )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      typename meta::retarget<A0, tag::sse_>::type a00, a01, a10, a11;
      slice(a0, a00, a01);
      slice(a1, a10, a11);
      return combine(max(a00, a10), max(a01, a11));
    }
  };
} } }
#endif
#endif


/*! *****************************************************************************
 *   \file libSampler.cpp
 *   \author moennen
 *   \brief
 *   \date 2018-04-06
 *   *****************************************************************************/

#include "sampleBuffDataset/libBuffDatasetSampler.h"

#include "utils/cv_utils.h"
#include "utils/imgFileLst.h"
#include "utils/Hop.h"

#include <boost/filesystem.hpp>
#include <boost/algorithm/string.hpp>

#include <glm/glm.hpp>

#include <future>

#include <random>
#include <fstream>
#include <string>
#include <vector>
#include <array>

#include <ctime>
#include <iostream>
#include <map>

using namespace std;
using namespace cv;
using namespace glm;

#define TRACE cout << __PRETTY_FUNCTION__ << "@" << __LINE__ << endl;

namespace
{
struct Sampler final
{
   mt19937 _rng;
   normal_distribution<> _rnGen;
   uniform_int_distribution<> _dataGen;
   uniform_real_distribution<> _transGen;
   uniform_real_distribution<> _ldAsBlendGen;

   ImgNFileLst _data;
   const ivec3 _sampleSz;
   const bool _toLinear;
   const float _downsample;
   const float _ldAsBlendFreq;
   const float _minPrevNextSqDiff;
   const float _maxPrevNextSqDiff;

   const unsigned _fullBuffSz;
   future<bool> _asyncSample;
   vector<float> _asyncBuff;

   enum
   {
      DefaultMode = 0,
      PrevIsClosestMode = 1,

      nInBuffers = 5,
      nOutBuffers = 5,
      nOutPlanes = 15  // 5xRGB
   };
   const int _mode;

   Sampler(
       const char* dataSetPath,
       const char* dataPath,
       const float downsampleFactor,
       const float ldAsBlendFreq,
       const float minPrevNextSqDiff,
       const float maxPrevNextSqDiff,
       const int mode,
       const ivec3 sampleSz,
       const bool toLinear,
       const bool doAsync,
       const int seed )
       : _rng( seed ),
         _transGen( 0.0, 1.0 ),
         _ldAsBlendGen( 0.0, 1.0 ),
         _data( 4 ),  // 3 files + 1 alpha
         _sampleSz( sampleSz ),
         _toLinear( toLinear ),
         _downsample( downsampleFactor ),
         _ldAsBlendFreq( ldAsBlendFreq ),
         _minPrevNextSqDiff( minPrevNextSqDiff ),
         _maxPrevNextSqDiff( maxPrevNextSqDiff ),
         _fullBuffSz( _sampleSz.y * _sampleSz.z * nOutPlanes ),
         _mode( mode )
   {
      HOP_PROF_FUNC();

      _data.open( dataSetPath, dataPath );
      if ( _data.size() )
      {
         std::cout << "Read dataset " << dataSetPath << " (" << _data.size() << ") "
                   << ldAsBlendFreq << " " << downsampleFactor << std::endl;
      }
      _dataGen = uniform_int_distribution<>( 0, _data.size() - 1 );

      if ( _data.size() && doAsync )
      {
         _asyncBuff.resize( sampleSz.x * _fullBuffSz );
         _asyncSample = async( launch::async, [&]() { return sample_internal( &_asyncBuff[0] ); } );
      }
   }

   bool sample( float* buff )
   {
      if ( _asyncBuff.empty() )
         return sample_internal( buff );
      else
      {
         bool success = _asyncSample.get();
         if ( success )
         {
#pragma omp parallel for
            for ( int b = 0; b < _sampleSz.x; ++b )
            {
               const size_t off = b * _fullBuffSz;
               memcpy( buff + off, &_asyncBuff[off], sizeof( float ) * _fullBuffSz );
            }
         }
         _asyncSample = async( launch::async, [&]() { return sample_internal( &_asyncBuff[0] ); } );
         return success;
      }
   }

   size_t nSamples() const { return _data.size(); }
   ivec3 sampleSizes() const { return _sampleSz; }

  private:
   bool sample_internal( float* buff )
   {
      HOP_PROF_FUNC();

      const unsigned buffSz = _sampleSz.z * _sampleSz.y * 3;
      const unsigned batchBuffSz = buffSz * _sampleSz.x;

      float* currBuffGTHD = buff;
      float* currBuffGTLD = buff + batchBuffSz;
      float* currBuffBlend = buff + 2 * batchBuffSz;
      float* currBuffPrev = buff + 3 * batchBuffSz;
      float* currBuffNext = buff + 4 * batchBuffSz;

      std::vector<char> sampled( _sampleSz.x, 0 );
      std::vector<size_t> v_si( _sampleSz.x );
      do
      {
         for ( auto& si : v_si ) si = _dataGen( _rng );

#pragma omp parallel for
         for ( size_t s = 0; s < _sampleSz.x; ++s )
         {
            if ( sampled[s] ) continue;

            const size_t si = v_si[s];

            float alpha = stof( _data( si, 3 ) );
            const bool swapPrevNext = ( alpha > 0.5 ) && ( _mode == PrevIsClosestMode );
            alpha = swapPrevNext ? 1.0 - alpha : alpha;

            const string pathA = _data.filePath( si, 0 );
            const string pathB = _data.filePath( si, 1 );
            const string pathC = _data.filePath( si, 2 );

            Mat prevImg = cv_utils::imread32FC3( swapPrevNext ? pathC : pathA, _toLinear );
            Mat currImg = cv_utils::imread32FC3( pathB, _toLinear );
            Mat nextImg = cv_utils::imread32FC3( swapPrevNext ? pathA : pathC, _toLinear );

            ivec2 imgSz( currImg.cols, currImg.rows );

            // ignore too small samples
            if ( ( imgSz.x < _sampleSz.y ) || ( imgSz.y < _sampleSz.z ) ) continue;

            // random rescale
            const float ds =
                mix( 1.0f,
                     std::max( (float)_sampleSz.z / imgSz.y, (float)_sampleSz.y / imgSz.x ),
                     _transGen( _rng ) );
            resize( prevImg, prevImg, Size(), ds, ds, INTER_AREA );
            resize( currImg, currImg, Size(), ds, ds, INTER_AREA );
            resize( nextImg, nextImg, Size(), ds, ds, INTER_AREA );
            imgSz = ivec2( currImg.cols, currImg.rows );

            // random translate
            const ivec2 trans(
                std::floor( _transGen( _rng ) * ( imgSz.x - _sampleSz.y ) ),
                std::floor( _transGen( _rng ) * ( imgSz.y - _sampleSz.z ) ) );

            // crop
            prevImg = prevImg( Rect( trans.x, trans.y, _sampleSz.y, _sampleSz.z ) );
            currImg = currImg( Rect( trans.x, trans.y, _sampleSz.y, _sampleSz.z ) );
            nextImg = nextImg( Rect( trans.x, trans.y, _sampleSz.y, _sampleSz.z ) );

            // preprocess
            const float contrast = ( 1.0f + 0.11f * _rnGen( _rng ) );
            const float brightness = 0.11f * _rnGen( _rng );
            cv_utils::adjustContrastBrightness<vec3>( prevImg, contrast, brightness );
            cv_utils::adjustContrastBrightness<vec3>( currImg, contrast, brightness );
            cv_utils::adjustContrastBrightness<vec3>( nextImg, contrast, brightness );
            // random small blur to remove artifacts
            const float blur = 1.5 * _transGen( _rng );
            GaussianBlur( prevImg, prevImg, Size( 5, 5 ), blur );
            GaussianBlur( currImg, currImg, Size( 5, 5 ), blur );
            GaussianBlur( nextImg, nextImg, Size( 5, 5 ), blur );

            // filter samples on differences :
            /*const double sqDiff = norm( prevImg, nextImg ) / buffSz;
            if ( ( sqDiff > _maxPrevNextSqDiff ) || ( sqDiff < _minPrevNextSqDiff ) )
            {
               --s;
               continue;
            }*/

            // For initial estimate we need to use the blend image as the LD
            // --> sample
            const bool blendInLD( _ldAsBlendGen( _rng ) < _ldAsBlendFreq );

            // currBuffGTHD / currBuffGTLD
            {
               Mat sple( _sampleSz.z, _sampleSz.y, CV_32FC3, currBuffGTHD + s * buffSz );
               cvtColor( currImg, sple, COLOR_BGR2RGB );
               // downsample / upsample
               if ( !blendInLD )
               {
                  Mat tmp;
                  resize( sple, tmp, Size(), _downsample, _downsample, CV_INTER_AREA );
                  sple = Mat( _sampleSz.z, _sampleSz.y, CV_32FC3, currBuffGTLD + s * buffSz );
                  resize( tmp, sple, Size( _sampleSz.y, _sampleSz.z ), 0, 0, CV_INTER_LINEAR );
               }
            }

            // prev / next
            {
               Mat sple( _sampleSz.z, _sampleSz.y, CV_32FC3, currBuffPrev + s * buffSz );
               cvtColor( prevImg, sple, COLOR_BGR2RGB );
               sple = Mat( _sampleSz.z, _sampleSz.y, CV_32FC3, currBuffNext + s * buffSz );
               cvtColor( nextImg, sple, COLOR_BGR2RGB );
            }

            // blend
            {
               Mat tmp = alpha * prevImg + ( 1.0 - alpha ) * nextImg;
               Mat sple( _sampleSz.z, _sampleSz.y, CV_32FC3, currBuffBlend + s * buffSz );
               cvtColor( tmp, sple, COLOR_BGR2RGB );
               // downsample / upsample
               if ( blendInLD )
               {
                  Mat tmp;
                  resize( sple, tmp, Size(), _downsample, _downsample, CV_INTER_AREA );
                  sple = Mat( _sampleSz.z, _sampleSz.y, CV_32FC3, currBuffGTLD + s * buffSz );
                  resize( tmp, sple, Size( _sampleSz.y, _sampleSz.z ), 0, 0, CV_INTER_LINEAR );
               }
            }

            sampled[s] = 1;
         }
      } while ( accumulate( sampled.begin(), sampled.end(), 0 ) != _sampleSz.x );

     return true;
   }
};

array<unique_ptr<Sampler>, 33> g_samplers;
};

extern "C" int getNbBuffers( const int /*sidx*/ ) { return Sampler::nOutBuffers; }

extern "C" int getBuffersDim( const int sidx, float* dims )
{
   HOP_PROF_FUNC();

   if ( !g_samplers[sidx].get() ) return ERROR_UNINIT;

   const ivec3 sz = g_samplers[sidx]->sampleSizes();
   float* d = dims;
   for ( size_t i = 0; i < Sampler::nOutBuffers; ++i )
   {
      d[0] = sz.y;
      d[1] = sz.z;
      d[2] = 3;
      d += 3;
   }

   return SUCCESS;
}

extern "C" int initBuffersDataSampler(
    const int sidx,
    const char* datasetPath,
    const char* dataPath,
    const int nParams,
    const float* params,
    const int seed )
{
   HOP_PROF_FUNC();

   // check input
   if ( ( nParams < 8 ) || ( sidx > g_samplers.size() ) ) return ERROR_BAD_ARGS;

   // parse params
   const ivec3 sz( params[0], params[1], params[2] );
   const float downsampleFactor = params[3];
   const float ldAsBlendFreq = params[4];
   const float minPrevNextSqDiff = params[5];
   const float maxPrevNextSqDiff = params[6];
   const int mode = static_cast<int>( params[7] );
   const bool toLinear( nParams > 8 ? params[8] > 0.0 : false );
   const bool doAsync( nParams > 9 ? params[9] > 0.0 : true );
   g_samplers[sidx].reset( new Sampler(
       datasetPath,
       dataPath,
       downsampleFactor,
       ldAsBlendFreq,
       minPrevNextSqDiff,
       maxPrevNextSqDiff,
       mode,
       sz,
       toLinear,
       doAsync,
       seed ) );

   return g_samplers[sidx]->nSamples() ? SUCCESS : ERROR_BAD_DB;
}

extern "C" int getBuffersDataSample( const int sidx, float* buff )
{
   HOP_PROF_FUNC();

   if ( !g_samplers[sidx].get() ) return ERROR_UNINIT;

   // sample
   if ( !g_samplers[sidx]->sample( buff ) ) return ERROR_GENERIC;

   return SUCCESS;
}


#include "ros/ros.h"
#include "std_msgs/String.h"
#include"Eigen/Dense"
#include <vector>
#include <iostream>
#include <QString>
#include <QList>
#include "Robot.h"
//#include"TaskSpace.h"
#include"taskspaceoffline.h"
#include <qmath.h>
#include <cstring>
#include<qdebug.h>
#include <Eigen/Geometry>
#include <cstdlib>
//#include <link.h>
#include "Eigen/eiquadprog.h"
#include "Eigen/Core"
#include "Eigen/Cholesky"
#include "Eigen/LU"
#include<std_msgs/Int32MultiArray.h>
#include<std_msgs/Float32MultiArray.h>
#include<math.h>
#include<sensor_msgs/Imu.h>
#include<std_msgs/Float64.h>
#include "pidcontroller.h"
#include<rosgraph_msgs/Clock.h>
#include <std_msgs/Empty.h>
#include "std_srvs/Empty.h"
#include "qcgenerator.h"
#include <qthread.h>
#include "/home/cast/humanoid/surena4/devel/include/xsens_msgs/orientationEstimate.h"

#include<gazebo_msgs/ApplyBodyWrenchRequest.h>
#include <gazebo_msgs/GetLinkState.h>
#include<gazebo_msgs/LinkStates.h>
#include <nav_msgs/Odometry.h>
#include "std_srvs/Empty.h"
#include "qcgenerator.h"

using namespace  std;
using namespace  Eigen;

//sensor imu----------------------------------------------sensor imu
//sensor imu-----------------------------------------------sensor imu
//void chatterCallback(const sensor_msgs::Imu::ConstPtr& msg)
//{
//  ROS_INFO("I heard: [%f]", msg->orientation.x);
//}
//sensor imu----------------------------------------------sensor imu
//sensor imu----------------------------------------------sensor imu

//ros::NodeHandle nh;

ros::Publisher pub1  ;
ros::Publisher pub2  ;
ros::Publisher pub3  ;
ros::Publisher pub4  ;
ros::Publisher pub5  ;
ros::Publisher pub6  ;
ros::Publisher pub7  ;
ros::Publisher pub8  ;
ros::Publisher pub9  ;
ros::Publisher pub10 ;
ros::Publisher pub11 ;
ros::Publisher pub12 ;
ros::Publisher pub13 ;
ros::Publisher pub14 ;
ros::Publisher pub15 ;
ros::Publisher pub16 ;
ros::Publisher pub17 ;
ros::Publisher pub18 ;
ros::Publisher pub19 ;
ros::Publisher pub20 ;
ros::Publisher pub21 ;
ros::Publisher pub22 ;
ros::Publisher pub23 ;
ros::Publisher pub24 ;
ros::Publisher pub25 ;
ros::Publisher pub26 ;
ros::Publisher pub27 ;
ros::Publisher pub28 ;

double teta_L=0;
double phi_L=0;

double teta_R=0;
double phi_R=0;

double teta_center=0;
double phi_center=0;

PIDController teta_pid_L;
PIDController phi_pid_L;
PIDController teta_pid_R;
PIDController phi_pid_R;

PIDController teta_pid_center;
PIDController phi_pid_center;

double p_teta,i_teta,d_teta,p_phi,i_phi,d_phi,dt,rate;
double p_teta_center,i_teta_center,d_teta_center,p_phi_center,i_phi_center,d_phi_center;
double teta_motor_L=0;
double teta_motor_R=0;
double phi_motor_L=0;
double phi_motor_R=0;
double teta_motor_center=0;
double phi_motor_center=0;
double timestep=.01;
double time_=0;

double quaternion2ankle_pitch(double q0,double q1,double q2,double q3){
    double R11,R32,R33,R31,theta;
    //    R11=q0*q0+q1*q1-q2*q2-q3*q3;
    R31=2*(q1*q3-q0*q2);
    //    theta=atan2(-R31,R11);
    R32=2*(q0*q1+q2*q3);
    R33=q0*q0-q1*q1-q2*q2+q3*q3;
    theta=atan2(-R31,sqrt(R32*R32+R33*R33));

    return theta;
}
double quaternion2ankle_roll(double q0,double q1,double q2,double q3){
        double R23,R22,phi,R33,R32;
    //R23=2*(q2*q3-q0*q1);
    //R22=q0*q0-q1*q1+q2*q2-q3*q3;
    //phi=atan2(-R23,R22);
    R32=2*(q0*q1+q2*q3);
    R33=q0*q0-q1*q1-q2*q2+q3*q3;
    phi=atan2(R32,R33);
    return phi;
}

geometry_msgs::Pose GetLinkPosition(QString linkName ,const gazebo_msgs::LinkStates::ConstPtr linkStates)
{
    geometry_msgs::Pose result;
    const std::vector<std::string> &names = linkStates->name;
    const std::vector<geometry_msgs::Pose> positions = linkStates->pose;

    for(int i=0;i<names.size();i++)
    {

        if(QString::fromStdString( names[i]).contains(linkName))
        {

            return  positions[i];

        }
    }

    return result;
}

void chatterCallback(const gazebo_msgs::LinkStates::ConstPtr& msg)
{
    const std::vector<std::string> &names = msg->name;
    //const std::vector<geometry_msgs::Pose> psitions = msg->pose;
    //  double distance=0;

    geometry_msgs::Pose parentPosition_L=GetLinkPosition("robot::LLeg_Foot_Link",msg);
//    qDebug()<<"we are here:"<< parentPosition.orientation.x<<parentPosition.orientation.y<<parentPosition.orientation.z<<parentPosition.orientation.w;
   teta_L= quaternion2ankle_pitch( parentPosition_L.orientation.w,parentPosition_L.orientation.x,parentPosition_L.orientation.y,parentPosition_L.orientation.z);
   phi_L=quaternion2ankle_roll( parentPosition_L.orientation.w,parentPosition_L.orientation.x,parentPosition_L.orientation.y,parentPosition_L.orientation.z);

   geometry_msgs::Pose parentPosition_R=GetLinkPosition("robot::LLeg_Foot_Link",msg);
//    qDebug()<<"we are here:"<< parentPosition_R.orientation.x<<parentPosition_R.orientation.y<<parentPosition_R.orientation.z<<parentPosition_R.orientation.w;
  teta_R= quaternion2ankle_pitch( parentPosition_R.orientation.w,parentPosition_R.orientation.x,parentPosition_R.orientation.y,parentPosition_R.orientation.z);
  phi_R=quaternion2ankle_roll( parentPosition_R.orientation.w,parentPosition_R.orientation.x,parentPosition_R.orientation.y,parentPosition_R.orientation.z);

  geometry_msgs::Pose parentPosition_center=GetLinkPosition("robot::pelvis",msg);
//    qDebug()<<"we are here:"<< parentPosition_center.orientation.x<<parentPosition_center.orientation.y<<parentPosition_center.orientation.z<<parentPosition_center.orientation.w;
 teta_center= quaternion2ankle_pitch( parentPosition_center.orientation.w,parentPosition_center.orientation.x,parentPosition_center.orientation.y,parentPosition_center.orientation.z);
 phi_center=quaternion2ankle_roll( parentPosition_center.orientation.w,parentPosition_center.orientation.x,parentPosition_center.orientation.y,parentPosition_center.orientation.z);

   qDebug()<<"teta_L:"<< teta_L*180/M_PI<<", Phi_L="<<phi_L*180/M_PI<<"teta_R:"<< teta_R*180/M_PI<<", Phi_R="<<phi_R*180/M_PI<<"teta_center:"<< teta_center*180/M_PI<<", Phi_center="<<phi_center*180/M_PI;
    //qDebug()<<QString::fromStdString( names[9])<<psitions[9].position.x<<psitions[9].position.y<<psitions[9].position.z;
    //ROS_INFO("I heard");

}

void  SendGazebo(QList<LinkM> links){
    if(links.count()<28){qDebug()<<"index err";return;}
    std_msgs::Float64 data;
    data.data=links[1].JointAngle;
    pub1.publish(data);
    data.data=links[2].JointAngle;
    pub2.publish(data);
    data.data=links[3].JointAngle;
    pub3.publish(data);
    data.data=links[4].JointAngle;
    pub4.publish(data);
    data.data=links[5].JointAngle;
    pub5.publish(data);
    data.data=links[6].JointAngle;
    pub6.publish(data);
    data.data=links[7].JointAngle;
    pub7.publish(data);
    data.data=links[8].JointAngle;
    pub8.publish(data);
    data.data=links[9].JointAngle;
    pub9.publish(data);
    data.data=links[10].JointAngle;
    pub10.publish(data);
    data.data=links[11].JointAngle;
    pub11.publish(data);
    data.data=links[12].JointAngle;
    pub12.publish(data);
    data.data=links[13].JointAngle;
    pub13.publish(data);
    data.data=links[14].JointAngle;
    pub14.publish(data);
    data.data=links[15].JointAngle;
    pub15.publish(data);
    data.data=links[16].JointAngle;
    pub16.publish(data);
    data.data=links[17].JointAngle;
    pub17.publish(data);
    data.data=links[18].JointAngle;
    pub18.publish(data);
    data.data=links[19].JointAngle;
    pub19.publish(data);
    data.data=links[20].JointAngle;
    pub20.publish(data);
    data.data=links[21].JointAngle;
    pub21.publish(data);
    data.data=links[22].JointAngle;
    pub22.publish(data);
    data.data=links[23].JointAngle;
    pub23.publish(data);
    data.data=links[24].JointAngle;
    pub24.publish(data);
    data.data=links[25].JointAngle;
    pub25.publish(data);
    data.data=links[26].JointAngle;
    pub26.publish(data);
    data.data=links[27].JointAngle;
    pub27.publish(data);
    data.data=links[28].JointAngle;
    pub28.publish(data);
}

void  SendGazebo_with_control(QList<LinkM> links,vector<double> cntrl){
    if(links.count()<28){qDebug()<<"index err";return;}
    std_msgs::Float64 data;
    data.data=links[1].JointAngle+cntrl[1];
    pub1.publish(data);
    data.data=links[2].JointAngle+cntrl[2];
    pub2.publish(data);
    data.data=links[3].JointAngle+cntrl[3];
    pub3.publish(data);
    data.data=links[4].JointAngle+cntrl[4];
    pub4.publish(data);
    data.data=links[5].JointAngle+cntrl[5];
    pub5.publish(data);
    data.data=links[6].JointAngle+cntrl[6];
    pub6.publish(data);
    data.data=links[7].JointAngle+cntrl[7];
    pub7.publish(data);
    data.data=links[8].JointAngle+cntrl[8];
    pub8.publish(data);
    data.data=links[9].JointAngle+cntrl[9];
    pub9.publish(data);
    data.data=links[10].JointAngle+cntrl[10];
    pub10.publish(data);
    data.data=links[11].JointAngle+cntrl[11];
    pub11.publish(data);
    data.data=links[12].JointAngle+cntrl[12];
    pub12.publish(data);
    data.data=links[13].JointAngle;
    pub13.publish(data);
    data.data=links[14].JointAngle;
    pub14.publish(data);
    data.data=links[15].JointAngle;
    pub15.publish(data);
    data.data=links[16].JointAngle;
    pub16.publish(data);
    data.data=links[17].JointAngle;
    pub17.publish(data);
    data.data=links[18].JointAngle;
    pub18.publish(data);
    data.data=links[19].JointAngle;
    pub19.publish(data);
    data.data=links[20].JointAngle;
    pub20.publish(data);
    data.data=links[21].JointAngle;
    pub21.publish(data);
    data.data=links[22].JointAngle;
    pub22.publish(data);
    data.data=links[23].JointAngle;
    pub23.publish(data);
    data.data=links[24].JointAngle;
    pub24.publish(data);
    data.data=links[25].JointAngle;
    pub25.publish(data);
    data.data=links[26].JointAngle;
    pub26.publish(data);
    data.data=links[27].JointAngle;
    pub27.publish(data);
    data.data=links[28].JointAngle;
    pub28.publish(data);
}

void RecievTime(const rosgraph_msgs::Clock & msg)
{
    time_ =double(msg.clock.nsec)*1e-9 + double(msg.clock.sec);
}

void RecievIMULeft(const sensor_msgs::Imu & msg)
{

    teta_L= msg.orientation.y;
    phi_L=msg.orientation.x;
    //ROS_INFO("Theta_L:[%f] Phi_L:[%f]",  teta_L*180/3.141592,phi_L*180/3.141592);

}

void RecievIMURight(const sensor_msgs::Imu & msg)
{
    teta_R= msg.orientation.y;
    phi_R=msg.orientation.x;
    //ROS_INFO("Theta_R:[%f] Phi_R:[%f]",  teta_R*180/3.141592,phi_R*180/3.141592);
}

void RecievIMUCenter(const xsens_msgs::orientationEstimate & msg)
{
    teta_center= msg.pitch;
   phi_center=msg.roll;
    //ROS_INFO("Theta_R:[%f] Phi_R:[%f]",  teta_R*180/3.141592,phi_R*180/3.141592);
}

int main(int argc, char **argv)
{
    //check _timesteps
    std_srvs::Empty emptyCall;
    QElapsedTimer timer;
    vector<int> qref(12);
    Robot SURENA;
    TaskSpaceOffline SURENAOffilneTaskSpace;
    QList<LinkM> links;
    MatrixXd PoseRoot;
    MatrixXd PoseRFoot;
    MatrixXd PoseLFoot;
    double dt;
    double hipRoll=0;
    double StartTime=0;
    double RollTime=0;
    double WalkTime=0;
    double  DurationOfStartPhase=6;
    double  DurationOfendPhase=6;
    //SURENAOffilneTaskSpace.GetAccVelPos();
    bool startPhase=true;
    bool endPhase=true;
    PoseRoot.resize(6,1);
    PoseRFoot.resize(6,1);
    PoseLFoot.resize(6,1);

    //*******************This part of code is for initialization of joints of the robot for walking**********************************
    int count = 0;

    ros::init(argc, argv, "myNode");

    ros::NodeHandle nh;
    ros::Publisher  chatter_pub  = nh.advertise<std_msgs::Int32MultiArray>("jointdata/qc",100);
    ros::Subscriber  IMULeft = nh.subscribe("/yei2000154", 100, RecievIMULeft);
    ros::Subscriber  IMURight = nh.subscribe("/yei200015B", 100, RecievIMURight);
    ros::Subscriber  IMULCenter = nh.subscribe("/mti/filter/orientation", 100, RecievIMUCenter);

    ros::ServiceClient tareLeft= nh.serviceClient<std_srvs::Empty>("/Tareyei2000154");
    ros::ServiceClient tareRight= nh.serviceClient<std_srvs::Empty>("/Tareyei200015B");

    ros::Subscriber sub = nh.subscribe("/gazebo/link_states", 100, &chatterCallback);

    pub1  = nh.advertise<std_msgs::Float64>("rrbot/joint1_position_controller/command",100);
    pub2  = nh.advertise<std_msgs::Float64>("rrbot/joint2_position_controller/command",100);
    pub3  = nh.advertise<std_msgs::Float64>("rrbot/joint3_position_controller/command",100);
    pub4  = nh.advertise<std_msgs::Float64>("rrbot/joint4_position_controller/command",100);
    pub5  = nh.advertise<std_msgs::Float64>("rrbot/joint5_position_controller/command",100);
    pub6  = nh.advertise<std_msgs::Float64>("rrbot/joint6_position_controller/command",100);
    pub7  = nh.advertise<std_msgs::Float64>("rrbot/joint7_position_controller/command",100);
    pub8  = nh.advertise<std_msgs::Float64>("rrbot/joint8_position_controller/command",100);
    pub9  = nh.advertise<std_msgs::Float64>("rrbot/joint9_position_controller/command",100);
    pub10 = nh.advertise<std_msgs::Float64>("rrbot/joint10_position_controller/command",100);
    pub11 = nh.advertise<std_msgs::Float64>("rrbot/joint11_position_controller/command",100);
    pub12 = nh.advertise<std_msgs::Float64>("rrbot/joint12_position_controller/command",100);
    pub13 = nh.advertise<std_msgs::Float64>("rrbot/joint13_position_controller/command",100);
    pub14 = nh.advertise<std_msgs::Float64>("rrbot/joint14_position_controller/command",100);
    pub15 = nh.advertise<std_msgs::Float64>("rrbot/joint15_position_controller/command",100);
    pub16 = nh.advertise<std_msgs::Float64>("rrbot/joint16_position_controller/command",100);
    pub17 = nh.advertise<std_msgs::Float64>("rrbot/joint17_position_controller/command",100);
    pub18 = nh.advertise<std_msgs::Float64>("rrbot/joint18_position_controller/command",100);
    pub19 = nh.advertise<std_msgs::Float64>("rrbot/joint19_position_controller/command",100);
    pub20 = nh.advertise<std_msgs::Float64>("rrbot/joint20_position_controller/command",100);
    pub21 = nh.advertise<std_msgs::Float64>("rrbot/joint21_position_controller/command",100);
    pub22 = nh.advertise<std_msgs::Float64>("rrbot/joint22_position_controller/command",100);
    pub23 = nh.advertise<std_msgs::Float64>("rrbot/joint23_position_controller/command",100);
    pub24 = nh.advertise<std_msgs::Float64>("rrbot/joint24_position_controller/command",100);
    pub25 = nh.advertise<std_msgs::Float64>("rrbot/joint25_position_controller/command",100);
    pub26 = nh.advertise<std_msgs::Float64>("rrbot/joint26_position_controller/command",100);
    pub27 = nh.advertise<std_msgs::Float64>("rrbot/joint27_position_controller/command",100);
    pub28 = nh.advertise<std_msgs::Float64>("rrbot/joint28_position_controller/command",100);

    rate=100;
    ros::Rate loop_rate(rate);
    std_msgs::Int32MultiArray msg;
    std_msgs::MultiArrayDimension msg_dim;
    dt=1/rate;
    p_teta=0.03;
    p_phi=0.03;
    i_teta=0;i_phi=0;
    d_teta=0;d_phi=0;
    teta_pid_L.Init(dt,0.15,-.15,p_teta,i_teta,d_teta);
    phi_pid_L.Init(dt,.15,-.15,p_phi,i_phi,d_phi);
    teta_pid_R.Init(dt,.15,-.15,p_teta,i_teta,d_teta);
    phi_pid_R.Init(dt,.15,-.15,p_phi,i_phi,d_phi);

    p_teta_center=0.00009;
    p_phi_center=0.00009;
    i_teta_center=0;i_phi_center=0;
    d_teta_center=0;d_phi_center=0;
    teta_pid_center.Init(dt,0.15,-.15,p_teta_center,i_teta_center,d_teta_center);
    phi_pid_center.Init(dt,.15,-.15,p_phi_center,i_phi_center,d_phi_center);

    msg_dim.label = "joint_position";
    msg_dim.size = 1;
    msg.layout.dim.clear();
    msg.layout.dim.push_back(msg_dim);

    //sensor imu----------------------------------------------sensor imu
    //sensor imu----------------------------------------------sensor imu
    //  timer.start();
    //  ros::Subscriber sub = nh.subscribe("/mti/sensor/imu", 1000, chatterCallback);
    //sensor imu----------------------------------------------sensor imu
    //sensor imu----------------------------------------------sensor imu
//    msg.data.clear();
//    for(int  i = 0;i < 12;i++)
//    {
//        qref[i] = 0;
//        //  cout << qref[i-1] <<" , "<<flush;
//        msg.data.push_back(qref[i]);
//    }
//    chatter_pub.publish(msg);
//    tareLeft.call(emptyCall);
//    tareRight.call(emptyCall);
//    QThread::msleep(500);

    while (ros::ok())
    {
        // qDebug()<<StartTime;

        if (startPhase==true && StartTime<=DurationOfStartPhase) {
            MinimumJerkInterpolation Coef;
            MatrixXd ZPosition(1,2);
            ZPosition<<0.95100,0.8300;
            MatrixXd ZVelocity(1,2);
            ZVelocity<<0.000,0.000;
            MatrixXd ZAcceleration(1,2);
            ZAcceleration<<0.000,0.000;

            MatrixXd Time(1,2);
            Time<<0,DurationOfStartPhase;
            MatrixXd CoefZStart =Coef.Coefficient(Time,ZPosition,ZVelocity,ZAcceleration);

            //R1=AnkleRollRight
            //R2=AnklePitchRight
            //R3=KneePitchRight
            //R4=HipPitchRight
            //R5=HipRollRight
            //R6=HipYawRight

            //L1=AnkleRollLeft
            //L2=AnklePitchLeft
            //L3=KneePitchLeft
            //L4=HipPitchLeft
            //L5=HipRollLeft
            //L6=HipYawLeft

            //R1=0*(1/2Pi)*(2304)*100  R2=1*-1*(1/2Pi)*(2304)*100   R3=2(1/2Pi)*(2304)*50   R4=3*-1(1/2Pi)*(2304)*80    L2=4*(1/2Pi)*(2304)*100    L1=5*(1/2Pi)*(2304)*100   L3=6*-1*(1/2Pi)*(2304)*50     L4=7*(1/2Pi)*(2304)*80   R6=8*-1*(1/2Pi)*(2304)*120   R5=9*(1/2Pi)*(2304)*120    L5=10*(1/2Pi)*(2304)*120    L6=11*-1*(1/2Pi)*(2304)*120

            //qDebug()<<mm(0,0);
            double zStart=0;
            double yStart=0;
            double xStart=0;
            StartTime=StartTime+SURENAOffilneTaskSpace._timeStep;

            MatrixXd outputZStart= SURENAOffilneTaskSpace.GetAccVelPos(CoefZStart,StartTime,0,5);
            zStart=outputZStart(0,0);

            PoseRoot<<xStart,yStart,zStart,0,0,0;

            PoseRFoot<<0,
                    -0.11500,
                    0.112000,
                    0,
                    0,
                    0;

            PoseLFoot<<0,
                    0.11500,
                    0.11200,
                    0,
                    0,
                    0;

            SURENA.doIK("LLeg_AnkleR_J6",PoseLFoot,"Body", PoseRoot);
            SURENA.doIK("RLeg_AnkleR_J6",PoseRFoot,"Body", PoseRoot);
        }

        if (StartTime>DurationOfStartPhase && StartTime<(DurationOfStartPhase+SURENAOffilneTaskSpace.MotionTime)){

            bool walk=true;
            double m1;
            double m2;
            double m3;
            double m4;
            double m5;
            double m6;
            StartTime=StartTime+SURENAOffilneTaskSpace._timeStep;
            //qDebug()<<StartTime;
            MatrixXd P;
            if(walk==true){
                MatrixXd m=SURENAOffilneTaskSpace.AnkleTrajectory(SURENAOffilneTaskSpace.globalTime);
                m1=m(0,0);
                m2=m(1,0);
                m3=m(2,0);
                m4=m(3,0);
                m5=m(4,0);
                m6=m(5,0);

                P=SURENAOffilneTaskSpace.PelvisTrajectory (SURENAOffilneTaskSpace.globalTime);

                // just some samples for plotting!!!
                //                SURENAOffilneTaskSpace.CoMXVector.append(P(0,0));
                //                SURENAOffilneTaskSpace.timeVector.append(SURENAOffilneTaskSpace.globalTime);
                //                SURENAOffilneTaskSpace.LeftFootXTrajectory.append(m1);

                SURENAOffilneTaskSpace.globalTime=SURENAOffilneTaskSpace.globalTime+SURENAOffilneTaskSpace._timeStep;

                if (round(SURENAOffilneTaskSpace.globalTime)<=round(SURENAOffilneTaskSpace.MotionTime)){

                    PoseRoot<<P(0,0),
                            P(1,0),
                            P(2,0),
                            0,
                            0,
                            0;

                    PoseRFoot<<m4,
                            m5,
                            m6,
                            0,
                            0,
                            0;

                    PoseLFoot<<m1,
                            m2,
                            m3,
                            0,
                            0,
                            0;

                    //// hip roll modification
                    //                    if (SURENAOffilneTaskSpace.LeftSupport==true && SURENAOffilneTaskSpace.HipRollModification==true){

                    //                        SURENA.doIKhipRollModify("LLeg_AnkleR_J6",PoseLFoot,"Body", PoseRoot,-1*hipRoll);
                    //                        SURENA.doIKhipRollModify("RLeg_AnkleR_J6",PoseRFoot,"Body", PoseRoot,0);

                    //                    }

                    //                    else if (SURENAOffilneTaskSpace.HipRollModification==true && SURENAOffilneTaskSpace.LeftSupport!=true )  {
                    //                        SURENA.doIKhipRollModify("LLeg_AnkleR_J6",PoseLFoot,"Body", PoseRoot,0);
                    //                        SURENA.doIKhipRollModify("RLeg_AnkleR_J6",PoseRFoot,"Body", PoseRoot,1*hipRoll);
                    //                    }

                    SURENA.doIK("LLeg_AnkleR_J6",PoseLFoot,"Body", PoseRoot);
                    SURENA.doIK("RLeg_AnkleR_J6",PoseRFoot,"Body", PoseRoot);
                    //R1=AnkleRollRight
                    //R2=AnklePitchRight
                    //R3=KneePitchRight
                    //R4=HipPitchRight
                    //R5=HipRollRight
                    //R6=HipYawRight

                    //L1=AnkleRollLeft
                    //L2=AnklePitchLeft
                    //L3=KneePitchLeft
                    //L4=HipPitchLeft
                    //L5=HipRollLeft
                    //L6=HipYawLeft

                    //R1=0*(1/2Pi)*(2304)*100  R2=1*-1*(1/2Pi)*(2304)*100   R3=2(1/2Pi)*(2304)*50   R4=3*-1(1/2Pi)*(2304)*80    L2=4*(1/2Pi)*(2304)*100    L1=5*(1/2Pi)*(2304)*100   L3=6*-1*(1/2Pi)*(2304)*50     L4=7*(1/2Pi)*(2304)*80   R6=8*-1*(1/2Pi)*(2304)*120   R5=9*(1/2Pi)*(2304)*120    L5=10*(1/2Pi)*(2304)*120    L6=11*-1*(1/2Pi)*(2304)*120

                }

            }
        }

        if (endPhase==true &&  StartTime>=(DurationOfStartPhase+SURENAOffilneTaskSpace.MotionTime) && StartTime<=DurationOfendPhase+DurationOfStartPhase+SURENAOffilneTaskSpace.MotionTime) {
            MinimumJerkInterpolation Coef;
            MatrixXd ZPosition(1,2);
            ZPosition<<0.830,0.95100;
            MatrixXd ZVelocity(1,2);
            ZVelocity<<0.000,0.000;
            MatrixXd ZAcceleration(1,2);
            ZAcceleration<<0.000,0.000;

            MatrixXd Time(1,2);
            Time<<DurationOfStartPhase+SURENAOffilneTaskSpace.MotionTime,DurationOfStartPhase+SURENAOffilneTaskSpace.MotionTime+DurationOfendPhase;
            MatrixXd CoefZStart =Coef.Coefficient(Time,ZPosition,ZVelocity,ZAcceleration);

            double zStart=0;
            double yStart=0;
            double xStart=0;
            StartTime=StartTime+SURENAOffilneTaskSpace._timeStep;

            MatrixXd outputZStart= SURENAOffilneTaskSpace.GetAccVelPos(CoefZStart,StartTime,DurationOfStartPhase+SURENAOffilneTaskSpace.MotionTime,5);
            zStart=outputZStart(0,0);

            PoseRoot<<xStart,yStart,zStart,0,0,0;

            PoseRFoot<<0,
                    -0.11500,
                    0.112000,
                    0,
                    0,
                    0;

            PoseLFoot<<0,
                    0.11500,
                    0.11200,
                    0,
                    0,
                    0;

            SURENA.doIK("LLeg_AnkleR_J6",PoseLFoot,"Body", PoseRoot);
            SURENA.doIK("RLeg_AnkleR_J6",PoseRFoot,"Body", PoseRoot);
        }
        links = SURENA.GetLinks();

        MatrixXi mappingJoints(12,1);
        //R6=0*(1/2Pi)*(2304)*100  R5=1*-1*(1/2Pi)*(2304)*100   R4=2*(1/2Pi)*(2304)*50   R3=3*-1(1/2Pi)*(2304)*80  R2=9*(1/2Pi)*(2304)*120  R1=8*-1*(1/2Pi)*(2304)*120
        //  L6=5*(1/2Pi)*(2304)*100   L5=4*(1/2Pi)*(2304)*100   L4=6*-1*(1/2Pi)*(2304)*50     L3=7*(1/2Pi)*(2304)*80  L2=10*(1/2Pi)*(2304)*120   L1=11*-1*(1/2Pi)*(2304)*120

        teta_motor_L=teta_motor_L+teta_pid_L.Calculate(0,teta_L);
        phi_motor_L=phi_motor_L+phi_pid_L.Calculate(0,phi_L);

        teta_motor_R=teta_motor_R+teta_pid_R.Calculate(0,teta_R);
        phi_motor_R=phi_motor_R+phi_pid_R.Calculate(0,phi_R);

        teta_motor_center=teta_motor_center+teta_pid_center.Calculate(0,teta_center);
        phi_motor_center=phi_motor_center+phi_pid_center.Calculate(0,phi_center);

        double maximum_d=.15;

        if (teta_motor_L<-maximum_d){teta_motor_L=-maximum_d;}
        if (teta_motor_L>maximum_d){teta_motor_L=maximum_d;}
        if (phi_motor_L<-maximum_d){phi_motor_L=-maximum_d;}
        if (phi_motor_L>maximum_d){phi_motor_L=maximum_d;}

        if (teta_motor_R<-maximum_d){teta_motor_R=-maximum_d;}
        if (teta_motor_R>maximum_d){teta_motor_R=maximum_d;}
        if (phi_motor_R<-maximum_d){phi_motor_R=-maximum_d;}
        if (phi_motor_R>maximum_d){phi_motor_R=maximum_d;}

        //mappingJoints<<links[6].JointAngle*(1/(2*M_PI))*(2304)*100+(phi_motor_R*2304*100/2/3.141592),//(phi_pid_R.Calculate(0,phi_R)*2304*100/2/3.141592),
        //    links[5].JointAngle*(-1)*(1/(2*M_PI))*(2304)*100+(-teta_motor_R*2304*100/2/3.141592),//(-teta_pid_R.Calculate(0,teta_R)*2304*100/2/3.141592),
        //    links[4].JointAngle*(1/(2*M_PI))*(2304)*50,
        //    links[3].JointAngle*-1*(1/(2*M_PI))*(2304)*80,
        //    links[11].JointAngle*(1/(2*M_PI))*(2304)*100+(teta_motor_L*2304*100/2/3.141592),//(teta_pid_L.Calculate(0,teta_L)*2304*100/2/3.141592),
        //    links[12].JointAngle*(1/(2*M_PI))*(2304)*100+(phi_motor_L*2304*100/2/3.141592),//(phi_pid_L.Calculate(0,phi_L)*2304*100/2/3.141592),
        //    links[10].JointAngle*-1*(1/(2*M_PI))*(2304)*50,
        //    links[9].JointAngle*(1/(2*M_PI))*(2304)*80,
        //    links[1].JointAngle*(-1)*(1/(2*M_PI))*(2304)*120,
        //    links[2].JointAngle*(1/(2*M_PI))*(2304)*120,
        //    links[8].JointAngle*(1/(2*M_PI))*(2304)*120,
        //    links[7].JointAngle*-1*(1/(2*M_PI))*(2304)*120;

        vector<double> cntrl(13);

        if (SURENAOffilneTaskSpace.DoubleSupport) {

            cntrl[0]=0.0;
            cntrl[1]=0;
            cntrl[2]=0;
            cntrl[3]=0;
            cntrl[4]=0;
            cntrl[5]=0;
            cntrl[6]=0;
            cntrl[7]=0;
            cntrl[8]=0;
            cntrl[9]=0;
            cntrl[10]=0;
            cntrl[11]=0;
            cntrl[12]=0;
            teta_motor_L=0;
            teta_motor_R=0;
            phi_motor_L=0;
            phi_motor_R=0;
            teta_motor_center=0;
            phi_motor_center=0;

           // ROS_INFO("t={%d} : double_support",timer.elapsed());
        }
        else{
            if (SURENAOffilneTaskSpace.LeftSupport) {
                cntrl[0]=0.0;
                cntrl[1]=0;
                cntrl[2]=0;
                cntrl[3]=0;
                cntrl[4]=0;
                cntrl[5]=1*teta_motor_R;
                cntrl[6]=1*phi_motor_R;
                cntrl[7]=0;
                cntrl[8]=0*phi_motor_center;
                cntrl[9]=0;
                cntrl[10]=0;
                cntrl[11]=0;
                cntrl[12]=0;
                teta_motor_L=0;
                phi_motor_L=0;
                teta_motor_center=0;

               // ROS_INFO("t={%d} : left_support",timer.elapsed());
            }
            else{
                cntrl[0]=0;
                cntrl[1]=0;
                cntrl[2]=-0*phi_motor_center;
                cntrl[3]=0;
                cntrl[4]=0;
                cntrl[5]=0;
                cntrl[6]=0;
                cntrl[5]=0;
                cntrl[6]=0;
                cntrl[7]=0;
                cntrl[8]=0;
                cntrl[9]=0;
                cntrl[10]=0;
                cntrl[11]=1*teta_motor_L;
                cntrl[12]=1*phi_motor_L;
                teta_motor_R=0;
                phi_motor_R=0;
                teta_motor_center=0;
              //  ROS_INFO("t={%d} : right_support",timer.elapsed());
            }
        }

        QCgenerator QC;
        qref=QC.data2qc(links,cntrl);
        msg.data.clear();
        for(int  i = 1;i < 13;i++)
        {
            // qref[i-1] = mappingJoints(i-1,0);
            //  cout << qref[i-1] <<" , "<<flush;
            msg.data.push_back(qref[i-1]);

        }
        // std::string varAsString = std::to_string(qref[i-1]);
        // msg.data =varAsString;
        SendGazebo_with_control(links,cntrl);
        chatter_pub.publish(msg);
         ROS_INFO("t={%d} c={%d}",timer.elapsed(),count);

        //ROS_INFO("teta_r={%f} phi_r={%f} teta_l={%f} phi_l={%f}",teta_motor_R,phi_motor_R,teta_motor_L,phi_motor_L);
//        ROS_INFO("teta_l_IMU={%f} phi_l_IMU={%f} teta_l={%f} phi_l={%f}",teta_L,phi_L,teta_motor_L,phi_motor_L);
 //ROS_INFO("teta_C_IMU={%f} phi_C_IMU={%f} ",teta_center,phi_center);
        //
        //    std_msgs::String msg;
        // msg.data = "milad";

        //R1=AnkleRollRight
        //R2=AnklePitchRight
        //R3=KneePitchRight
        //R4=HipPitchRight
        //R5=HipRollRight
        //R6=HipYawRight

        //L1=AnkleRollLeft
        //L2=AnklePitchLeft
        //L3=KneePitchLeft
        //L4=HipPitchLeft
        //L5=HipRollLeft
        //L6=HipYawLeft

        //R1=0*(1/2Pi)*(2304)*100  R2=1*-1*(1/2Pi)*(2304)*100   R3=2(1/2Pi)*(2304)*50   R4=3*-1(1/2Pi)*(2304)*80   R5=9*(1/2Pi)*(2304)*120  R6=8*-1*(1/2Pi)*(2304)*120      L1=5*(1/2Pi)*(2304)*100  L2=4*(1/2Pi)*(2304)*100  L3=6*-1*(1/2Pi)*(2304)*50    L4=7*(1/2Pi)*(2304)*80    L5=10*(1/2Pi)*(2304)*120    L6=11*-1*(1/2Pi)*(2304)*120

        //R6=AnkleRollRight
        //R5=AnklePitchRight
        //R4=KneePitchRight
        //R3=HipPitchRight
        //R2=HipRollRight
        //R1=HipYawRight

        //L6=AnkleRollLeft
        //L5=AnklePitchLeft
        //L4=KneePitchLeft
        //L3=HipPitchLeft
        //L2=HipRollLeft
        //L1=HipYawLeft

        //R6=0*(1/2Pi)*(2304)*100  R5=1*-1*(1/2Pi)*(2304)*100   R4=2(1/2Pi)*(2304)*50   R3=3*-1(1/2Pi)*(2304)*80  R2=9*(1/2Pi)*(2304)*120  R1=8*-1*(1/2Pi)*(2304)*120    L6=5*(1/2Pi)*(2304)*100   L5=4*(1/2Pi)*(2304)*100   L4=6*-1*(1/2Pi)*(2304)*50     L3=7*(1/2Pi)*(2304)*80  L2=10*(1/2Pi)*(2304)*120   L1=11*-1*(1/2Pi)*(2304)*120

        ros::spinOnce();
        loop_rate.sleep();
        ++count;
    }

    return 0;
}


#include <Spirit_Defines.h>
#include <engine/Method_MMF.hpp>
#include <engine/Vectormath.hpp>
#include <engine/Manifoldmath.hpp>
#include <engine/Eigenmodes.hpp>
#include <io/IO.hpp>
#include <io/OVF_File.hpp>
#include <utility/Logging.hpp>
#include <utility/Version.hpp>

#include <string.h>

#include <Eigen/Core>
#include <Eigen/Eigenvalues>

#include <fmt/format.h>

using Utility::Log_Level;
using Utility::Log_Sender;
namespace C = Utility::Constants;

namespace Engine
{
    template <Solver solver>
    Method_MMF<solver>::Method_MMF(std::shared_ptr<Data::Spin_System> system, int idx_chain) :
        Method_Solver<solver>(system->mmf_parameters, -1, idx_chain)
    {
        this->systems = std::vector<std::shared_ptr<Data::Spin_System>>(1, system);
        this->system = system;
        this->noi = this->systems.size();
        this->nos = this->systems[0]->nos;

        switched1 = false;
        switched2 = false;
        this->SenderName = Utility::Log_Sender::MMF;

        // History
        this->history = std::map<std::string, std::vector<scalar>>{
            {"max_torque", {this->max_torque}} };

        // We assume that the systems are not converged before the first iteration
        this->max_torque = system->mmf_parameters->force_convergence + 1.0;

        this->hessian = MatrixX(3*this->nos, 3*this->nos);
        // Forces
        this->gradient     = vectorfield(this->nos, {0,0,0});
        this->minimum_mode = vectorfield(this->nos, {0,0,0});
        this->xi           = vectorfield(this->nos, {0,0,0});

        // Last iteration
        this->spins_last = vectorfield(this->nos);
        this->Rx_last = 0.0;

        // Force function
        // ToDo: move into parameters
        this->mm_function = "Spectra Matrix"; // "Spectra Matrix" "Spectra Prefactor" "Lanczos"

        this->mode_follow_previous = 0;

        // Create shared pointers to the method's systems' spin configurations
        this->configurations = std::vector<std::shared_ptr<vectorfield>>(1);
        this->configurations[0] = this->system->spins;

        //---- Initialise Solver-specific variables
        this->Initialize();
    }

    template <Solver solver>
    void Method_MMF<solver>::Calculate_Force(const std::vector<std::shared_ptr<vectorfield>> & configurations, std::vector<vectorfield> & forces)
    {
        // if (this->mm_function == "Spectra Matrix")
        // {
            this->Calculate_Force_Spectra_Matrix(configurations, forces);
        // }
        // else if (this->mm_function == "Lanczos")
        // {
        //     this->Calculate_Force_Lanczos(configurations, forces);
        // }
        #ifdef SPIRIT_ENABLE_PINNING
            Vectormath::set_c_a(1, forces[0], forces[0], this->systems[0]->geometry->mask_unpinned);
        #endif // SPIRIT_ENABLE_PINNING
    }

    void check_modes(const vectorfield & image, const vectorfield & gradient, const MatrixX & tangent_basis, const VectorX & eigenvalues, const MatrixX & eigenvectors_2N, const vectorfield & minimum_mode)
    {
        int nos = image.size();

        // ////////////////////////////////////////////////////////////////
        // // Check for complex numbers in the eigenvalues
        // if (std::abs(hessian_spectrum.eigenvalues().imag()[0]) > 1e-8)
        //     std::cerr << "     >>>>>>>> WARNING  nonzero complex EW    WARNING" << std::endl;
        // for (int ispin=0; ispin<nos; ++ispin)
        // {
        //     if (std::abs(hessian_spectrum.eigenvectors().col(0).imag()[0]) > 1e-8)
        //         std::cerr << "     >>>>>>>> WARNING  nonzero complex EV x  WARNING" << std::endl;
        //     if (std::abs(hessian_spectrum.eigenvectors().col(0).imag()[1]) > 1e-8)
        //         std::cerr << "     >>>>>>>> WARNING  nonzero complex EV y  WARNING" << std::endl;
        //     if (std::abs(hessian_spectrum.eigenvectors().col(0).imag()[2]) > 1e-8)
        //         std::cerr << "     >>>>>>>> WARNING  nonzero complex EV z  WARNING" << std::endl;
        // }
        // ////////////////////////////////////////////////////////////////

        ////////////////////////////////////////////////////////////////
        // For one of the tests
        auto gradient_tangential = gradient;
        Manifoldmath::project_tangential(gradient_tangential, image);
        // Get the tangential gradient in 2N-representation
        Eigen::Ref<VectorX> gradient_tangent_3N = Eigen::Map<VectorX>(gradient_tangential[0].data(), 3 * nos);
        VectorX gradient_tangent_2N             = tangent_basis.transpose() * gradient_tangent_3N;
        // Eigenstuff
        scalar eval_lowest = eigenvalues[0];
        VectorX evec_lowest_2N = eigenvectors_2N.col(0);
        VectorX evec_lowest_3N = tangent_basis * evec_lowest_2N;
        /////////
        // Norms
        scalar image_norm           = Manifoldmath::norm(image);
        scalar grad_norm            = Manifoldmath::norm(gradient);
        scalar grad_tangent_norm    = Manifoldmath::norm(gradient_tangential);
        scalar mode_norm            = Manifoldmath::norm(minimum_mode);
        scalar mode_norm_2N         = evec_lowest_2N.norm();
        // Scalar products
        scalar mode_dot_image       = std::abs(Vectormath::dot(minimum_mode, image) / mode_norm); // mode should be orthogonal to image in 3N-space
        scalar mode_grad_angle      = std::abs(evec_lowest_3N.dot(gradient_tangent_3N) / evec_lowest_3N.norm() / gradient_tangent_3N.norm());
        scalar mode_grad_angle_2N   = std::abs(evec_lowest_2N.dot(gradient_tangent_2N) / evec_lowest_2N.norm() / gradient_tangent_2N.norm());
        // Do some more checks to ensure the mode fulfills our requirements
        bool bad_image_norm         = 1e-8  < std::abs( image_norm - std::sqrt((scalar)nos) ); // image norm should be sqrt(nos)
        bool bad_grad_norm          = 1e-8  > grad_norm;         // gradient should not be a zero vector
        bool bad_grad_tangent_norm  = 1e-8  > grad_tangent_norm; // gradient should not be a zero vector in tangent space
        bool bad_mode_norm          = 1e-8  > mode_norm;         // mode should not be a zero vector
        /////////
        bool bad_mode_dot_image     = 1e-10 < mode_dot_image;     // mode should be orthogonal to image in 3N-space
        bool bad_mode_grad_angle    = 1e-8  > mode_grad_angle;    // mode should not be orthogonal to gradient in 3N-space
        bool bad_mode_grad_angle_2N = 1e-8  > mode_grad_angle_2N; // mode should not be orthogonal to gradient in 2N-space
        /////////
        bool eval_nonzero           = 1e-8  < std::abs(eval_lowest);
        /////////
        if ( bad_image_norm     || bad_mode_norm  || bad_grad_norm        || bad_grad_tangent_norm   ||
             bad_mode_dot_image || ( eval_nonzero && (bad_mode_grad_angle || bad_mode_grad_angle_2N) ) )
        {
            // scalar theta, phi;
            // Manifoldmath::spherical_from_cartesian(image[1], theta, phi);
            std::cerr << "-------------------------"                       << std::endl;
            std::cerr << "BAD MODE! evalue =      " << eigenvalues[0]          << std::endl;
            // std::cerr << "image (theta,phi):      " << theta << " " << phi << std::endl;
            std::cerr << "image norm:             " << image_norm          << std::endl;
            std::cerr << "mode norm:              " << mode_norm           << std::endl;
            std::cerr << "mode norm 2N:           " << mode_norm_2N        << std::endl;
            std::cerr << "grad norm:              " << grad_norm           << std::endl;
            std::cerr << "grad norm tangential:   " << grad_tangent_norm   << std::endl;
            if (bad_image_norm)
                std::cerr << "   image norm is not equal to sqrt(nos): " << image_norm << std::endl;
            if (bad_mode_norm)
                std::cerr << "   mode norm is too small: "               << mode_norm  << std::endl;
            if (bad_grad_norm)
                std::cerr << "   gradient norm is too small: "           << grad_norm  << std::endl;
            if (bad_mode_dot_image)
            {
                std::cerr << "   mode NOT TANGENTIAL to SPINS: "         << mode_dot_image << std::endl;
                std::cerr << "             >>> check the (3N x 2N) spherical basis matrix" << std::endl;
            }
            if ( eval_nonzero && (bad_mode_grad_angle || bad_mode_grad_angle_2N) )
            {
                std::cerr << "   mode is ORTHOGONAL to GRADIENT: 3N = " << mode_grad_angle << std::endl;
                std::cerr << "                              >>>  2N = " << mode_grad_angle_2N << std::endl;
            }
            std::cerr << "-------------------------" << std::endl;
        }
        ////////////////////////////////////////////////////////////////
    }

    template <Solver solver>
    void Method_MMF<solver>::Calculate_Force_Spectra_Matrix(const std::vector<std::shared_ptr<vectorfield>> & configurations, std::vector<vectorfield> & forces)
    {
        auto& image = *configurations[0];
        auto& force = forces[0];
        // auto& force_virtual = forces_virtual[0];
        auto& parameters = *this->systems[0]->mmf_parameters;

        const int nos = this->nos;

        // Number of lowest modes to be calculated
        int n_modes = parameters.n_modes;
        // Mode to follow in the positive region
        int mode_positive = parameters.n_mode_follow;
        mode_positive = std::max(0, std::min(n_modes-1, mode_positive));
        // Mode to follow in the negative region
        int mode_negative = parameters.n_mode_follow;
        if (false)  // if (parameters.negative_follow_lowest)
            mode_negative = 0;
        mode_negative = std::max(0, std::min(n_modes-1, mode_negative));

        // The gradient (unprojected)
        this->systems[0]->hamiltonian->Gradient(image, gradient, NULL);
        Vectormath::set_c_a(1, gradient, gradient, this->systems[0]->geometry->mask_unpinned);

        // The Hessian (unprojected)
        this->systems[0]->hamiltonian->Hessian(image, hessian);

        Eigen::Ref<VectorX> image_3N = Eigen::Map<VectorX>(image[0].data(), 3*nos);
        Eigen::Ref<VectorX> gradient_3N  = Eigen::Map<VectorX>(gradient[0].data(), 3*nos);

        ////////////////////////////////////////////////////////////////////////////////////////////////////////////////
        ////////////////////////////////////////////////////////////////////////////////////////////////////////////////

        // Get the eigenspectrum
        MatrixX hessian_final = MatrixX::Zero(2*nos, 2*nos);
        MatrixX basis_3Nx2N = MatrixX::Zero(3*nos, 2*nos);
        VectorX eigenvalues;
        MatrixX eigenvectors;
        bool successful = Eigenmodes::Hessian_Partial_Spectrum(this->parameters, image, gradient, hessian, n_modes, basis_3Nx2N, hessian_final, eigenvalues, eigenvectors);

        if (successful)
        {
            // TODO: if the mode that is followed in the positive region is not no. 0,
            //       the following won't work!!
            //       Need to save the mode_follow as a local variable and update it as necessary.
            //       Question: what to do when MMF is paused and re-started? Do we remember the
            //       correct mode, and if yes how?
            //       Note: it should be distinguished if we are starting in the positive or negative
            //       region. Probably some of this should go into the constructor.

            // Determine the mode to follow
            int mode_follow = mode_negative;
            if (eigenvalues[0] > -1e-6)
                mode_follow = mode_positive;

            if (true)// && eigenvalues[0] > -1e-6)
            {
                // Determine if we are still following the same mode and correct if not
                // std::abs(mode_2N_previous.dot(eigenvectors.col(mode_follow))) < 1e-2
                // std::abs(mode_2N_previous.dot(eigenvectors.col(itest)))       >= 1-1e-4
                if (mode_2N_previous.size() > 0)
                {
                    mode_follow = mode_follow_previous;
                    scalar mode_dot_mode = std::abs(mode_2N_previous.dot(eigenvectors.col(mode_follow)));
                    if (mode_dot_mode < 0.99)
                    {
                        // Need to look for our mode
                        std::cerr << fmt::format("Looking for previous mode, which used to be {}...", mode_follow_previous);
                        int ntest = 6;
                        // int start = std::max(0, mode_follow_previous-ntest);
                        // int stop  = std::min(n_modes, mode_follow_previous+ntest);
                        for (int itest = 0; itest < n_modes; ++itest)
                        {
                            scalar m_dot_m_test = std::abs(mode_2N_previous.dot(eigenvectors.col(itest)));
                            if (m_dot_m_test > mode_dot_mode)
                            {
                                mode_follow = itest;
                                mode_dot_mode = m_dot_m_test;
                            }
                        }
                        if (mode_follow != mode_follow_previous)
                            std::cerr << fmt::format("Found mode no. {}", mode_follow) << std::endl;
                        else
                            std::cerr << "Did not find a new mode..." << std::endl;
                    }
                }

                // Save chosen mode as "previous" for next iteration
                mode_follow_previous = mode_follow;
                mode_2N_previous = eigenvectors.col(mode_follow);
            }

            // Ref to correct mode
            Eigen::Ref<VectorX> mode_2N = eigenvectors.col(mode_follow);
            scalar mode_evalue = eigenvalues[mode_follow];

            // Retrieve the chosen mode as vectorfield
            VectorX mode_3N = basis_3Nx2N * mode_2N;
            for (int n=0; n<nos; ++n)
                this->minimum_mode[n] = {mode_3N[3*n], mode_3N[3*n+1], mode_3N[3*n+2]};

            // Get the scalar product of mode and gradient
            scalar mode_grad = mode_3N.dot(gradient_3N);
            // Get the angle between mode and gradient (in the tangent plane!)
            VectorX graient_tangent_3N = gradient_3N - gradient_3N.dot(image_3N) * image_3N;
            scalar mode_grad_angle = std::abs( mode_grad / (mode_3N.norm()*gradient_3N.norm()) );

            // Make sure there is nothing wrong
            check_modes(image, gradient, basis_3Nx2N, eigenvalues, eigenvectors, minimum_mode);

            Manifoldmath::project_tangential(gradient, image);

            // Some debugging prints
            if (mode_evalue < -1e-6 && mode_grad_angle > 1e-8)// -1e-6)// || switched2)
            {
                std::cerr << fmt::format("negative region: {:<65}   mode={}   angle = {:15.10f}   lambda*F = {:15.10f}", eigenvalues.transpose(), mode_follow, std::acos(std::min(mode_grad_angle, scalar(1.0)))*180.0/C::Pi, std::abs(mode_grad)) << std::endl;
            }
            else if (mode_grad_angle > 1e-8)
            {
                std::cerr << fmt::format("positive region: {:<65}   mode={}   angle = {:15.10f}   lambda*F = {:15.10f}", eigenvalues.transpose(), mode_follow, std::acos(std::min(mode_grad_angle, scalar(1.0)))*180.0/C::Pi, std::abs(mode_grad)) << std::endl;
            }
            else
            {
                if (std::abs(mode_evalue) > 1e-8)
                {
                    std::cerr << fmt::format("bad region:      {:<65}   mode={}   angle = {:15.10f}   lambda*F = {:15.10f}", eigenvalues.transpose(), mode_follow, std::acos(std::min(mode_grad_angle, scalar(1.0)))*180.0/C::Pi, std::abs(mode_grad)) << std::endl;
                }
                else
                {
                    std::cerr << fmt::format("zero region:     {:<65}   mode={}   angle = {:15.10f}   lambda*F = {:15.10f}", eigenvalues.transpose(), mode_follow, std::acos(std::min(mode_grad_angle, scalar(1.0)))*180.0/C::Pi, std::abs(mode_grad)) << std::endl;
                }
            }

            // TODO: parameter whether to *always* follow the minimum mode
            if (false)
            {
                // Invert the gradient force along the minimum mode
                Manifoldmath::invert_parallel(gradient, minimum_mode);

                // Copy out the forces
                Vectormath::set_c_a(-1, gradient, force, this->systems[0]->geometry->mask_unpinned);
            }
            else
            {
                if (eigenvalues[0] < -1e-6 && mode_grad_angle > 1e-8)// -1e-6)// || switched2)
                {
                    // Invert the gradient force along the minimum mode
                    Manifoldmath::invert_parallel(gradient, minimum_mode);

                    // Copy out the forces
                    Vectormath::set_c_a(-1, gradient, force, this->systems[0]->geometry->mask_unpinned);
                }
                else if (mode_grad_angle > 1e-8)
                {
                    // TODO: add switch between gradient and mode following for positive region
                    if (false)
                    {
                        // Calculate the force
                        // Vectormath::set_c_a(mode_grad, this->minimum_mode, force, this->systems[0]->geometry->mask_unpinned);
                        int sign = (scalar(0) < mode_grad) - (mode_grad < scalar(0));
                        Vectormath::set_c_a(sign, this->minimum_mode, force, this->systems[0]->geometry->mask_unpinned);
                    }
                    else
                    {
                        // Copy out the forces
                        Vectormath::set_c_a(1, gradient, force, this->systems[0]->geometry->mask_unpinned);
                    }
                }
                else
                {
                    if (std::abs(mode_evalue) > 1e-8)
                    {
                        // Invert the gradient force along the minimum mode
                        Manifoldmath::invert_parallel(gradient, minimum_mode);

                        // Copy out the forces
                        Vectormath::set_c_a(-1, gradient, force, this->systems[0]->geometry->mask_unpinned);
                    }
                    else
                    {
                        // Copy out the forces
                        Vectormath::set_c_a(1, gradient, force, this->systems[0]->geometry->mask_unpinned);
                    }
                }
            }
        }
        else
        {
            // Spectra was not successful in calculating an eigenvector
            Log(Log_Level::Error, Log_Sender::MMF, "Failed to calculate eigenvectors of the Hessian!");
            Log(Log_Level::Info,  Log_Sender::MMF, "Zeroing the MMF force...");
            Vectormath::fill(force, Vector3{0,0,0});
        }
    }

    void printmatrix(MatrixX & m)
    {
        std::cerr << m << std::endl;
    }

    // Check if the Forces are converged
    template <Solver solver>
    bool Method_MMF<solver>::Converged()
    {
        if (this->max_torque < this->systems[0]->mmf_parameters->force_convergence) return true;
        return false;
    }

    template <Solver solver>
    void Method_MMF<solver>::Hook_Pre_Iteration()
    {

    }

    template <Solver solver>
    void Method_MMF<solver>::Hook_Post_Iteration()
    {
        // --- Convergence Parameter Update
        this->max_torque = 0;
        // Loop over images to calculate the maximum torques
        for (unsigned int img = 0; img < this->systems.size(); ++img)
        {
            auto fmax = this->MaxTorque_on_Image(*(this->systems[img]->spins), this->forces_virtual[img]);
            if (fmax > 0) this->max_torque = fmax;
            else this->max_torque = 0;
        }
    }

    template <Solver solver>
    void Method_MMF<solver>::Save_Current(std::string starttime, int iteration, bool initial, bool final)
    {
        // History save
        this->history["max_torque"].push_back(this->max_torque);

        // File save
        if (this->parameters->output_any)
        {
            // Convert indices to formatted strings
            auto s_img  = fmt::format("{:0>2}", this->idx_image);
            int base = (int)log10(this->parameters->n_iterations);
            std::string s_iter = fmt::format("{:0>"+fmt::format("{}",base)+"}", iteration);

            std::string preSpinsFile;
            std::string preEnergyFile;
            std::string fileTag;

            if (this->parameters->output_file_tag == "<time>")
                fileTag = starttime + "_";
            else if (this->parameters->output_file_tag != "")
                fileTag = this->parameters->output_file_tag + "_";
            else
                fileTag = "";

            preSpinsFile = this->parameters->output_folder + "/" + fileTag + "Image-" + s_img + "_Spins";
            preEnergyFile = this->parameters->output_folder + "/"+ fileTag + "Image-" + s_img + "_Energy";

            // Function to write or append image and energy files
            auto writeOutputConfiguration = [this, preSpinsFile, preEnergyFile, iteration](std::string suffix, bool append)
            {
                try
                {
                    // File name and comment
                    std::string spinsFile = preSpinsFile + suffix + ".ovf";
                    std::string output_comment = fmt::format( "{} simulation ({} solver)\n# Desc:      Iteration: {}\n# Desc:      Maximum torque: {}",
                        this->Name(), this->SolverFullName(), iteration, this->max_torque );

                    // File format
                    IO::VF_FileFormat format = this->systems[0]->mmf_parameters->output_vf_filetype;

                    // Spin Configuration
                    auto& spins = *this->systems[0]->spins;
                    auto segment = IO::OVF_Segment(*this->systems[0]);
                    std::string title = fmt::format( "SPIRIT Version {}", Utility::version_full );
                    segment.title = strdup(title.c_str());
                    segment.comment = strdup(output_comment.c_str());
                    segment.valuedim = 3;
                    segment.valuelabels = strdup("spin_x spin_y spin_z");
                    segment.valueunits  = strdup("none none none");
                    if( append )
                        IO::OVF_File(spinsFile).append_segment(segment, spins[0].data(), int(format));
                    else
                        IO::OVF_File(spinsFile).write_segment(segment, spins[0].data(), int(format));
                }
                catch( ... )
                {
                   spirit_handle_exception_core( "LLG output failed" );
                }
            };

            auto writeOutputEnergy = [this, preSpinsFile, preEnergyFile, iteration](std::string suffix, bool append)
            {
                bool normalize = this->systems[0]->llg_parameters->output_energy_divide_by_nspins;
                bool readability = this->systems[0]->llg_parameters->output_energy_add_readability_lines;

                // File name
                std::string energyFile = preEnergyFile + suffix + ".txt";
                std::string energyFilePerSpin = preEnergyFile + "-perSpin" + suffix + ".txt";

                // Energy
                if (append)
                {
                    // Check if Energy File exists and write Header if it doesn't
                    std::ifstream f(energyFile);
                    if (!f.good()) IO::Write_Energy_Header(*this->systems[0], energyFile, {"iteration", "E_tot"}, true, normalize, readability);
                    // Append Energy to File
                    IO::Append_Image_Energy(*this->systems[0], iteration, energyFile, normalize, readability);
                }
                else
                {
                    IO::Write_Energy_Header(*this->systems[0], energyFile, {"iteration", "E_tot"}, true, normalize, readability);
                    IO::Append_Image_Energy(*this->systems[0], iteration, energyFile, normalize, readability);
                    if (this->systems[0]->mmf_parameters->output_energy_spin_resolved)
                    {
                        // Gather the data
                        std::vector<std::pair<std::string, scalarfield>> contributions_spins(0);
                        this->systems[0]->UpdateEnergy();
                        this->systems[0]->hamiltonian->Energy_Contributions_per_Spin(*this->systems[0]->spins, contributions_spins);
                        int datasize = (1+contributions_spins.size())*this->systems[0]->nos;
                        scalarfield data(datasize, 0);
                        for( int ispin=0; ispin<this->systems[0]->nos; ++ispin )
                        {
                            scalar E_spin=0;
                            int j = 1;
                            for( auto& contribution : contributions_spins )
                            {
                                E_spin += contribution.second[ispin];
                                data[ispin+j] = contribution.second[ispin];
                                ++j;
                            }
                            data[ispin] = E_spin;
                        }

                        // Segment
                        auto segment = IO::OVF_Segment(*this->systems[0]);

                        std::string title = fmt::format( "SPIRIT Version {}", Utility::version_full );
                        segment.title = strdup(title.c_str());
                        std::string comment = fmt::format("Energy per spin. Total={}meV", this->systems[0]->E);
                        for( auto& contribution : this->systems[0]->E_array )
                            comment += fmt::format(", {}={}meV", contribution.first, contribution.second);
                        segment.comment = strdup(comment.c_str());
                        segment.valuedim = 1 + this->systems[0]->E_array.size();

                        std::string valuelabels = "Total";
                        std::string valueunits  = "meV";
                        for( auto& pair : this->systems[0]->E_array )
                        {
                            valuelabels += fmt::format(" {}", pair.first);
                            valueunits  += " meV";
                        }
                        segment.valuelabels = strdup(valuelabels.c_str());

                        // File format
                        IO::VF_FileFormat format = this->systems[0]->llg_parameters->output_vf_filetype;

                        // open and write
                        IO::OVF_File(energyFilePerSpin).write_segment(segment, data.data(), int(format));

                        Log( Utility::Log_Level::Info, Utility::Log_Sender::API, fmt::format(
                            "Wrote spins to file \"{}\" with format {}", energyFilePerSpin, int(format) ),
                            -1, -1 );
                    }
                }
            };

            // Initial image before simulation
            if (initial && this->parameters->output_initial)
            {
                writeOutputConfiguration("-initial", false);
                writeOutputEnergy("-initial", false);
            }
            // Final image after simulation
            else if (final && this->parameters->output_final)
            {
                writeOutputConfiguration("-final", false);
                writeOutputEnergy("-final", false);
            }

            // Single file output
            if (this->systems[0]->mmf_parameters->output_configuration_step)
            {
                writeOutputConfiguration("_" + s_iter, false);
            }
            if (this->systems[0]->mmf_parameters->output_energy_step)
            {
                writeOutputEnergy("_" + s_iter, false);
            }

            // Archive file output (appending)
            if (this->systems[0]->mmf_parameters->output_configuration_archive)
            {
                writeOutputConfiguration("-archive", true);
            }
            if (this->systems[0]->mmf_parameters->output_energy_archive)
            {
                writeOutputEnergy("-archive", true);
            }

            // Save Log
            Log.Append_to_File();
        }
    }

    template <Solver solver>
    void Method_MMF<solver>::Finalize()
    {
        this->systems[0]->iteration_allowed = false;
    }

    // Method name as string
    template <Solver solver>
    std::string Method_MMF<solver>::Name() { return "MMF"; }

    // Template instantiations
    template class Method_MMF<Solver::SIB>;
    template class Method_MMF<Solver::Heun>;
    template class Method_MMF<Solver::Depondt>;
    template class Method_MMF<Solver::RungeKutta4>;
    template class Method_MMF<Solver::LBFGS_OSO>;
    template class Method_MMF<Solver::LBFGS_Atlas>;
    template class Method_MMF<Solver::VP>;
    template class Method_MMF<Solver::VP_OSO>;
}