#include <boost/endian/conversion.hpp>

#include "../include/client_p.hpp"

Zsync3::ClientPrivate::RollingChecksum::RollingChecksum()
    : a(0),
      b(0),
      nb(0) { }

//// Expects the given variables to be in big endian
Zsync3::ClientPrivate::RollingChecksum::RollingChecksum(uint16_t gA, uint16_t gB)
    : RollingChecksum() {

    /// Convert to host endian and store.
    /// We need to convert from network endian to host endian,
    /// Network endian is nothing but big endian byte order, So if we have little
    /// endian byte order, We need to convert the data but if we have a big endian
    /// byte order, We can simply avoid this conversion to save computation power.
    /////
    /// But most of the time we will need little endian since intel's microproccessors
    /// always follows the little endian byte order.

    if(boost::endian::order::native == boost::endian::order::little) {
        a = boost::endian::endian_reverse(gA);
        b = boost::endian::endian_reverse(gB);
    }
}

Zsync3::ClientPrivate::RollingChecksum::RollingChecksum(const uint8_t *data, std::size_t len)
    : RollingChecksum() {
    if(!data) {
        return;
    }
    while(len) {
        uint8_t c = *data++;
        a += c;
        b += len * c;
        len--;
    }
}

Zsync3::ClientPrivate::RollingChecksum::RollingChecksum(const RollingChecksum &other) {
    this->a = other.a;
    this->b = other.b;
    this->nb = other.nb;
}

//// Let a = Char1 + Char2 + .... of some data
//// So to update a
//// Let updated a = Char1 + Char2 + ... - Char1 + NewChar1
////               = NewChar1 + Char2 + ...
//// I think you get the idea, I can explain b it takes a lot
//// of space.
void Zsync3::ClientPrivate::RollingChecksum::update(uint8_t oldChar,
        uint8_t newChar,
        int32_t blockShift) {
    a += newChar - oldChar;
    b += a - (oldChar << blockShift);
}

void Zsync3::ClientPrivate::RollingChecksum::operator = (const RollingChecksum &obj) {
    this->a = obj.a;
    this->b = obj.b;
    this->nb = obj.nb;
}

//// Hasher for the RollingChecksum made for std::map variants.
Zsync3::ClientPrivate::RollingChecksumHasher::RollingChecksumHasher() { }

Zsync3::ClientPrivate::RollingChecksumHasher::RollingChecksumHasher(uint16_t wm, uint16_t hm, int matches)
	: wmask(wm),
	  hmask(hm),
	  n_seq(matches) { }

std::size_t Zsync3::ClientPrivate::RollingChecksumHasher::operator () (const RollingChecksum &key) const {
	size_t h = key.b;

	h ^= (n_seq > 1) ? key.nb : key.a & wmask;
	return std::hash<uint32_t>()(h & hmask);
}

bool Zsync3::ClientPrivate::RollingChecksumEqual::operator () (const RollingChecksum &lhs, const RollingChecksum &rhs) const {
	return lhs.a == rhs.a && lhs.b == rhs.b;
}


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

#include "amount.h"
#include "chainparams.h"
#include "coincontrol.h"
#include "denomination_functions.h"
#include "main.h"
#include "txdb.h"
#include "wallet.h"
#include "walletdb.h"
#include <boost/test/unit_test.hpp>
#include <iostream>

using namespace libzerocoin;

BOOST_AUTO_TEST_SUITE(zerocoin_denom_tests)

//translation from tradetensor quantity to zerocoin denomination
BOOST_AUTO_TEST_CASE(amount_to_denomination_test)
{
    cout << "Running amount_to_denomination_test...\n";

    //valid amount (min edge)
    CAmount amount = 1 * COIN;
    BOOST_CHECK_MESSAGE(AmountToZerocoinDenomination(amount) == ZQ_ONE, "For COIN denomination should be ZQ_ONE");

    //valid amount (max edge)
    CAmount amount1 = 5000 * COIN;
    BOOST_CHECK_MESSAGE(AmountToZerocoinDenomination(amount1) == ZQ_FIVE_THOUSAND, "For 5000*COIN denomination should be ZQ_ONE");

    //invalid amount (too much)
    CAmount amount2 = 7000 * COIN;
    BOOST_CHECK_MESSAGE(AmountToZerocoinDenomination(amount2) == ZQ_ERROR, "For 7000*COIN denomination should be Invalid -> ZQ_ERROR");

    //invalid amount (not enough)
    CAmount amount3 = 1;
    BOOST_CHECK_MESSAGE(AmountToZerocoinDenomination(amount3) == ZQ_ERROR, "For 1 denomination should be Invalid -> ZQ_ERROR");
}

BOOST_AUTO_TEST_CASE(denomination_to_value_test)
{
    cout << "Running ZerocoinDenominationToValue_test...\n";

    int64_t Value = 1 * COIN;
    CoinDenomination denomination = ZQ_ONE;
    BOOST_CHECK_MESSAGE(ZerocoinDenominationToAmount(denomination) == Value, "Wrong Value - should be 1");

    Value = 10 * COIN;
    denomination = ZQ_TEN;
    BOOST_CHECK_MESSAGE(ZerocoinDenominationToAmount(denomination) == Value, "Wrong Value - should be 10");

    Value = 50 * COIN;
    denomination = ZQ_FIFTY;
    BOOST_CHECK_MESSAGE(ZerocoinDenominationToAmount(denomination) == Value, "Wrong Value - should be 50");

    Value = 500 * COIN;
    denomination = ZQ_FIVE_HUNDRED;
    BOOST_CHECK_MESSAGE(ZerocoinDenominationToAmount(denomination) == Value, "Wrong Value - should be 500");

    Value = 100 * COIN;
    denomination = ZQ_ONE_HUNDRED;
    BOOST_CHECK_MESSAGE(ZerocoinDenominationToAmount(denomination) == Value, "Wrong Value - should be 100");

    Value = 0 * COIN;
    denomination = ZQ_ERROR;
    BOOST_CHECK_MESSAGE(ZerocoinDenominationToAmount(denomination) == Value, "Wrong Value - should be 0");
}

BOOST_AUTO_TEST_CASE(zerocoin_spend_test241)
{
    const int nMaxNumberOfSpends = 4;
    const bool fMinimizeChange = false;
    const int DenomAmounts[] = {1, 2, 3, 4, 0, 0, 0, 0};
    CAmount nSelectedValue;
    std::list<CZerocoinMint> listMints;
    std::map<CoinDenomination, CAmount> mapDenom;

    int j = 0;
    CAmount nTotalAmount = 0;
    int CoinsHeld = 0;

    // Create a set of Minted coins that fits profile given by DenomAmounts
    // Also setup Map array corresponding to DenomAmount which is the current set of coins available

    for (const auto& denom : zerocoinDenomList) {
        for (int i = 0; i < DenomAmounts[j]; i++) {
            CAmount currentAmount = ZerocoinDenominationToAmount(denom);
            nTotalAmount += currentAmount;
            CBigNum value;
            CBigNum rand;
            CBigNum serial;
            bool isUsed = false;
            CZerocoinMint mint(denom, value, rand, serial, isUsed);
            listMints.push_back(mint);
        }
        mapDenom.insert(std::pair<CoinDenomination, CAmount>(denom, DenomAmounts[j]));
        j++;
    }
    CoinsHeld = nTotalAmount / COIN;
    std::cout << "Curremt Amount held = " << CoinsHeld << ": ";

    // Show what we have
    j = 0;
    for (const auto& denom : zerocoinDenomList)
        std::cout << DenomAmounts[j++] << "*" << ZerocoinDenominationToAmount(denom) / COIN << " + ";
    std::cout << "\n";

    // For DenomAmounts[] = {1,2,3,4,0,0,0,0}; we can spend up to 200 without requiring more than 4 Spends
    // Amounts above this can not be met
    CAmount MaxLimit = 200;
    CAmount OneCoinAmount = ZerocoinDenominationToAmount(ZQ_ONE);
    CAmount nValueTarget = OneCoinAmount;
    int nCoinsReturned;
    int nNeededSpends = 0;  // Number of spends which would be needed if selection failed

    bool fDebug = 0;

    // Go through all possible spend between 1 and 241 and see if it's possible or not
    for (int i = 0; i < CoinsHeld; i++) {
        std::vector<CZerocoinMint> vSpends = SelectMintsFromList(nValueTarget, nSelectedValue,
                                                                 nMaxNumberOfSpends,
                                                                 fMinimizeChange,
                                                                 nCoinsReturned,
                                                                 listMints,
                                                                 mapDenom,
                                                                 nNeededSpends);

        if (fDebug) {
            if (vSpends.size() > 0) {
                std::cout << "SUCCESS : Coins = " << nValueTarget / COIN << " # spends used = " << vSpends.size()
                << " # of coins returned = " << nCoinsReturned
                          << " Spend Amount = " << nSelectedValue / COIN << " Held = " << CoinsHeld << "\n";
            } else {
                std::cout << "FAILED : Coins = " << nValueTarget / COIN << " Held = " << CoinsHeld << "\n";
            }
        }

        if (i < MaxLimit) {
            BOOST_CHECK_MESSAGE(vSpends.size() < 5, "Too many spends");
            BOOST_CHECK_MESSAGE(vSpends.size() > 0, "No spends");
        } else {
            bool spends_not_ok = ((vSpends.size() >= 4) || (vSpends.size() == 0));
            BOOST_CHECK_MESSAGE(spends_not_ok, "Expected to fail but didn't");
        }
        nValueTarget += OneCoinAmount;
    }
    //std::cout << "241 Test done!\n";
}
BOOST_AUTO_TEST_CASE(zerocoin_spend_test115)
{
    const int nMaxNumberOfSpends = 4;
    const bool fMinimizeChange = false;
    const int DenomAmounts[] = {0, 1, 1, 2, 0, 0, 0, 0};
    CAmount nSelectedValue;
    std::list<CZerocoinMint> listMints;
    std::map<CoinDenomination, CAmount> mapDenom;

    int j = 0;
    CAmount nTotalAmount = 0;
    int CoinsHeld = 0;

    // Create a set of Minted coins that fits profile given by DenomAmounts
    // Also setup Map array corresponding to DenomAmount which is the current set of coins available
    for (const auto& denom : zerocoinDenomList) {
        for (int i = 0; i < DenomAmounts[j]; i++) {
            CAmount currentAmount = ZerocoinDenominationToAmount(denom);
            nTotalAmount += currentAmount;
            CBigNum value;
            CBigNum rand;
            CBigNum serial;
            bool isUsed = false;
            CZerocoinMint mint(denom, value, rand, serial, isUsed);
            listMints.push_back(mint);
        }
        mapDenom.insert(std::pair<CoinDenomination, CAmount>(denom, DenomAmounts[j]));
        j++;
    }
    CoinsHeld = nTotalAmount / COIN;
    std::cout << "Curremt Amount held = " << CoinsHeld << ": ";

    // Show what we have
    j = 0;
    for (const auto& denom : zerocoinDenomList)
        std::cout << DenomAmounts[j++] << "*" << ZerocoinDenominationToAmount(denom) / COIN << " + ";
    std::cout << "\n";

    CAmount OneCoinAmount = ZerocoinDenominationToAmount(ZQ_ONE);
    CAmount nValueTarget = OneCoinAmount;

    bool fDebug = 0;
    int nCoinsReturned;
    int nNeededSpends = 0;  // Number of spends which would be needed if selection failed

    std::vector<CZerocoinMint> vSpends = SelectMintsFromList(nValueTarget, nSelectedValue,
                                                             nMaxNumberOfSpends,
                                                             fMinimizeChange,
                                                             nCoinsReturned,
                                                             listMints,
                                                             mapDenom,
                                                             nNeededSpends);

    if (fDebug) {
        if (vSpends.size() > 0) {
            std::cout << "SUCCESS : Coins = " << nValueTarget / COIN << " # spends used = " << vSpends.size()
            << " # of coins returned = " << nCoinsReturned
                      << " Spend Amount = " << nSelectedValue / COIN << " Held = " << CoinsHeld << "\n";
        } else {
            std::cout << "FAILED : Coins = " << nValueTarget / COIN << " Held = " << CoinsHeld << "\n";
        }
    }

    BOOST_CHECK_MESSAGE(vSpends.size() < 5, "Too many spends");
    BOOST_CHECK_MESSAGE(vSpends.size() > 0, "No spends");
    nValueTarget += OneCoinAmount;
}
BOOST_AUTO_TEST_CASE(zerocoin_spend_test_from_245)
{
    const int nMaxNumberOfSpends = 5;
    // For 36:
    // const int nSpendValue = 36;
    // Here have a 50 so use for 36 since can't meet exact amount
    //    const int DenomAmounts[] = {0,1,4,1,0,0,0,0};
    // Here have 45 so use 4*10 for 36 since can't meet exact amount
    //    const int DenomAmounts[] = {0, 1, 4, 0, 0, 0, 0, 0};
    // For 51
    //const int nSpendValue = 51;

    // CoinsHeld = 245
    const int DenomAmounts[] = {0, 1, 4, 2, 1, 0, 0, 0};
    // We can spend up to this amount for above set for less 6 spends
    // Otherwise, 6 spends are required
    const int nMaxSpendAmount = 220;
    CAmount nSelectedValue;
    std::list<CZerocoinMint> listMints;
    std::map<CoinDenomination, CAmount> mapOfDenomsHeld;

    int j = 0;
    CAmount nTotalAmount = 0;
    int CoinsHeld = 0;

    // Create a set of Minted coins that fits profile given by DenomAmounts
    // Also setup Map array corresponding to DenomAmount which is the current set of coins available
    for (const auto& denom : zerocoinDenomList) {
        for (int i = 0; i < DenomAmounts[j]; i++) {
            CAmount currentAmount = ZerocoinDenominationToAmount(denom);
            nTotalAmount += currentAmount;
            CBigNum value;
            CBigNum rand;
            CBigNum serial;
            bool isUsed = false;
            CZerocoinMint mint(denom, value, rand, serial, isUsed);
            listMints.push_back(mint);
        }
        mapOfDenomsHeld.insert(std::pair<CoinDenomination, CAmount>(denom, DenomAmounts[j]));
        j++;
    }
    CoinsHeld = nTotalAmount / COIN;
    std::cout << "Curremt Amount held = " << CoinsHeld << ": ";

    // Show what we have
    j = 0;
    for (const auto& denom : zerocoinDenomList)
        std::cout << DenomAmounts[j++] << "*" << ZerocoinDenominationToAmount(denom) / COIN << " + ";
    std::cout << "\n";

    CAmount OneCoinAmount = ZerocoinDenominationToAmount(ZQ_ONE);
    CAmount nValueTarget = OneCoinAmount;

    bool fDebug = 0;
    int nCoinsReturned;
    int nNeededSpends = 0;  // Number of spends which would be needed if selection failed

    // Go through all possible spend between 1 and 241 and see if it's possible or not
    for (int i = 0; i < CoinsHeld; i++) {
        std::vector<CZerocoinMint> vSpends = SelectMintsFromList(nValueTarget, nSelectedValue,
                                                                 nMaxNumberOfSpends,
                                                                 false,
                                                                 nCoinsReturned,
                                                                 listMints,
                                                                 mapOfDenomsHeld,
                                                                 nNeededSpends);

        if (fDebug) {
            if (vSpends.size() > 0) {
                std::cout << "SUCCESS : Coins = " << nValueTarget / COIN << " # spends = " << vSpends.size()
                          << " # coins returned = " << nCoinsReturned
                          << " Amount = " << nSelectedValue / COIN << " Held = " << CoinsHeld << " ";
            } else {
                std::cout << "UNABLE TO SPEND : Coins = " << nValueTarget / COIN << " Held = " << CoinsHeld << "\n";
            }
        }

        bool spends_not_ok = ((vSpends.size() > nMaxNumberOfSpends) || (vSpends.size() == 0));
        if (i < nMaxSpendAmount) BOOST_CHECK_MESSAGE(!spends_not_ok, "Too many spends");
        else BOOST_CHECK_MESSAGE(spends_not_ok, "Expected to fail but didn't");

        std::vector<CZerocoinMint> vSpendsAlt = SelectMintsFromList(nValueTarget, nSelectedValue,
                                                                    nMaxNumberOfSpends,
                                                                    true,
                                                                    nCoinsReturned,
                                                                    listMints,
                                                                    mapOfDenomsHeld,
                                                                    nNeededSpends);

        if (fDebug) {
            if (vSpendsAlt.size() > 0) {
                std::cout << "# spends = " << vSpendsAlt.size()
                          << " # coins returned = " << nCoinsReturned
                          << " Amount = " << nSelectedValue / COIN << "\n";
            } else {
                std::cout << "UNABLE TO SPEND : Coins = " << nValueTarget / COIN << " Held = " << CoinsHeld << "\n";
            }
        }

        spends_not_ok = ((vSpendsAlt.size() > nMaxNumberOfSpends) || (vSpendsAlt.size() == 0));
        if (i < nMaxSpendAmount) BOOST_CHECK_MESSAGE(!spends_not_ok, "Too many spends");
        else BOOST_CHECK_MESSAGE(spends_not_ok, "Expected to fail but didn't");

        nValueTarget += OneCoinAmount;
    }
}

BOOST_AUTO_TEST_CASE(zerocoin_spend_test_from_145)
{
    const int nMaxNumberOfSpends = 5;
    // CoinsHeld = 145
    const int DenomAmounts[] = {0, 1, 4, 2, 0, 0, 0, 0};
    CAmount nSelectedValue;
    std::list<CZerocoinMint> listMints;
    std::map<CoinDenomination, CAmount> mapOfDenomsHeld;

    int j = 0;
    CAmount nTotalAmount = 0;
    int CoinsHeld = 0;

    // Create a set of Minted coins that fits profile given by DenomAmounts
    // Also setup Map array corresponding to DenomAmount which is the current set of coins available
    for (const auto& denom : zerocoinDenomList) {
        for (int i = 0; i < DenomAmounts[j]; i++) {
            CAmount currentAmount = ZerocoinDenominationToAmount(denom);
            nTotalAmount += currentAmount;
            CBigNum value;
            CBigNum rand;
            CBigNum serial;
            bool isUsed = false;
            CZerocoinMint mint(denom, value, rand, serial, isUsed);
            listMints.push_back(mint);
        }
        mapOfDenomsHeld.insert(std::pair<CoinDenomination, CAmount>(denom, DenomAmounts[j]));
        j++;
    }
    CoinsHeld = nTotalAmount / COIN;
    std::cout << "Curremt Amount held = " << CoinsHeld << ": ";
    // We can spend up to this amount for above set for less 6 spends
    // Otherwise, 6 spends are required
    const int nMaxSpendAmount = 130;

    // Show what we have
    j = 0;
    for (const auto& denom : zerocoinDenomList)
        std::cout << DenomAmounts[j++] << "*" << ZerocoinDenominationToAmount(denom) / COIN << " + ";
    std::cout << "\n";

    CAmount OneCoinAmount = ZerocoinDenominationToAmount(ZQ_ONE);
    CAmount nValueTarget = OneCoinAmount;

    bool fDebug = 0;
    int nCoinsReturned;
    int nNeededSpends = 0;  // Number of spends which would be needed if selection failed

    // Go through all possible spend between 1 and 241 and see if it's possible or not
    for (int i = 0; i < CoinsHeld; i++) {
        std::vector<CZerocoinMint> vSpends = SelectMintsFromList(nValueTarget, nSelectedValue,
                                                                 nMaxNumberOfSpends,
                                                                 false,
                                                                 nCoinsReturned,
                                                                 listMints,
                                                                 mapOfDenomsHeld,
                                                                 nNeededSpends);

        if (fDebug) {
            if (vSpends.size() > 0) {
                std::cout << "SUCCESS : Coins = " << nValueTarget / COIN << " # spends = " << vSpends.size()
                          << " # coins returned = " << nCoinsReturned
                          << " Amount = " << nSelectedValue / COIN << " Held = " << CoinsHeld << " ";
            } else {
                std::cout << "UNABLE TO SPEND : Coins = " << nValueTarget / COIN << " Held = " << CoinsHeld << "\n";
            }
        }

        bool spends_not_ok = ((vSpends.size() > nMaxNumberOfSpends) || (vSpends.size() == 0));
        if (i < nMaxSpendAmount) BOOST_CHECK_MESSAGE(!spends_not_ok, "Too many spends");
        else BOOST_CHECK_MESSAGE(spends_not_ok, "Expected to fail but didn't");

        std::vector<CZerocoinMint> vSpendsAlt = SelectMintsFromList(nValueTarget, nSelectedValue,
                                                                    nMaxNumberOfSpends,
                                                                    true,
                                                                    nCoinsReturned,
                                                                    listMints,
                                                                    mapOfDenomsHeld,
                                                                    nNeededSpends);

        if (fDebug) {
            if (vSpendsAlt.size() > 0) {
                std::cout << "# spends = " << vSpendsAlt.size()
                          << " # coins returned = " << nCoinsReturned
                          << " Amount = " << nSelectedValue / COIN << "\n";
            } else {
                std::cout << "UNABLE TO SPEND : Coins = " << nValueTarget / COIN << " Held = " << CoinsHeld << "\n";
            }
        }

        spends_not_ok = ((vSpendsAlt.size() > nMaxNumberOfSpends) || (vSpendsAlt.size() == 0));
        if (i < nMaxSpendAmount) BOOST_CHECK_MESSAGE(!spends_not_ok, "Too many spends");
        else BOOST_CHECK_MESSAGE(spends_not_ok, "Expected to fail but didn't");

        nValueTarget += OneCoinAmount;
    }
}

BOOST_AUTO_TEST_CASE(zerocoin_spend_test99)
{
    const int nMaxNumberOfSpends = 4;
    const bool fMinimizeChange = false;
    const int DenomAmounts[] = {0, 1, 4, 2, 1, 0, 0, 0};
    CAmount nSelectedValue;
    std::list<CZerocoinMint> listMints;
    std::map<CoinDenomination, CAmount> mapOfDenomsHeld;

    int j = 0;
    CAmount nTotalAmount = 0;
    int CoinsHeld = 0;

    // Create a set of Minted coins that fits profile given by DenomAmounts
    // Also setup Map array corresponding to DenomAmount which is the current set of coins available
    for (const auto& denom : zerocoinDenomList) {
        for (int i = 0; i < DenomAmounts[j]; i++) {
            CAmount currentAmount = ZerocoinDenominationToAmount(denom);
            nTotalAmount += currentAmount;
            CBigNum value;
            CBigNum rand;
            CBigNum serial;
            bool isUsed = false;
            CZerocoinMint mint(denom, value, rand, serial, isUsed);
            listMints.push_back(mint);
        }
        mapOfDenomsHeld.insert(std::pair<CoinDenomination, CAmount>(denom, DenomAmounts[j]));
        j++;
    }
    CoinsHeld = nTotalAmount / COIN;
    std::cout << "Curremt Amount held = " << CoinsHeld << ": ";

    // Show what we have
    j = 0;
    for (const auto& denom : zerocoinDenomList)
        std::cout << DenomAmounts[j++] << "*" << ZerocoinDenominationToAmount(denom) / COIN << " + ";
    std::cout << "\n";

    CAmount OneCoinAmount = ZerocoinDenominationToAmount(ZQ_ONE);
    CAmount nValueTarget = 99 * OneCoinAmount;

    bool fDebug = 0;
    int nCoinsReturned;
    int nNeededSpends = 0;  // Number of spends which would be needed if selection failed

    std::vector<CZerocoinMint> vSpends = SelectMintsFromList(nValueTarget, nSelectedValue,
                                                             nMaxNumberOfSpends,
                                                             fMinimizeChange,
                                                             nCoinsReturned,
                                                            listMints,
                                                             mapOfDenomsHeld,
                                                             nNeededSpends);

    if (fDebug) {
        if (vSpends.size() > 0) {
            std::cout << "SUCCESS : Coins = " << nValueTarget / COIN << " # spends used = " << vSpends.size()
            << " # of coins returned = " << nCoinsReturned
                      << " Spend Amount = " << nSelectedValue / COIN << " Held = " << CoinsHeld << "\n";
        } else {
            std::cout << "FAILED : Coins = " << nValueTarget / COIN << " Held = " << CoinsHeld << "\n";
        }
    }

    BOOST_CHECK_MESSAGE(vSpends.size() < 5, "Too many spends");
    BOOST_CHECK_MESSAGE(vSpends.size() > 0, "No spends");
    nValueTarget += OneCoinAmount;
}

BOOST_AUTO_TEST_SUITE_END()


//
// Created by huangkun on 2020/11/3.
//

#include <fstream>

#include <Eigen/Eigen>
#include <opencv2/core/eigen.hpp>

#include <opengv2/frame/Bodyframe.hpp>
#include <opengv2/system/SystemBase.hpp>
#include <opengv2/viewer/PointLandmarkViewer.hpp>

using namespace opengv2;
using namespace std;

void
loadGT(const string &file, long long baseTime, double shiftTime, std::map<double, Eigen::Matrix<double, 7, 1>> &gTwb);

void calibrateHandEye(const std::map<double, Eigen::Matrix<double, 7, 1>> &Tws,
                      const std::map<double, Eigen::Matrix<double, 7, 1>> &Tw1b,
                      Eigen::Quaterniond &unitQbs, Eigen::Vector3d &tbs);

int main(int argc, char **argv) {
    if (argc != 7) {
        cerr << endl
             << "Usage: ./handEyeCalib settingFilePath GTFilePath baseTime EventTrajectoryFile cvTrajectoryFile SavePath"
             << endl;
        return 1;
    }

    //Check settings file
    cv::FileStorage fsSettings(string(argv[1]), cv::FileStorage::READ);
    if (!fsSettings.isOpened()) {
        cerr << "Failed to open settings file at: " << string(argv[1]) << endl;
        exit(-1);
    }

    // map part
    MapBase::Ptr map = make_shared<MapBase>();
    // viewer part
    PointLandmarkViewer::Ptr viewer = make_shared<PointLandmarkViewer>(string(argv[1]), map, nullptr);
    // Extrinsic parameters, to be calibrated
    std::vector<Eigen::Quaterniond, Eigen::aligned_allocator<Eigen::Quaterniond>> Qsb;
    std::vector<Eigen::Vector3d> tsb;
    Qsb.emplace_back(1, 0, 0, 0);
    tsb.emplace_back(0, 0, 0);

    SystemBase SLAM(nullptr, map, viewer, nullptr, Qsb, tsb, nullptr, nullptr, string(argv[1]));

    // add landmarks
    bool isAsymmetric = true;
    fsSettings["Is_Pattern_Asymmetric"] >> isAsymmetric;
    int rows = fsSettings["BoardSize_Rows"];
    int cols = fsSettings["BoardSize_Cols"];
    double squareSize = fsSettings["Square_Size"];
    if (isAsymmetric) {
        for (int i = 0; i < rows; i++)
            for (int j = 0; j < cols; j++) {
                auto lm = std::make_shared<LandmarkBase>
                        (j + i * cols, Eigen::Vector3d((2 * j + i % 2) * squareSize, i * squareSize, 0));
                SLAM.map->addLandmark(lm);
            }
    } else {
        for (int i = 0; i < rows; ++i)
            for (int j = 0; j < cols; ++j) {
                auto lm = std::make_shared<LandmarkBase>
                        (j + i * cols, Eigen::Vector3d(j * squareSize, i * squareSize, 0));
                SLAM.map->addLandmark(lm);
            }
    }

    // load GT, EventTrajectory cvTrajectory
    std::map<double, Eigen::Matrix<double, 7, 1>> GT_Twb, event_Tws, cv_Tws;
    double shiftTime = fsSettings["shiftTime"];
    loadGT(string(argv[2]), stoll(argv[3]), shiftTime, GT_Twb);
    opengv2::SystemBase::loadTUM(string(argv[4]), event_Tws);
    opengv2::SystemBase::loadTUM(string(argv[5]), cv_Tws);
    std::cout << event_Tws.size() << std::endl;
    std::cout << cv_Tws.size() << std::endl;

    // hand-eye calibration
    bool calculateTbs = true;
    fsSettings["calculateTbs"] >> calculateTbs;
    Eigen::Quaterniond unitQbs;
    Eigen::Vector3d tbs;
    if (calculateTbs) {
        std::cout << "calibrateHandEye for Event" << std::endl;
        Eigen::Quaterniond event_unitQbs;
        Eigen::Vector3d event_tbs;
        calibrateHandEye(event_Tws, GT_Twb, event_unitQbs, event_tbs);
        Eigen::Quaterniond cv_unitQbs;
        Eigen::Vector3d cv_tbs;
        if (cv_Tws.size() >= 4) {
            std::cout << "calibrateHandEye for CV" << std::endl;
            calibrateHandEye(cv_Tws, GT_Twb, cv_unitQbs, cv_tbs);
        }

        bool chooseCV = false;//event_tbs.norm() > cv_tbs.norm();
        std::cout << "Choose: " << (chooseCV ? "CV hand-eye result." : "Event hand-eye result.")
                  << std::endl;
        if (chooseCV) {
            unitQbs = cv_unitQbs;
            tbs = cv_tbs;
        } else {
            unitQbs = event_unitQbs;
            tbs = event_tbs;
        }
    } else {
        cv::FileNode data = fsSettings["Qbs"];
        std::vector<double> v;
        for (auto &&itData : data) {
            v.push_back(itData);
        }
        unitQbs = Eigen::Map<Eigen::Quaterniond>(v.data());
        unitQbs.normalize();

        data = fsSettings["tbs"];
        v.clear();
        for (auto &&itData : data) {
            v.push_back(itData);
        }
        tbs = Eigen::Map<Eigen::Vector3d>(v.data());
    }

    // save GT
    std::ofstream f(string(argv[6]) + "/gtTrajectory.txt", std::ofstream::trunc);
    f << std::fixed << std::setprecision(10);
    if (!f.is_open())
        throw std::logic_error("Unable to open files: " + string(argv[6]) + "/gtTrajectory.txt");
    for (auto &itr: GT_Twb) {//qw qx qy qz tx ty tz
        Eigen::Map<const Eigen::Quaterniond> unitQw1b(itr.second.data());
        Eigen::Map<const Eigen::Vector3d> tw1b(itr.second.data() + 4);

        Eigen::Quaterniond unitQw1s = unitQw1b * unitQbs;
        Eigen::Vector3d tw1s = unitQw1b * tbs + tw1b;
        itr.second.head<4>() = unitQw1s.coeffs();
        itr.second.tail<3>() = tw1s;

        //timestamp tx ty tz qx qy qz qw
        f << itr.first << " " << tw1s[0] << " " << tw1s[1] << " " << tw1s[2] << " "
          << unitQw1s.x() << " " << unitQw1s.y() << " " << unitQw1s.z() << " " << unitQw1s.w() << std::endl;
    }
    f.close();

    // TODO: add trajectory to map.
    for (const auto &itr: event_Tws) {
        Eigen::Quaterniond unitQws(itr.second.data());
        Eigen::Vector3d tws(itr.second.data() + 4);

        vector<FrameBase::Ptr> frames(1);
        auto bf = std::make_shared<Bodyframe>(frames, itr.first, tws, unitQws);
        SLAM.map->addFrame(bf);
    }
    SLAM.loadGT(0, GT_Twb);

    std::cout << "press any key to exit..." << std::endl;
    std::cin.ignore();
    SLAM.shutdown();
    return 0;
}

void calibrateHandEye(const std::map<double, Eigen::Matrix<double, 7, 1>> &Tws,
                      const std::map<double, Eigen::Matrix<double, 7, 1>> &Tw1b,
                      Eigen::Quaterniond &unitQbs, Eigen::Vector3d &tbs) {
    std::vector<cv::Mat> Rw1bSet, tw1bSet, RswSet, tswSet;
    for (const auto &itr: Tws) {
        // Tbw1Set
        auto n1 = Tw1b.lower_bound(itr.first);
        if (n1 == Tw1b.end()) {
            n1--;
        } else if (n1 != Tw1b.begin()) {
            auto n2 = n1--;
            n1 = std::abs(itr.first - n1->first) > std::abs(itr.first - n2->first) ? n2 : n1;
        }

        auto itr1 = Tws.lower_bound(n1->first);
        if (itr1 == Tws.end()) {
            itr1--;
        } else if (itr1 != Tws.begin()) {
            auto itr2 = itr1--;
            itr1 = std::abs(n1->first - itr1->first) > std::abs(n1->first - itr2->first) ? itr2 : itr1;
        }

        if (itr1->first != itr.first || std::abs(n1->first - itr.first) > 3e-2)
            continue;

        Eigen::Quaterniond unitQw1b(n1->second.data());
        Eigen::Vector3d tw1b(n1->second.data() + 4);
        cv::Mat cv_Rw1b, cv_tw1b;
        cv::eigen2cv(unitQw1b.toRotationMatrix(), cv_Rw1b);
        cv::eigen2cv(tw1b, cv_tw1b);
        Rw1bSet.push_back(cv_Rw1b);
        tw1bSet.push_back(cv_tw1b);

        // TswSet
        Eigen::Quaterniond unitQws(itr.second.data());
        Eigen::Vector3d tws(itr.second.data() + 4);
        cv::Mat cv_Rsw, cv_tsw;
        cv::eigen2cv(unitQws.conjugate().toRotationMatrix(), cv_Rsw);
        cv::eigen2cv(Eigen::Vector3d(unitQws.conjugate() * (-tws)), cv_tsw);
        RswSet.push_back(cv_Rsw);
        tswSet.push_back(cv_tsw);
    }

    cv::Mat cv_Rbs, cv_tbs;
    cv::calibrateHandEye(Rw1bSet, tw1bSet, RswSet, tswSet, cv_Rbs, cv_tbs,
                         cv::CALIB_HAND_EYE_PARK /*CALIB_HAND_EYE_HORAUD*/);
    Eigen::Matrix3d Rbs;
    cv::cv2eigen(cv_Rbs, Rbs);
    cv::cv2eigen(cv_tbs, tbs);
    unitQbs = Rbs;
    unitQbs.normalize();

    std::cout.precision(10);
    std::cout << "Estimated extrinsic parameter: " << std::endl <<
              unitQbs.coeffs().transpose() << std::endl << tbs.transpose() << std::endl;
}

void
loadGT(const string &file, long long baseTime, double shiftTime, std::map<double, Eigen::Matrix<double, 7, 1>> &gTwb) {
    //GT format:timestamp qx qy qz qw tx ty tz. diff from TUM
    std::ifstream fInput(file, std::ifstream::in);

    std::vector<double> T(8);
    long long time;
    char line[512];
    fInput.getline(line, 512);
    while (!fInput.eof() && !fInput.fail()) {
        std::stringstream ss(line, std::stringstream::in);
        ss >> time;
        T[0] = (time - baseTime) * 1e-6 + shiftTime;
        for (int i = 1; i < 8; ++i) {
            ss >> T[i];
        }

        Eigen::Quaterniond Qwb(T.data() + 1);
        Qwb.normalize(); // normalize for further usage
        Eigen::Matrix<double, 7, 1> Twb;
        Twb.head(4) = Qwb.coeffs();
        Twb.tail(3) << 100 * T[5], 100 * T[6], 100 * T[7]; // to cm
        gTwb[T[0]] = std::move(Twb);

        fInput.getline(line, 512);
    }
}


//////////////////////////////////////////////////////////////////////////////////
// distribution::toolkit::meta::result_of_support.hpp              			    //
//                                                                              //
//  (C) Copyright 2009 Erwann Rogard                                            //
//  Use, modification and distribution are subject to the                       //
//  Boost Software License, Version 1.0. (See accompanying file                 //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)            //
//////////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_DISTRIBUTION_TOOLKIT_META_RESULT_OF_SUPPORT_HPP_ER_2009
#define BOOST_DISTRIBUTION_TOOLKIT_META_RESULT_OF_SUPPORT_HPP_ER_2009
#include <utility>
#include <boost/statistics/detail/distribution_common/meta/value.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace distribution{
namespace common{
namespace meta{

	template<typename D>
    struct result_of_support
    {
    	typedef typename distribution::common::meta::value<D>::type val_;
        typedef std::pair<val_,val_> type;
    }

}// meta
}// toolkit
}// distribution
}// detail
}// statistics
}// boost

#endif


#include <iostream>
#include <fstream>

#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include <imgproc/derivative_gradient.hpp>
#include <imgproc/threshold.hpp>
#include <edge/nms.hpp>

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

using namespace lsfm;
using namespace std;
namespace fs = boost::filesystem;

constexpr int runs = 1;

typedef double FT;

struct Entry {
    Entry() {}

    Entry(const cv::Ptr<FilterI<uchar>>& a, const cv::Ptr<Threshold<FT>> &t, const std::string& b, int f = 0)
        : filter(a), threshold(t), name(b), flags(f), images(0), time(0) {}

    cv::Ptr<FilterI<uchar>> filter;
    cv::Ptr<Threshold<FT>> threshold;
    std::string name;
    int flags, images;
    int64 time;

    inline FilterResults processFilter(const cv::Mat src) {
        filter->process(src);
        return filter->results();
    }

    inline cv::Mat processThreshold(const cv::Mat src) {
        int64 start = cv::getTickCount();
        cv::Mat ret = threshold->process(src);
        time += cv::getTickCount() - start;
        ++images;
        return ret;
    }
};

cv::Mat createNMS(const cv::Mat &emap) {

    cv::Mat emapImg;
    emapImg.create(emap.rows, emap.cols, CV_8UC3);

    emapImg.setTo(cv::Vec3b(0, 0, 0));
    emapImg.setTo(cv::Vec3b(220, 150, 255), emap == 7); // magenta2
    emapImg.setTo(cv::Vec3b(255, 0, 150), emap == 6); // lila
    emapImg.setTo(cv::Vec3b(255, 0, 0), emap == 5); // blue
    emapImg.setTo(cv::Vec3b(255, 255, 0), emap == 4); // cyan
    emapImg.setTo(cv::Vec3b(0, 255, 0), emap == 3); // green
    emapImg.setTo(cv::Vec3b(0, 255, 255), emap == 2); // yellow
    emapImg.setTo(cv::Vec3b(0, 150, 255), emap == 1); // orange
    emapImg.setTo(cv::Vec3b(0, 0, 255), emap == 0); // red
    return emapImg;
}

inline cv::Mat normalize(const cv::Mat &in) {
    cv::Mat cpy;
    if (in.type() != CV_32F || in.type() != CV_64F)
        in.convertTo(cpy, CV_32F);
    else
        in.copyTo(cpy);

    double vmin, vmax;
    cv::minMaxIdx(cpy, &vmin, &vmax);
    cpy *= 255.0 / vmax;
    cpy.convertTo(cpy, CV_8U);
    return cpy;
}

void saveEdge(const cv::Mat &data, const std::string &name) {
    cv::imwrite("./threshold/" + name + ".png", createNMS(data));
}

void saveTh(const cv::Mat &data, const std::string &name) {
    cv::imwrite("./threshold/" + name + ".png", normalize(data));
}

void saveResults(FilterResults &f, const cv::Mat& t, const std::string &name) {
    NonMaximaSuppression<short, FT, FT> nms;
    cv::Mat high = t.clone();
    cv::GaussianBlur(high, high, cv::Size(0, 0),15);
    cv::Mat low = high * 0.5;
    // set fixed lower threshold
    low.setTo(f["mag"].range.upper * 0.004, low < f["mag"].range.upper * 0.004);
    nms.process(f["gx"].data, f["gy"].data, f["mag"].data, low, high);
    saveEdge(nms.hysteresis(), name + "_mag");
    saveTh(high, name + "_th");
}

void parseFolder(const fs::path &folder, std::vector<fs::path> &files) {
    fs::directory_iterator end_iter;
    for_each(fs::directory_iterator(folder), fs::directory_iterator(), [&files](const fs::path& file) {
        if (fs::is_regular_file(file))
        {
            std::string ext = file.extension().generic_string();
            boost::algorithm::to_lower(ext);
            if (ext == ".jpg" || ext == ".png") {
                files.push_back(file);
            }
        }
        if (fs::is_directory(file))
            parseFolder(file, files);
    });
}

void processPath(std::vector<Entry> &entries, const std::pair<fs::path, std::string> &path) {
    std::cout << "processing " << path.first << std::endl;
    std::vector<fs::path> files;
    parseFolder(path.first, files);

    std::for_each(files.begin(), files.end(), [&](const fs::path& file) {
        cv::Mat src = cv::imread(file.generic_string());
        if (src.empty())
        {
            cout << "Can not open " << file.generic_string() << endl;
            return;
        }
        std::cout << file << std::endl;
        cv::cvtColor(src, src, CV_BGR2GRAY);
        cv::GaussianBlur(src, src, cv::Size(3, 3), 0.6);

        for_each(entries.begin(), entries.end(), [&](Entry &e) {
            FilterResults fs = e.processFilter(src);
            saveResults(fs, e.processThreshold(fs["mag"].data), path.second + "/" + file.stem().generic_string() + "_" + e.name);
        });
    });
}

int main(int argc, char** argv)
{
    std::vector<std::pair<fs::path, std::string>> sets;
    //sets.push_back(std::pair<fs::path, std::string>("../../images/noise", "noise"));
    sets.push_back(std::pair<fs::path, std::string>("../../images/Selection", "Selection"));
    //sets.push_back(std::pair<fs::path, std::string>("../../images/BSDS500", "BSDS500"));
    //sets.push_back(std::pair<fs::path, std::string>("../../images/MDB/MiddEval3-Q", "MDB-Q"));
    //sets.push_back(std::pair<fs::path, std::string>("../../images/MDB/MiddEval3-H", "MDB-H"));
    //sets.push_back(std::pair<fs::path, std::string>("../../images/MDB/MiddEval3-F", "MDB-F"));

    std::vector<Entry> filter;
    filter.push_back(Entry(new DerivativeGradient<uchar, short,FT, FT, SobelDerivative>, new GlobalThreshold<FT,ThresholdOtsu<FT,256>>(1141), "Otsu_G"));
    filter.push_back(Entry(new DerivativeGradient<uchar, short, FT, FT, SobelDerivative>, new LocalThresholdTiles<FT, ThresholdOtsu<FT, 256>>(3, 3, 1141), "Otsu_LTiles4"));
    filter.push_back(Entry(new DerivativeGradient<uchar, short, FT, FT, SobelDerivative>, new LocalThresholdTiles<FT, ThresholdOtsu<FT, 256>>(10, 10, 1141), "Otsu_LTiles10"));
    filter.push_back(Entry(new DerivativeGradient<uchar, short, FT, FT, SobelDerivative>, new LocalThreshold<FT, ThresholdOtsu<FT, 256>>(30,30,true,1141), "Otsu_LWindow"));
    filter.push_back(Entry(new DerivativeGradient<uchar, short, FT, FT, SobelDerivative>, new DynamicThreshold<FT, ThresholdOtsu<FT, 256>>(5, 1141), "Otsu_D"));

    fs::create_directory("./threshold");
    for_each(sets.begin(), sets.end(), [&](const std::pair<fs::path, std::string> &data) {
        fs::create_directory("./threshold/" + data.second);
        processPath(filter, data);
    });

    for_each(filter.begin(), filter.end(), [&](Entry &e) {
        std::cout << e.name << ": " << e.time * 1000 / (e.images * cv::getTickFrequency()) << "ms" << std::endl;
    });

    return 0;
}


// Copyright Aleksey Gurtovoy 2000-2004
// Copyright David Abrahams 2003-2004
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
// See http://www.boost.org/libs/mpl for documentation.

// $Id: transform.cpp 49268 2008-10-11 06:26:17Z agurtovoy $
// $Date: 2008-10-10 23:26:17 -0700 (Fri, 10 Oct 2008) $
// $Revision: 49268 $

#include <boost/mpl/transform.hpp>

#include <boost/mpl/list.hpp>
#include <boost/mpl/list_c.hpp>
#include <boost/mpl/equal.hpp>
#include <boost/mpl/equal_to.hpp>
#include <boost/mpl/plus.hpp>
#include <boost/mpl/aux_/test.hpp>
#include <boost/mpl/aux_/config/gcc.hpp>
#include <boost/mpl/aux_/config/workaround.hpp>

#include <boost/type_traits/add_pointer.hpp>

MPL_TEST_CASE()
{
    typedef list<char,short,int,long,float,double> types;
    typedef list<char*,short*,int*,long*,float*,double*> pointers;

    typedef transform1< types,add_pointer<_1> >::type result;
    MPL_ASSERT(( equal<result,pointers> ));
}

MPL_TEST_CASE()
{
    typedef list_c<long,0,2,4,6,8,10> evens;
    typedef list_c<long,2,3,5,7,11,13> primes;
    typedef list_c<long,2,5,9,13,19,23> sums;

    typedef transform2< evens, primes, plus<> >::type result;
    MPL_ASSERT(( equal< result,sums,equal_to<_1,_2> > ));

#if !defined(BOOST_MPL_CFG_NO_HAS_XXX)
    typedef transform< evens, primes, plus<> >::type result2;
    MPL_ASSERT(( is_same<result2,result> ));
#endif
}


// Copyright (c) 2018 Graphcore Ltd. All rights reserved.
#define BOOST_TEST_MODULE ReduceMaxClassGather
#include <poplar/Engine.hpp>
#include <poplibs_support/TestDevice.hpp>

#include "poplibs_support/Algorithm.hpp"
#include "poplibs_test/Util.hpp"
#include "popnn/codelets.hpp"
#include "poputil/VertexTemplates.hpp"

#include <boost/program_options.hpp>
#include <boost/random.hpp>
#include <cstdint>

using namespace poplar;
using namespace poplar::program;
using namespace poplibs_test::util;
using namespace poputil;
using namespace poplibs_support;

namespace {

static void modelVertex(const std::vector<double> &activations, unsigned index,
                        std::vector<double> &maxActs,
                        std::vector<std::uint64_t> &maxIndices, unsigned size,
                        unsigned divisor) {

  const auto nOutputs = (size + divisor - 1) / divisor;
  for (std::size_t i = 0; i < nOutputs; ++i) {
    unsigned maxLabel = i * divisor;
    double max = activations[maxLabel];
    const auto end = std::min(size, maxLabel + divisor);
    for (std::size_t j = maxLabel + 1; j < end; ++j) {
      if (activations[j] > max) {
        max = activations[j];
        maxLabel = j;
      }
    }
    maxActs[i] = max;
    maxIndices[i] = maxLabel + index;
  }
}

static bool doTest(const DeviceType &deviceType, const Type &activationsType,
                   const Type &labelType, unsigned divisor, unsigned size) {
  auto device = createTestDevice(deviceType);
  auto &target = device.getTarget();

  if ((divisor & (divisor - 1)) != 0) {
    throw std::logic_error("divisor is not a power of 2");
  }
  const auto nOutputs = (size + divisor - 1) / divisor;
  if (nOutputs > target.getNumWorkerContexts()) {
    throw std::logic_error(
        "Divisor is not large enough for the vertex to process all inputs");
  }

  Graph graph(target);
  popnn::addCodelets(graph);
  auto partialsType = (activationsType == HALF || activationsType == FLOAT)
                          ? FLOAT
                          : activationsType;

  auto activations = graph.addVariable(activationsType, {size}, "activations");
  auto maxActs =
      graph.addVariable(partialsType, {nOutputs}, "maxValuePartials");
  auto maxIndices =
      graph.addVariable(labelType, {nOutputs}, "maxIndexPartials");
  graph.setTileMapping(activations, 0);
  graph.setTileMapping(maxActs, 0);
  graph.setTileMapping(maxIndices, 0);

  Sequence uploadProg, downloadProg;
  std::vector<std::pair<std::string, char *>> tmap;
  auto rawHostActivations = allocateHostMemoryForTensor(
      activations, "activations", graph, uploadProg, downloadProg, tmap);
  auto rawHostMaxActs = allocateHostMemoryForTensor(
      maxActs, "maxValuePartials", graph, uploadProg, downloadProg, tmap);
  auto rawHostMaxIndices = allocateHostMemoryForTensor(
      maxIndices, "maxIndexPartials", graph, uploadProg, downloadProg, tmap);

  std::mt19937 randomEngine;
  std::vector<double> hostActivations(size);
  const bool isFpType = activationsType == FLOAT || activationsType == HALF;
  const bool isInt = activationsType == INT;
  writeRandomValues(target, activationsType, hostActivations.data(),
                    hostActivations.data() + hostActivations.size(),
                    isInt ? std::numeric_limits<int>::min() : 0.0,
                    isFpType ? 1.0 : std::numeric_limits<int>::max(),
                    randomEngine);
  copy(target, hostActivations.data(), size, activationsType,
       rawHostActivations.get());

  auto cs = graph.addComputeSet();
  auto v = graph.addVertex(cs, templateVertex("popnn::ReduceMaxClassGather",
                                              activationsType, labelType));
  graph.setTileMapping(v, 0);

  // TODO: T12987 Add test case for when index != 0.
  unsigned index = 0;
  graph.connect(v["activations"], activations);
  graph.setInitialValue(v["index"], index);
  graph.connect(v["maxValue"], maxActs);
  graph.connect(v["maxIndex"], maxIndices);
  graph.setInitialValue(v["size"], size);
  graph.setInitialValue(v["workerSize"], divisor);

  Engine e(std::move(graph), Sequence{uploadProg, Execute(cs), downloadProg});
  attachStreams(e, tmap);
  device.bind([&](const Device &d) { e.loadAndRun(d); });

  std::vector<double> modelMaxActs(nOutputs);
  std::vector<std::uint64_t> modelMaxIndices(nOutputs);
  modelVertex(hostActivations, index, modelMaxActs, modelMaxIndices, size,
              divisor);

  std::vector<double> hostMaxActs(nOutputs);
  std::vector<std::uint64_t> hostMaxIndices(nOutputs);
  copy(target, partialsType, rawHostMaxActs.get(), hostMaxActs.data(),
       nOutputs);
  copy(target, labelType, rawHostMaxIndices.get(), hostMaxIndices.data(),
       nOutputs);

  bool success = true;
  success &= checkIsClose("maxValue", hostMaxActs.data(), {nOutputs},
                          modelMaxActs.data(), nOutputs, 0.1,
                          activationsType == HALF ? 1e-7 : 1e-20);
  success &= checkEqual("maxIndex", hostMaxIndices.data(), {nOutputs},
                        modelMaxIndices.data(), nOutputs);
  return success;
}

} // end anonymous namespace

int main(int argc, char **argv) {
  namespace po = boost::program_options;

  DeviceType deviceType;
  Type activationType;
  Type labelType = UNSIGNED_INT;
  unsigned divisor, size;
  po::options_description desc("Options");
  // clang-format off
  desc.add_options()
    ("help", "Print help")
    ("device-type",
     po::value<DeviceType>(&deviceType)->required(),
     "Device Type")
    ("activation-type",
     po::value<Type>(&activationType)->required(),
     "Element type for input activations")
    ("divisor",
     po::value<unsigned>(&divisor)->required(),
     "Factor by which to reduce max class")
    ("size",
     po::value<unsigned>(&size)->required(),
     "Total size to process with vertex");
  // clang-format on

  po::variables_map vm;
  try {
    po::store(po::parse_command_line(argc, argv, desc), vm);
    if (vm.count("help")) {
      std::cout << desc << "\n\n";
      return 1;
    }
    po::notify(vm);
  } catch (std::exception &e) {
    std::cerr << "error: " << e.what() << "\n";
    return 1;
  }

  if (!doTest(deviceType, activationType, labelType, divisor, size))
    return 1;
  return 0;
}


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

#include <boost/preprocessor/seq/for_each.hpp>
#include <boost/preprocessor/variadic/to_seq.hpp>
#include <boost/preprocessor/seq/elem.hpp>
#include <boost/preprocessor/seq/rest_n.hpp>
#include <boost/preprocessor/seq/enum.hpp>

#include "is_constant.hpp"
#include "simplifier.hpp"

#ifndef BOOST_PP_SEQ_ENUM_0
#define BOOST_PP_SEQ_ENUM_0
#define CTA_BOOST_PP_SEQ_ENUM_0_DEFINED
#endif

namespace cta {

namespace detail {
template <int var, typename T>
struct differentiator {
	static zero_type<int> differentiate(const T&) { return {}; }
};

template <int var, typename number>
struct differentiator<var, var_type<number, var>> {
	static one_type<int> differentiate(const var_type<number, var>&) { return {}; }
};

template <int var, int ivar, typename number, int index>
struct differentiator<var, dvar_type<number, ivar, index>> {
	static dvar_type<number, ivar, index+1> differentiate(const dvar_type<number, ivar, index>&) { return {}; }
};

#define CTA_DERIV_TEMPLATE_ARG(_, __, a) , decltype(a)
#define CTA_DERIV_ARGS(seq) BOOST_PP_SEQ_FOR_EACH(CTA_DERIV_TEMPLATE_ARG, _, seq)
#define CTA(type, seq) (type<number CTA_DERIV_ARGS(seq)>{ BOOST_PP_SEQ_ENUM(seq) })
#define CTA_DERIV(arg) (differentiator<var, decltype(arg)>::differentiate(arg))
#define CTA_IF(condition, t) std::enable_if<condition::value, t>::type
#define CTA_STATIC_POW(t, pot) (static_power<number, decltype(t), pot>{ t })

#define CTA_VISIT_TYPE(type, n, _, __, def, ...)                                                   \
	template <int var, CTA_TEMPLATE_DECL_ARGS(n)>                                                  \
	struct differentiator<var, type<CTA_TEMPLATE_USE_ARGS(n)>> {                                   \
		static decltype(auto) differentiate(const type<CTA_TEMPLATE_USE_ARGS(n)>& t) {             \
			(void) t;                                                                              \
			return def;                                                                            \
		}                                                                                \
	};

template <int var, typename number, typename A0>
struct differentiator<var, static_power<number, A0, 0>> {
	static zero_type<number> differentiate(const static_power<number, A0, 0>&) { return {}; }
};

template <int var, typename number, typename A0, int pow>
struct differentiator<var, static_power<number, A0, pow>> {
	static decltype(auto) differentiate(const static_power<number, A0, pow>& t) {
		auto deriv = differentiator<var, A0>::differentiate(t.a0);
		return product<number, decltype(deriv),
		               product<number, number, static_power<number, A0, pow - 1>>>{
			deriv, { pow, { t.a0 } }
		};
	}
};

#include "types.hpp"

} // namespace detail

template <int var, typename T>
decltype(auto) differentiate(const T& t) {
	return simplify(detail::differentiator<var, T>::differentiate(t));
}

template <int var, typename T>
decltype(auto) differentiate(const T& t, const detail::var_type<typename T::number_t, var>&) {
	return differentiate<var>(t);
}

} // namespace cta

#ifdef CTA_BOOST_PP_SEQ_ENUM_0_DEFINED
#undef CTA_BOOST_PP_SEQ_ENUM_0_DEFINED
#undef BOOST_PP_SEQ_ENUM_0
#endif

#endif


/*
 * @Description: g2o edge for LIO IMU pre-integration measurement
 * @Author: Ge Yao
 * @Date: 2020-11-29 15:47:49
 */
#ifndef LIDAR_LOCALIZATION_MODELS_GRAPH_OPTIMIZER_G2O_EDGE_EDGE_PRVAG_IMU_PRE_INTEGRATION_HPP_
#define LIDAR_LOCALIZATION_MODELS_GRAPH_OPTIMIZER_G2O_EDGE_EDGE_PRVAG_IMU_PRE_INTEGRATION_HPP_

#include <iostream>

#include <Eigen/Core>
#include <Eigen/Dense>

#include <sophus/so3.hpp>

#include <g2o/types/slam3d/types_slam3d.h>
#include <g2o/types/slam3d_addons/types_slam3d_addons.h>

#include "lidar_localization/models/graph_optimizer/g2o/vertex/vertex_prvag.hpp"

#include <g2o/core/base_binary_edge.h>

#include "glog/logging.h"

typedef Eigen::Matrix<double, 15, 1> Vector15d;

namespace g2o {

class EdgePRVAGIMUPreIntegration : public g2o::BaseBinaryEdge<15, Vector15d, g2o::VertexPRVAG, g2o::VertexPRVAG> {
public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    static const int INDEX_P = 0;
    static const int INDEX_R = 3;
    static const int INDEX_V = 6;
    static const int INDEX_A = 9;
    static const int INDEX_G = 12;

	EdgePRVAGIMUPreIntegration()
	 : g2o::BaseBinaryEdge<15, Vector15d, g2o::VertexPRVAG, g2o::VertexPRVAG>() {
	 }

	virtual void computeError() override {
        g2o::VertexPRVAG* v0 = dynamic_cast<g2o::VertexPRVAG*>(_vertices[0]);
        g2o::VertexPRVAG* v1 = dynamic_cast<g2o::VertexPRVAG*>(_vertices[1]);

		const Eigen::Vector3d &pos_i = v0->estimate().pos;
		const Sophus::SO3d    &ori_i = v0->estimate().ori;
		const Eigen::Vector3d &vel_i = v0->estimate().vel;
		const Eigen::Vector3d &b_a_i = v0->estimate().b_a;
		const Eigen::Vector3d &b_g_i = v0->estimate().b_g;

		const Eigen::Vector3d &pos_j = v1->estimate().pos;
		const Sophus::SO3d    &ori_j = v1->estimate().ori;
		const Eigen::Vector3d &vel_j = v1->estimate().vel;
		const Eigen::Vector3d &b_a_j = v1->estimate().b_a;
		const Eigen::Vector3d &b_g_j = v1->estimate().b_g;

		//
		// TODO: update pre-integration measurement caused by bias change:
		//

		//
		// TODO: compute error:
		//
		_error.block<3, 1>(INDEX_P, 0) = Eigen::Vector3d::Zero();
		_error.block<3, 1>(INDEX_R, 0) = Eigen::Vector3d::Zero();
		_error.block<3, 1>(INDEX_V, 0) = Eigen::Vector3d::Zero();
		_error.block<3, 1>(INDEX_A, 0) = Eigen::Vector3d::Zero();
		_error.block<3, 1>(INDEX_G, 0) = Eigen::Vector3d::Zero();
    }

	void setT(const double &T) {
		T_ = T;
	}

	void setGravitiy(const Eigen::Vector3d &g) {
		g_ = g;
	}

	void setJacobian(const Eigen::MatrixXd &J) {
		J_ = J;
	}

    virtual void setMeasurement(const Vector15d& m) override {
		_measurement = m;
	}

	void updateMeasurement(
		const Eigen::Vector3d &d_b_a_i,
		const Eigen::Vector3d &d_b_g_i
	) {
		_measurement.block<3, 1>(INDEX_P, 0) += (
			J_.block<3, 3>(INDEX_P, INDEX_A)*d_b_a_i + J_.block<3, 3>(INDEX_P, INDEX_G)*d_b_g_i
		);
		_measurement.block<3, 1>(INDEX_R, 0) = (
			Sophus::SO3d::exp(_measurement.block<3, 1>(INDEX_R, 0)) *
			Sophus::SO3d::exp(J_.block<3, 3>(INDEX_R, INDEX_G)*d_b_g_i)
		).log();
		_measurement.block<3, 1>(INDEX_V, 0) += (
			J_.block<3, 3>(INDEX_V, INDEX_A)*d_b_a_i + J_.block<3, 3>(INDEX_V, INDEX_G)*d_b_g_i
		);
	}

	virtual bool read(std::istream& is) override {
		double T;
		is >> T;

		Eigen::Vector3d g;
		is >> g.x() >> g.y() >> g.z();

		Vector15d v;
		is >> v(INDEX_P + 0) >> v(INDEX_P + 1) >> v(INDEX_P + 2)
		   >> v(INDEX_R + 0) >> v(INDEX_R + 1) >> v(INDEX_R + 2)
		   >> v(INDEX_V + 0) >> v(INDEX_V + 1) >> v(INDEX_V + 2)
		   >> v(INDEX_A + 0) >> v(INDEX_A + 1) >> v(INDEX_A + 2)
		   >> v(INDEX_G + 0) >> v(INDEX_G + 1) >> v(INDEX_G + 2);

		setT(T);
		setGravitiy(g);
    	setMeasurement(v);

		for (int i = 0; i < information().rows(); ++i) {
			for (int j = i; j < information().cols(); ++j) {
				is >> information()(i, j);
				// update cross-diagonal element:
				if (i != j) {
					information()(j, i) = information()(i, j);
				}
			}
		}

		return true;
	}

	virtual bool write(std::ostream& os) const override {
    	Vector15d v = _measurement;

		os << T_ << " ";

		os << g_.x() << " " << g_.y() << " " << g_.z() << " ";

		os << v(INDEX_P + 0) << " " <<v(INDEX_P + 1) << " " << v(INDEX_P + 2) << " "
		   << v(INDEX_R + 0) << " " <<v(INDEX_R + 1) << " " << v(INDEX_R + 2) << " "
		   << v(INDEX_V + 0) << " " <<v(INDEX_A + 1) << " " << v(INDEX_A + 2) << " "
		   << v(INDEX_A + 0) << " " <<v(INDEX_V + 1) << " " << v(INDEX_V + 2) << " "
		   << v(INDEX_G + 0) << " " <<v(INDEX_G + 1) << " " << v(INDEX_G + 2) << " ";

		for (int i = 0; i < information().rows(); ++i)
			for (int j = i; j < information().cols(); ++j)
				os << " " << information()(i, j);

		return os.good();
	}

private:
	double T_ = 0.0;

	Eigen::Vector3d g_ = Eigen::Vector3d::Zero();

	Eigen::MatrixXd J_;
};

} // namespace g2o

#endif // LIDAR_LOCALIZATION_MODELS_GRAPH_OPTIMIZER_G2O_EDGE_EDGE_PRVAG_IMU_PRE_INTEGRATION_HPP_


/*
 * protonReconstruct.cpp
 *
 *  Created on: Dec 22, 2011
 *      Author: u051747
 */
#include <iostream>
#include "parameterparser.h"
#include "cuNDArray.h"
#include "hoCuNDArray.h"

#include "protonSubsetOperator.h"
#include "hoNDArray_fileio.h"
#include "check_CUDA.h"

//#include "hoCuGPBBSolver.h"
#include "hoCuPartialDerivativeOperator.h"
//#include "hoCuNDArray_blas.h"
#include "hoCuNDArray_elemwise.h"

#include "cuNDArray_math.h"

#include "osSARTSolver.h"
//#include "hoOSGPBBSolver.h"
//#include "hoOSCGSolver.h"
//#include "hoOSCGPBBSolver.h"
//#include "hoCuBILBSolver.h"
//#include "hoABIBBSolver.h"
#include "protonDROPSolver.h"
#include "cuSolverUtils.h"
#include "osMOMSolverD.h"
#include "osMOMSolverD3.h"
#include "hdf5_utils.h"

#include "encodingOperatorContainer.h"
//#include "hoCuOperator.h"
#include "hoImageOperator.h"
#include "identityOperator.h"
#include <boost/program_options.hpp>
#include "vector_td_io.h"

//#include "hoCuTvOperator.h"
//#include "hoCuTvPicsOperator.h"
#include "projectionSpaceOperator.h"
#include "hdf5.h"

using namespace std;
using namespace Gadgetron;
typedef float _real;

typedef solver<hoCuNDArray<_real>, hoCuNDArray<_real> > baseSolver;
namespace po = boost::program_options;
int main( int argc, char** argv)
{

  //
  // Parse command line
  //

  _real background =  0.00106;
  std::string dataName;
	std::string outputFile;
	vector_td<int,3> dimensions;
	vector_td<float,3> physical_dims;
	vector_td<float,3> origin;
	int iterations;
	int device;
	int subsets;
	bool use_hull,use_weights, use_non_negativity;
	float beta,gamma;
	float huber;
	float tv_weight;
	po::options_description desc("Allowed options");
	desc.add_options()
			("help", "produce help message")
			("data,D", po::value<std::string>(&dataName)->default_value("data.hdf5"), "HDF5 file containing projections and splines")
			("dimensions,d", po::value<vector_td<int,3> >(&dimensions)->default_value(vector_td<int,3>(512,512,1)), "Pixel dimensions of the image")
			("size,S", po::value<vector_td<float,3> >(&physical_dims)->default_value(vector_td<float,3>(20,20,5)), "Dimensions of the image")
			("center,c", po::value<vector_td<float,3> >(&origin)->default_value(vector_td<float,3>(0,0,0)), "Center of the reconstruction")
			("iterations,i", po::value<int>(&iterations)->default_value(10), "Dimensions of the image")
			("output,f", po::value<std::string>(&outputFile)->default_value("image.hdf5"), "Output filename")
			("prior,P", po::value<std::string>(),"Prior image filename")
			("prior-weight,k",po::value<float>(),"Weight of the prior image")
			("device",po::value<int>(&device)->default_value(0),"Number of the device to use (0 indexed)")
			("TV,T",po::value<float>(&tv_weight)->default_value(0),"TV Weight ")
			("subsets,n", po::value<int>(&subsets)->default_value(10), "Number of subsets to use")
			("beta",po::value<float>(&beta)->default_value(1),"Step size for SART")
			("gamma",po::value<float>(&gamma)->default_value(1e-1),"Relaxation Gamma")
			("huber",po::value<float>(&huber)->default_value(0),"Huber value")
			("use_hull",po::value<bool>(&use_hull)->default_value(true),"Estimate convex hull of object")
			("use_weights",po::value<bool>(&use_weights)->default_value(false),"Use weights if available. ")
			("use_non_negativity",po::value<bool>(&use_non_negativity)->default_value(true),"Use non-negativity constraint. ")
	;

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

	if (vm.count("help")) {
		cout << desc << "\n";
		return 1;
	}
	std::cout << "Command line options:" << std::endl;
	for (po::variables_map::iterator it = vm.begin(); it != vm.end(); ++it){
		boost::any a = it->second.value();
		std::cout << it->first << ": ";
		if (a.type() == typeid(std::string)) std::cout << it->second.as<std::string>();
		else if (a.type() == typeid(int)) std::cout << it->second.as<int>();
		else if (a.type() == typeid(float)) std::cout << it->second.as<float>();
		else if (a.type() == typeid(vector_td<float,3>)) std::cout << it->second.as<vector_td<float,3> >();
		else if (a.type() == typeid(vector_td<int,3>)) std::cout << it->second.as<vector_td<int,3> >();
		else if (a.type() == typeid(bool)) std::cout << it->second.as<bool>();
		else std::cout << "Unknown type" << std::endl;
		std::cout << std::endl;
	}
	cudaSetDevice(device);
	//cudaDeviceReset();

/*
  hoCuGPBBSolver< _real> solver;

  solver.set_max_iterations( iterations);

  solver.set_output_mode( hoCuGPBBSolver< _real>::OUTPUT_VERBOSE );*/
	//osSARTSolver<hoCuNDArray<_real> > solver;
	//hoCuNCGSolver<_real> solver;
	//hoOSCGPBBSolver<hoCuNDArray<_real> > solver;
	//hoABIBBSolver<hoCuNDArray<_real> > solver;
	//hoOSCGSolver<hoCuNDArray<_real> > solver;
	//hoCuBILBSolver<hoCuNDArray<_real> > solver;
	//protonDROPSolver<hoCuNDArray > solver;

	osMOMSolverD<hoCuNDArray<float>> solver;

  //solver.set_output_mode( hoCuGPBBSolver< _real>::OUTPUT_VERBOSE );
	  solver.set_non_negativity_constraint(use_non_negativity);
	  solver.set_max_iterations(iterations);
	  solver.set_huber(huber);
	  solver.set_reg_steps(5);
	  solver.set_dump(false);
	  solver.set_tau(1e-4);
	//solver.set_m(subsets);
	//solver.set_gamma(1.0f/15);
  solver.set_non_negativity_constraint(use_non_negativity);
  solver.set_max_iterations(iterations);

  H5Eset_auto1(0,0);//Disable hdf5 error reporting
  std::vector<size_t> rhs_dims(&dimensions[0],&dimensions[3]); //Quick and dirty vector_td to vector
std::cout << "Loading data" << std::endl;
  boost::shared_ptr<protonDataset<hoCuNDArray> >  data(new protonDataset<hoCuNDArray>(dataName,use_weights));
std::cout << "Done" << std::endl;
  data = protonDataset<hoCuNDArray>::shuffle_dataset(data,subsets);

  data->preprocess(rhs_dims,physical_dims,false);

  boost::shared_ptr< protonSubsetOperator<hoCuNDArray> > E (new protonSubsetOperator<hoCuNDArray>(data->get_subsets(), physical_dims) );

  E->set_domain_dimensions(&rhs_dims);
  E->set_codomain_dimensions(data->get_projections()->get_dimensions().get());
/*
  boost::shared_ptr<hoCuNDArray<_real > > prior;
  if (vm.count("prior")){
 	  std::cout << "Prior image regularization in use" << std::endl;
		prior = boost::static_pointer_cast<hoCuNDArray<_real > >(read_nd_array<_real >(vm["prior"].as<std::string>().c_str()));

		prior->reshape(&rhs_dims);
		_real offset = _real(0.01);
		//cuNDA_add(offset,prior.get());

		if (vm.count("prior-weight")){

		//boost::shared_ptr<hoImageOperator<_real> > I (new hoImageOperator<_real > ());
		//I->compute(prior.get());
			boost::shared_ptr<identityOperator<hoCuNDArray<_real> > > Itmp ( new identityOperator<hoCuNDArray<_real> >);

			boost::shared_ptr<projectionSpaceOperator<hoCuNDArray<_real> > > I (new projectionSpaceOperator<hoCuNDArray<_real> >(Itmp));

		I->set_weight(vm["prior-weight"].as<float>());

		I->set_codomain_dimensions(&rhs_dims);
		I->set_domain_dimensions(&rhs_dims);
		I->set_projections(prior);
		//hoCuNDArray<_real> tmp = *prior;

		//I->mult_M(prior.get(),&tmp);
		solver.add_regularization_operator(I);

		}
		solver.set_x0(prior);
  }
  */

  if (tv_weight > 0){

  	auto Dx = boost::make_shared<hoCuPartialDerivativeOperator<float,3>>(0);
  	Dx->set_weight(tv_weight);
  	Dx->set_domain_dimensions(&rhs_dims);
  	Dx->set_codomain_dimensions(&rhs_dims);

  	auto Dy = boost::make_shared<hoCuPartialDerivativeOperator<float,3>>(1);
  	Dy->set_weight(tv_weight);
  	Dy->set_domain_dimensions(&rhs_dims);
  	Dy->set_codomain_dimensions(&rhs_dims);

  	auto Dz = boost::make_shared<hoCuPartialDerivativeOperator<float,3>>(2);
  	Dz->set_weight(tv_weight);
  	Dz->set_domain_dimensions(&rhs_dims);
  	Dz->set_codomain_dimensions(&rhs_dims);

  	//solver.add_regularization_operator(Dx);
  	//solver.add_regularization_operator(Dy);
  	solver.add_regularization_group({Dx,Dy,Dz});
  }
  /*
  if (vm.count("TV")){
	  std::cout << "Total variation regularization in use" << std::endl;
	  boost::shared_ptr<hoCuTvOperator<float,3> > tv(new hoCuTvOperator<float,3>);
	  tv->set_weight(vm["TV"].as<float>());
	  solver.add_nonlinear_operator(tv);
  }
  */

  solver.set_encoding_operator(E);
  solver.set_output_mode(baseSolver::OUTPUT_VERBOSE);
	//hoCuNDA_clear(projections.get());
	//CHECK_FOR_CUDA_ERROR();

	//float res = dot(projections.get(),projections.get());
  boost::shared_ptr< hoCuNDArray<_real> > result;
  {
  	GPUTimer timer("Time to solve");
  	result = solver.solve(data->get_projections().get());
  }

	std::stringstream ss;
	for (int i = 0; i < argc; i++){
		ss << argv[i] << " ";
	}
	saveNDArray2HDF5<3>(result.get(),outputFile,physical_dims,origin,ss.str(), solver.get_max_iterations());
}


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

#include <scitbx/random/boost_python/random.h>

#include <boost/random/uniform_real.hpp>
#include <scitbx/sparse/random.h>

namespace scitbx { namespace sparse { namespace boost_python {

  template <class ElementDistribution>
  struct matrix_distribution_traits
  {};

  template<typename FloatType, class ElementDistribution>
  struct matrix_distribution
  {
    typedef sparse::matrix_distribution<FloatType, ElementDistribution>  wt;
    typedef typename wt::index_type index_type;

    static std::string name() {
      return matrix_distribution_traits<ElementDistribution>::name();
    }

    static wt *make_1(index_type n_rows, index_type n_cols, double density,
                      ElementDistribution &e)
    {
      return new wt(n_rows, n_cols, density, e);
    }

    static wt *make_2(index_type n_rows, index_type n_cols, index_type n_zeroes,
                      ElementDistribution &e)
    {
      return new wt(n_rows, n_cols, n_zeroes, e);
    }

    static void wrap_specific(boost::python::class_<wt> &klass) {
      using namespace boost::python;
      klass
        .add_property("n_rows", &wt::n_rows)
        .add_property("n_cols", &wt::n_cols)
        .add_property("non_zeroes", &wt::non_zeroes)
        ;
      def("matrix_distribution", make_1,
          return_value_policy<manage_new_object>(),
           ((arg("n_rows"), arg("n_cols"), arg("density"),
             arg("elements"))));
      def("matrix_distribution", make_2,
          return_value_policy<manage_new_object>(),
           ((arg("n_rows"), arg("n_cols"), arg("non_zeroes"),
             arg("elements"))));
    }
  };

  template <>
  struct matrix_distribution_traits<boost::uniform_real<double> >
  {
    static std::string name() { return "matrix_with_uniform_element"; }
  };

  template <class ElementDistribution>
  struct vector_distribution_traits
  {};

  template<typename FloatType, class ElementDistribution>
  struct vector_distribution
  {
    typedef sparse::vector_distribution<FloatType, ElementDistribution>  wt;
    typedef typename wt::index_type index_type;

    static std::string name() {
      return vector_distribution_traits<ElementDistribution>::name();
    }

    static wt *make_1(index_type size, double density,
                      ElementDistribution &e)
    {
      return new wt(size, density, e);
    }

    static wt *make_2(index_type size, index_type n_zeroes,
                      ElementDistribution &e)
    {
      return new wt(size, n_zeroes, e);
    }

    static void wrap_specific(boost::python::class_<wt> &klass) {
      using namespace boost::python;
      klass
        .add_property("size", &wt::size)
        .add_property("non_zeroes", &wt::non_zeroes)
        ;
    def("vector_distribution", make_1,
        return_value_policy<manage_new_object>(),
         ((arg("size"), arg("density"), arg("elements"))));
    def("vector_distribution", make_2,
        return_value_policy<manage_new_object>(),
         ((arg("size"), arg("non_zeroes"), arg("elements"))));
    }
  };

  template <>
  struct vector_distribution_traits<boost::uniform_real<double> >
  {
    static std::string name() { return "vector_with_uniform_element"; }
  };

void wrap_random() {
  using scitbx::random::boost_python::wrap_distribution_and_variate;

  wrap_distribution_and_variate<
    matrix_distribution<double, boost::uniform_real<double> > >();

  wrap_distribution_and_variate<
    vector_distribution<double, boost::uniform_real<double> > >();
}

}}} // scitbx::sparse::boost_python


// Copyright (c) 2021 Graphcore Ltd. All rights reserved.
#define BOOST_TEST_MODULE Test_SGD0_CompoundScalarHelpers
#include <boost/test/unit_test.hpp>
#include <popart/compoundscalarhelper.hpp>
#include <popart/sessionoptions.hpp>
#include <popart/sgd.hpp>

using namespace popart;

namespace {
void validate(SGD sgd, float expected_lr, float expected_wd) {
  ScaledLearningRate0Helper lr;
  WeightDecayScaleFactor0Helper wd;

  BOOST_CHECK_EQUAL(lr.val("", sgd), expected_lr);
  BOOST_CHECK_EQUAL(wd.val("", sgd), expected_wd);
}
} // namespace

BOOST_AUTO_TEST_CASE(TestSGD0_Base) {
  SGD sgd{{{"defaultLearningRate", {0.1f, true}},
           {"defaultWeightDecay", {0.01f, true}}}};
  SessionOptions opts;
  sgd.setFactorsFromOptions(opts);
  validate(sgd, 0.1f, 1.0f - (0.1f * 0.01f));
}

BOOST_AUTO_TEST_CASE(TestSGD0_lossScaling) {
  SGD sgd{{{"defaultLearningRate", {0.1f, true}},
           {"defaultWeightDecay", {0.01f, true}},
           {"lossScaling", {4.0f, true}}}};
  SessionOptions opts;
  sgd.setFactorsFromOptions(opts);
  validate(sgd, 0.1f / 4.0f, 1.0f - (0.1f * 0.01f));
}

BOOST_AUTO_TEST_CASE(TestSGD0_ReplicaSum) {
  SGD sgd{{{"defaultLearningRate", {0.1f, true}},
           {"defaultWeightDecay", {0.01f, true}}}};
  SessionOptions opts;
  opts.enableReplicatedGraphs = true;
  opts.replicatedGraphCount   = 2;
  sgd.setFactorsFromOptions(opts);
  validate(sgd, 0.1f, 1.0f - (0.1f * 0.01f));
}

BOOST_AUTO_TEST_CASE(TestSGD0_ReplicaMeanPost) {
  SGD sgd{{{"defaultLearningRate", {0.1f, true}},
           {"defaultWeightDecay", {0.01f, true}}}};
  SessionOptions opts;
  opts.enableReplicatedGraphs                  = true;
  opts.replicatedGraphCount                    = 2;
  opts.accumulationAndReplicationReductionType = ReductionType::Mean;
  opts.meanAccumulationAndReplicationReductionStrategy =
      MeanReductionStrategy::Post;
  sgd.setFactorsFromOptions(opts);
  validate(sgd, 0.1f / 2.0f, 1.0f - (0.1f * 0.01f));
}

BOOST_AUTO_TEST_CASE(TestSGD0_ReplicaMeanRunning) {
  SGD sgd{{{"defaultLearningRate", {0.1f, true}},
           {"defaultWeightDecay", {0.01f, true}}}};
  SessionOptions opts;
  opts.enableReplicatedGraphs                  = true;
  opts.replicatedGraphCount                    = 2;
  opts.accumulationAndReplicationReductionType = ReductionType::Mean;
  opts.meanAccumulationAndReplicationReductionStrategy =
      MeanReductionStrategy::Running;
  sgd.setFactorsFromOptions(opts);
  validate(sgd, 0.1f, 1.0f - (0.1f * 0.01f));
}

BOOST_AUTO_TEST_CASE(TestSGD0_AccumSum) {
  SGD sgd{{{"defaultLearningRate", {0.1f, true}},
           {"defaultWeightDecay", {0.01f, true}}}};
  SessionOptions opts;
  opts.enableReplicatedGraphs = true;
  opts.replicatedGraphCount   = 2;
  sgd.setFactorsFromOptions(opts);
  validate(sgd, 0.1f, 1.0f - (0.1f * 0.01f));
}

BOOST_AUTO_TEST_CASE(TestSGD0_AccumMeanPost) {
  SGD sgd{{{"defaultLearningRate", {0.1f, true}},
           {"defaultWeightDecay", {0.01f, true}}}};
  SessionOptions opts;
  opts.enableGradientAccumulation              = true;
  opts.accumulationFactor                      = 4;
  opts.accumulationAndReplicationReductionType = ReductionType::Mean;
  opts.meanAccumulationAndReplicationReductionStrategy =
      MeanReductionStrategy::Post;
  sgd.setFactorsFromOptions(opts);
  validate(sgd, 0.1f / 4.0f, 1.0f - (0.1f * 0.01f));
}

BOOST_AUTO_TEST_CASE(TestSGD0_AccumMeanRunning) {
  SGD sgd{{{"defaultLearningRate", {0.1f, true}},
           {"defaultWeightDecay", {0.01f, true}}}};
  SessionOptions opts;
  opts.enableGradientAccumulation              = true;
  opts.accumulationFactor                      = 4;
  opts.accumulationAndReplicationReductionType = ReductionType::Mean;
  opts.meanAccumulationAndReplicationReductionStrategy =
      MeanReductionStrategy::Running;
  sgd.setFactorsFromOptions(opts);
  validate(sgd, 0.1f, 1.0f - (0.1f * 0.01f));
}

BOOST_AUTO_TEST_CASE(TestSGD0_Dampening) {
  SGD sgd{{{"defaultLearningRate", {0.1f, true}},
           {"defaultWeightDecay", {0.01f, true}},
           {"defaultDampening", {0.2f, true}},
           {"defaultMomentum", {0.0f, true}}}};
  SessionOptions opts;
  sgd.setFactorsFromOptions(opts);
  validate(sgd, (1.0f - 0.2f) * 0.1f, 1.0f - ((1.0f - 0.2f) * 0.1f * 0.01f));
}


///////////////////////////////////////////////////////////////////////////////
// traits.hpp                                                                //
//                                                                           //
//  Copyright 2010 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_TOOLS_DETAIL_SERIES_TRAITS_HPP_ER_2010
#define BOOST_STATISTICS_TOOLS_DETAIL_SERIES_TRAITS_HPP_ER_2010
#include <boost/mpl/apply.hpp>
#include <boost/type_traits/remove_cv.hpp>
#include <boost/type_traits/remove_reference.hpp>
#include <boost/statistics/tools/series/data_field.hpp>
#include <boost/mpl/fold.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/serialization/detail/inherit/inherit_linearly.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace series{

    template<typename A,typename Tag>
    struct value_of_extract{
        typedef typename boost::mpl::apply<
            A, Tag>::type::result_type cref_;
        typedef typename boost::remove_cv<
            typename boost::remove_reference<cref_>::type
        >::type type;
    };

    template<typename A,typename Tag>
    struct data_field_of{
       typedef detail::series::data<
           Tag,
           typename detail::series::value_of_extract<A,Tag>::type
       > type;
    };

    template<typename A,typename F>
    struct data_fields_of : boost::mpl::fold<
        F,
        detail::series::data_field_of<A,boost::mpl::_1>
    >{};

    template<typename A,typename F,typename R = boost::mpl::empty_base>
    struct data_of{
       typedef typename detail::series::data_fields_of<A,F>::type fields_;
       typedef boost::serialization::detail::inherit_linearly<fields_,R> type;
    };

}// series
}// detail
}// statistics
}// boost

#endif


#include <iostream>
#include <Eigen/Dense>
#include <fstream>
#include <sstream>
#include <vector>
#include <string>

#include "ros/ros.h"
#include "std_msgs/Float64MultiArray.h"

#include <HLrobotconfig.h>

void readcsv(const std::string& filename,double* T);

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

    double joint[6]={0};
    ros::init(argc, argv, "IKsolver");
    ros::NodeHandle n;
    ros::Publisher pub = n.advertise<std_msgs::Float64MultiArray>("/HL_controller/command", 1000);
    ros::Rate loop_rate(10);
    double TMatrix[16]={0};

    readcsv("/home/liuxiao/catkin_ws/src/HLRobot_gazebo/data/eye_hand/002_pose.csv",TMatrix);

    while (ros::ok()) {
        std_msgs::Float64MultiArray msg;
        //HLRobot::SetRobotEndPos(-20.067, 33.435, 96.225, -15.926, 79.755, 267.17);
        HLRobot::setTMatrix(TMatrix);
        HLRobot::GetJointAngles(joint[0],joint[1],joint[2],joint[3],joint[4],joint[5]);
        msg.data.push_back(joint[0]);
        msg.data.push_back(joint[1]);
        msg.data.push_back(joint[2]);
        msg.data.push_back(joint[3]);
        msg.data.push_back(joint[4]);
        msg.data.push_back(joint[5]);

        pub.publish(msg);
        ros::spinOnce();
        loop_rate.sleep();
        }
    return 0;
}

void readcsv(const std::string& filename, double* T){
    std::ifstream ifs(filename);
    if(!ifs){
        std::cerr << "failed to open " << filename << std::endl;
    }

    std::vector<std::vector<double>> matrix;
    while(!ifs.eof()) {
        std::string line;
        std::getline(ifs, line);

        if(line.empty()) {
            continue;
        }

        std::vector<double> row;
        std::stringstream sst(line);
        while(!sst.eof()) {
            double value;
            sst >> value;
            row.push_back(value);
        }

        matrix.push_back(row);
    }

    for(int i=0; i<matrix.size(); i++) {
        for(int j=0; j<matrix[i].size(); j++) {
            T[4*i+j] = matrix[i][j];
        }
    }

}


#include <boost/test/unit_test.hpp>
#include "algorithms/search/find_next_greater_element.hpp"

BOOST_AUTO_TEST_SUITE(TestFindNextGreaterElement)

BOOST_AUTO_TEST_CASE(empty_data)
{
    std::vector<int> nums1, nums2;
    std::vector<int> expected;

    BOOST_CHECK(expected ==
                Algo::Search::FindNextGreaterElem::FindSimple(nums1, nums2));

    BOOST_CHECK(expected ==
                Algo::Search::FindNextGreaterElem::FindUsingStack(nums1, nums2));

    BOOST_CHECK(expected ==
                Algo::Search::FindNextGreaterElem::FindInCircularArray(nums1));
}

BOOST_AUTO_TEST_CASE(invalid_data)
{
    std::vector<int> nums1(2, 0), nums2(1, 0);
    std::vector<int> expected;

    BOOST_CHECK(expected ==
                Algo::Search::FindNextGreaterElem::FindSimple(nums1, nums2));

    BOOST_CHECK(expected ==
                Algo::Search::FindNextGreaterElem::FindUsingStack(nums1, nums2));
}

BOOST_AUTO_TEST_CASE(two_arrays)
{
    {
        std::vector<int> nums1 = {4, 1, 2};
        std::vector<int> nums2 = {1, 3, 4, 2};
        std::vector<int> expected = {-1, 3, -1};

        BOOST_CHECK(expected ==
            Algo::Search::FindNextGreaterElem::FindSimple(nums1, nums2));

        BOOST_CHECK(expected ==
            Algo::Search::FindNextGreaterElem::FindUsingStack(nums1, nums2));
    }

    {
        std::vector<int> nums1 = {2, 4};
        std::vector<int> nums2 = {1, 2, 3, 4};
        std::vector<int> expected = {3, -1};

        BOOST_CHECK(expected ==
            Algo::Search::FindNextGreaterElem::FindSimple(nums1, nums2));

        BOOST_CHECK(expected ==
            Algo::Search::FindNextGreaterElem::FindUsingStack(nums1, nums2));
    }
}

BOOST_AUTO_TEST_CASE(next_greater_temperature)
{
    const std::vector<int> temp = {73, 74, 75, 71, 69, 72, 76, 73};
    const std::vector<int> expected = {1, 1, 4, 2, 1, 1, 0, 0};
    BOOST_CHECK(expected == Algo::Search::FindNextGreaterElem::FindNextGreaterTemperature(temp));
}

BOOST_AUTO_TEST_CASE(in_circular_arr)
{
    {
        std::vector<int> nums = {4};
        std::vector<int> expected = {-1};

        BOOST_CHECK(expected ==
            Algo::Search::FindNextGreaterElem::FindInCircularArray(nums));
    }

    {
        std::vector<int> nums = {1, 2, 3};
        std::vector<int> expected = {2, 3, -1};

        BOOST_CHECK(expected ==
            Algo::Search::FindNextGreaterElem::FindInCircularArray(nums));
    }

    {
        std::vector<int> nums = {3, 2, 1};
        std::vector<int> expected = {-1, 3, 3};

        BOOST_CHECK(expected ==
            Algo::Search::FindNextGreaterElem::FindInCircularArray(nums));
    }

    {
        std::vector<int> nums = {1, 2, 1};
        std::vector<int> expected = {2, -1, 2};

        BOOST_CHECK(expected ==
            Algo::Search::FindNextGreaterElem::FindInCircularArray(nums));
    }
}

BOOST_AUTO_TEST_CASE(smallest_greater_num)
{
    BOOST_CHECK(-1 ==
        Algo::Search::FindNextGreaterElem::FindSmallestGreaterNum(0));

    BOOST_CHECK(-1 ==
        Algo::Search::FindNextGreaterElem::FindSmallestGreaterNum(1));

    BOOST_CHECK(-1 ==
        Algo::Search::FindNextGreaterElem::FindSmallestGreaterNum(10));

    BOOST_CHECK(21 ==
        Algo::Search::FindNextGreaterElem::FindSmallestGreaterNum(12));

    BOOST_CHECK(132 ==
        Algo::Search::FindNextGreaterElem::FindSmallestGreaterNum(123));

    BOOST_CHECK(-1 ==
        Algo::Search::FindNextGreaterElem::FindSmallestGreaterNum(1999999999));

    BOOST_CHECK(1243 ==
        Algo::Search::FindNextGreaterElem::FindSmallestGreaterNum(1234));

    BOOST_CHECK(230412 ==
        Algo::Search::FindNextGreaterElem::FindSmallestGreaterNum(230241));

    BOOST_CHECK(13222344 ==
        Algo::Search::FindNextGreaterElem::FindSmallestGreaterNum(12443322));
}

BOOST_AUTO_TEST_SUITE_END()


//
//  main.cpp
//
//  Created by Dylan Menzies on 10/11/2014.
//  Copyright (c) 2014 ISVR, University of Southampton. All rights reserved.
//

#include <libpanning/VBAP.h>
#include <libpanning/CAP.h>
#include <libpanning/AllRAD.h>
#include <boost/filesystem.hpp>

#include <boost/test/unit_test.hpp>

#include <algorithm>
#include <cstdio>
#include <cstdlib>
#include <iostream>
#include <iterator>
#include <stdexcept>
#include <vector>

namespace visr
{
namespace panning
{
namespace test
{

namespace // unnamed
{

void capTestCase( std::string const & arrayConfig,
                  XYZ const & sourcePos,
                  XYZ const & listenerPos,
                  XYZ const & auralAxis,
                  std::vector<Afloat> const & referenceGains )
{
  LoudspeakerArray array;
  BOOST_CHECK_NO_THROW( array.loadXmlString( arrayConfig ) );

  CAP cap;
  cap.setLoudspeakerArray( &array );
  cap.setListenerPosition( listenerPos.x, listenerPos.y, listenerPos.z );
  cap.setListenerAuralAxis( auralAxis.x, auralAxis.y, auralAxis.z );     // head forward (rL left)

  cap.setNumSources( 1 );
  cap.setSourcePositions( &sourcePos );

  BOOST_CHECK( cap.calcGains() == 0);

  if( not referenceGains.empty() )
  {
    efl::BasicMatrix<Afloat> const & gains = cap.getGains();
    BOOST_CHECK_MESSAGE( gains.numberOfRows() == referenceGains.size(), "CAP gain vector size differs from reference." );

    std::size_t const numCheckEl = std::min( gains.numberOfRows(), referenceGains.size() );
    for( std::size_t idx(0); idx < numCheckEl; ++idx )
    {
      BOOST_CHECK_CLOSE( gains(idx,0), referenceGains[idx], 1.0e-4f );
    }
  }
}

/**
 * Overloaded version that can be used with initializer lists, that is,
 * constant arrays of the form {0.0, 1.0}
 */
void capTestCase( std::string const & arrayConfig,
  XYZ const & sourcePos,
  XYZ const & listenerPos,
  XYZ const & auralAxis,
  std::initializer_list<Afloat> const & referenceGains = {} )
{
  capTestCase( arrayConfig,
               sourcePos,
               listenerPos,
               auralAxis,
               std::vector<Afloat>(referenceGains.begin(), referenceGains.end() ));
}

// Define some setups to be used later on.
std::string const arrayTwoSpeakers =
"<panningConfiguration dimension = \"3\" infinite = \"false\">"
"<loudspeaker channel = \"1\" id =\"1\" > <cart x = \"1.0\" y = \"1.0\" z= \"0.0\" /></loudspeaker>"
"<loudspeaker channel = \"2\" id = \"2\" > <cart x = \"1.0\" y = \"-1.0\" z = \"0.0\" /></loudspeaker>"
"</panningConfiguration>";

std::string const arrayThreeSpeakers =
"<panningConfiguration dimension = \"3\" infinite = \"false\">"
"<loudspeaker channel = \"1\" id =\"1\" > <cart x = \"0.0\" y = \"1.0\" z= \"0.0\" /></loudspeaker>"
"<loudspeaker channel = \"2\" id = \"2\" > <cart x = \"0.0\" y = \"-1.0\" z = \"0.0\" /></loudspeaker>"
"<loudspeaker channel = \"3\" id = \"3\" > <cart x = \"1.0\" y = \"1.0\" z = \"0.0\" /></loudspeaker>"
"</panningConfiguration>";

} // Unnamed namespace

BOOST_AUTO_TEST_CASE( Cap2Loudspeakers )
{
  XYZ const sourcePos(0.0f, 1.0f, 0.0f, true );
  XYZ const listenerPos( 0.0f, 0.0f, 0.0f );
  XYZ const auralAxis( 0.0f, 1.0f, 0.0f ); // head forward (rL left)

  // TODO: Add reference gains!
  capTestCase( arrayTwoSpeakers, sourcePos, listenerPos, auralAxis /*, {0.5f, 0.5f }*/ );
}

BOOST_AUTO_TEST_CASE( Cap3Loudspeakers )
{
  XYZ const sourcePos( 0.0f, 1.0f, 0.0f, true );     // image left
  XYZ const listenerPos( 0.0f, 0.0f, 0.0f );    // Listener
  XYZ const auralAxis( 0.0f, 1.0f, 0.0f );           // head forward (rL left)
  // XYZ const auralAxis( -1.0f, 0.0f, 0.0f );       // head left (rL back)

  // TODO: Add reference gains!
  capTestCase( arrayThreeSpeakers, sourcePos, listenerPos, auralAxis /*, { 0.5f, 0.5f, 0.5f }*/ );
}

} // namespace test
} // namespace panning
} // namespace visr