//
// Copyright (c) 2020 Kenshi Abe
//

#include "Trainer.hpp"

#include <iostream>
#include <boost/archive/binary_iarchive.hpp>
#include <boost/archive/binary_oarchive.hpp>
#include <boost/filesystem.hpp>
#include <boost/serialization/unordered_map.hpp>
#include "Node.hpp"

namespace Trainer {

/// @param mode variant of CFR algorithm
/// @param seed random seed
/// @param strategyPaths paths to the binary files that represent fixed strategies
template <typename T>
Trainer<T>::Trainer(const std::string &mode, const uint32_t seed, const std::vector<std::string> &strategyPaths) : mEngine(seed), mNodeTouchedCnt(0), mModeStr(mode) {
    mGame = new T(mEngine);
    mFolderPath = "../strategies/" + mGame->name();
    boost::filesystem::create_directories(mFolderPath);
    mFixedStrategies = new std::unordered_map<std::string, Node *>[mGame->playerNum()];
    mUpdate = new bool[mGame->playerNum()];
    for (int i = 0; i < mGame->playerNum(); ++i) {
        if (strategyPaths.size() >= i + 1 && !strategyPaths[i].empty()) {
            std::cout << "load strategy \"" << strategyPaths[i] << "\" as static player " << i << std::endl;
            std::ifstream ifs(strategyPaths[i]);
            boost::archive::binary_iarchive ia(ifs);
            ia >> mFixedStrategies[i];
            ifs.close();
            mUpdate[i] = false;
        } else {
            mUpdate[i] = true;
        }
    }
}

template <typename T>
Trainer<T>::~Trainer() {
    for (auto &itr : mNodeMap) {
        delete itr.second;
    }
    for (int i = 0; i < mGame->playerNum(); ++i) {
        if (mUpdate[i]) {
            continue;
        }
        for (auto &itr : mFixedStrategies[i]) {
            delete itr.second;
        }
    }
    delete[] mFixedStrategies;
    delete[] mUpdate;
    delete mGame;
}

/// @brief Calculate the expected payoff of each player
/// @param game game
/// @param strategies list of strategies for each player
/// @return list of expected payoffs
template <typename T>
std::vector<double> Trainer<T>::CalculatePayoff(const T &game, const std::vector<std::function<const double *(const T &)>> &strategies) {
    // return payoff for terminal states
    if (game.done()) {
        std::vector<double> payoffs(game.playerNum());
        for (int i = 0; i < game.playerNum(); ++i) {
            payoffs[i] = game.payoff(i);
        }
        return payoffs;
    }

    // chance node turn
    const int actionNum = game.actionNum();
    if (game.isChanceNode()) {
        std::vector<double> nodeUtils(game.playerNum());
        for (int i = 0; i < game.playerNum(); ++i) {
            nodeUtils[i] = 0.0;
        }
        for (int a = 0; a < actionNum; ++a) {
            auto game_cp(game);
            game_cp.step(a);
            const double chanceProbability = game_cp.chanceProbability();
            std::vector<double> utils = CalculatePayoff(game_cp, strategies);
            for (int i = 0; i < game.playerNum(); ++i) {
                nodeUtils[i] += chanceProbability * utils[i];
            }
        }
        return nodeUtils;
    }

    // for each action, recursively calculate payoff with additional history and probability
    const int player = game.currentPlayer();
    std::vector<double> nodeUtils(game.playerNum());
    for (int i = 0; i < game.playerNum(); ++i) {
        nodeUtils[i] = 0.0;
    }
    for (int a = 0; a < actionNum; ++a) {
        auto game_cp(game);
        game_cp.step(a);
        std::vector<double> utils = CalculatePayoff(game_cp, strategies);
        for (int i = 0; i < game.playerNum(); ++i) {
            nodeUtils[i] += strategies[player](game)[a] * utils[i];
        }
    }
    return nodeUtils;
}

/// @brief Calculate exploitability of a given strategy profile
/// @param game game
/// @param strategies list of strategies for each player
/// @return exploitability of a given strategy profile
template <typename T>
double Trainer<T>::CalculateExploitability(const T &game, const std::vector<std::function<const double *(const T &)>> &strategies) {
    InfoSets infoSets;
    for (int p = 0; p < game.playerNum(); ++p) {
        auto game_cp(game);
        game_cp.reset(false);
        CreateInfoSets(game_cp, p, strategies, 1.0, infoSets);
    }

    double exploitability = 0.0;
    for (int p = 0; p < game.playerNum(); ++p) {
        auto game_cp(game);
        game_cp.reset(false);
        std::unordered_map<std::string, std::vector<double>> bestResponseStrategies;
        exploitability += CalculateBestResponseValue(game_cp, p, strategies, bestResponseStrategies, 1.0, infoSets);
    }
    return exploitability;
}

/// @brief Fill the ordered map that maps information sets to game nodes and reach probabilities
/// @param game game
/// @param playerIndex player whose strategy is updated in the current iteration
/// @param strategies list of strategies for each player
/// @param po the probability of reaching the current game node if the acting player always chooses actions leading to the current game node
/// @return exploitability of a given strategy profile
template <typename T>
void Trainer<T>::CreateInfoSets(const T &game, const int playerIndex, const std::vector<std::function<const double *(const T &)>> &strategies, const double po, InfoSets &infoSets) {
    // return at terminal states
    if (game.done()) {
        return;
    }

    // chance node turn
    const int actionNum = game.actionNum();
    if (game.isChanceNode()) {
        for (int a = 0; a < actionNum; ++a) {
            auto game_cp(game);
            game_cp.step(a);
            const double chanceProbability = game_cp.chanceProbability();
            CreateInfoSets(game_cp, playerIndex, strategies, po * chanceProbability, infoSets);
        }
        return;
    }

    const int player = game.currentPlayer();
    if (player == playerIndex) {
        std::string infoSet = game.infoSetStr();
        if (infoSets.count(infoSet) == 0) {
            infoSets[infoSet] = std::vector<std::tuple<T, double>>();
        }
        infoSets[infoSet].push_back(std::make_tuple(game, po));
    }

    for (int a = 0; a < actionNum; ++a) {
        auto game_cp(game);
        game_cp.step(a);
        if (player == playerIndex) {
            CreateInfoSets(game_cp, playerIndex, strategies, po, infoSets);
        } else {
            const double actionProb = strategies[player](game)[a];
            CreateInfoSets(game_cp, playerIndex, strategies, po * actionProb, infoSets);
        }
    }
}

/// @brief Calculate best response value for a given player
/// @param game game
/// @param playerIndex player whose strategy is updated in the current iteration
/// @param strategies list of strategies for each player
/// @param bestResponseStrategies best response strategy for a given player
/// @param po the probability of reaching the current game node if the acting player always chooses actions leading to the current game node
/// @param infoSets ordered map that maps information sets to game nodes and reach probabilities
/// @return best response value for a given player
template <typename T>
double Trainer<T>::CalculateBestResponseValue(const T &game, const int playerIndex,
                                             const std::vector<std::function<const double *(const T &)>> &strategies,
                                             std::unordered_map<std::string, std::vector<double>> &bestResponseStrategies,
                                             const double po,
                                             const InfoSets &infoSets) {
    // return payoff for terminal states
    if (game.done()) {
        return game.payoff(playerIndex);
    }

    // chance node turn
    const int actionNum = game.actionNum();
    if (game.isChanceNode()) {
        double nodeUtil = 0.0;
        for (int a = 0; a < actionNum; ++a) {
            auto game_cp(game);
            game_cp.step(a);
            const double chanceProbability = game_cp.chanceProbability();
            nodeUtil += chanceProbability * CalculateBestResponseValue(game_cp, playerIndex, strategies, bestResponseStrategies, po * chanceProbability, infoSets);
        }
        return nodeUtil;
    }

    const int player = game.currentPlayer();
    if (player == playerIndex) {
        // get information set string representation
        std::string infoSet = game.infoSetStr();
        if (bestResponseStrategies.count(infoSet) == 0) {
            // calculate action values
            double actionValues[actionNum];
            for (int a = 0; a < actionNum; ++a) {
                actionValues[a] = 0.0;
            }
            for (int i = 0; i < infoSets.at(infoSet).size(); ++i) {
                auto game_ = std::get<0>(infoSets.at(infoSet)[i]);
                auto po_ = std::get<1>(infoSets.at(infoSet)[i]);
                double brValues[actionNum];
                for (int a = 0; a < actionNum; ++a) {
                    auto game_cp(game_);
                    game_cp.step(a);
                    brValues[a] = CalculateBestResponseValue(game_cp, playerIndex, strategies, bestResponseStrategies, po_, infoSets);
                    actionValues[a] += po_ * brValues[a];
                }
            }
            // calculate best response strategy
            int brAction = 0;
            for (int a = 0; a < actionNum; ++a) {
                if (actionValues[a] > actionValues[brAction]) {
                    brAction = a;
                }
            }
            bestResponseStrategies[infoSet] = std::vector<double>(actionNum, 0.0);
            bestResponseStrategies[infoSet][brAction] = 1.0;
        }
        // calculate best response value
        double utils[actionNum];
        for (int a = 0; a < actionNum; ++a) {
            auto game_cp(game);
            game_cp.step(a);
            utils[a] = CalculateBestResponseValue(game_cp, playerIndex, strategies, bestResponseStrategies, po, infoSets);
        }
        double bestResponseValue = 0.0;
        for (int a = 0; a < actionNum; ++a) {
            bestResponseValue += utils[a] * bestResponseStrategies.at(infoSet)[a];
        }
        return bestResponseValue;
    } else {
        // for each action, recursively calculate payoff with additional history and probability
        double nodeUtil = 0.0;
        for (int a = 0; a < actionNum; ++a) {
            auto game_cp(game);
            game_cp.step(a);
            const double actionProb = strategies[player](game)[a];
            nodeUtil += actionProb * CalculateBestResponseValue(game_cp, playerIndex, strategies, bestResponseStrategies, po * actionProb, infoSets);
        }
        return nodeUtil;
    }
}

/// @brief Execute the CFR algorithm to compute an approximate Nash equilibrium
/// @param iterations number of iterations of CFR
template <typename T>
void Trainer<T>::train(const int iterations) {
    double utils[mGame->playerNum()];

    for (int i = 0; i < iterations; ++i) {
        for (int p = 0; p < mGame->playerNum(); ++p) {
            if (!mUpdate[p]) {
                continue;
            }
            if (mModeStr == "vanilla") {
                mGame->reset(false);
                utils[p] = CFR(*mGame, p, 1.0, 1.0);
                for (auto & itr : mNodeMap) {
                    itr.second->updateStrategy();
                }
            } else {
                mGame->reset();
                if (mModeStr == "chance") {
                    utils[p] = chanceSamplingCFR(*mGame, p, 1.0, 1.0);
                    for (auto & itr : mNodeMap) {
                        itr.second->updateStrategy();
                    }
                } else if (mModeStr == "external") {
                    utils[p] = externalSamplingCFR(*mGame, p);
                } else if (mModeStr == "outcome") {
                    utils[p] = std::get<0>(outcomeSamplingCFR(*mGame, p, i, 1.0, 1.0, 1.0));
                } else {
                    assert(false);
                }
            }
        }
        if (i % 1000 == 0) {
            std::cout << "iteration:" << i << ", cumulative nodes touched: " << mNodeTouchedCnt << ", infosets num: " << mNodeMap.size() << ", expected payoffs: (";
            for (int p = 0; p < mGame->playerNum(); ++p) {
                std::cout << utils[p] << ",";
            }
            std::cout << ")" << std::endl;
        }
        if (i != 0 && i % 10000000 == 0) {
            writeStrategyToBin(i);
        }
    }

    writeStrategyToBin();
}

/// @brief Main procedure of vanilla CFR
/// @param game game
/// @param playerIndex player whose strategy is updated in the current iteration
/// @param pi the probability of reaching the current game node if all players other than the acting player always choose actions leading to the current game node
/// @param po the probability of reaching the current game node if the acting player always chooses actions leading to the current game node
/// @return expected payoff of the specified player at the current game node
template <typename T>
double Trainer<T>::CFR(const T &game, const int playerIndex, const double pi, const double po) {
    ++mNodeTouchedCnt;

    // return payoff for terminal states
    if (game.done()) {
        return game.payoff(playerIndex);
    }

    // chance node turn
    const int actionNum = game.actionNum();
    if (game.isChanceNode()) {
        double nodeUtil = 0.0;
        for (int a = 0; a < actionNum; ++a) {
            auto game_cp(game);
            game_cp.step(a);
            const double chanceProbability = game_cp.chanceProbability();
            nodeUtil += chanceProbability * CFR(game_cp, playerIndex, pi, po * chanceProbability);
        }
        return nodeUtil;
    }

    // get information set string representation
    std::string infoSet = game.infoSetStr();

    // treat static player as chance node
    const int player = game.currentPlayer();
    if (!mUpdate[player]) {
        double nodeUtil = 0.0;
        for (int a = 0; a < actionNum; ++a) {
            auto game_cp(game);
            game_cp.step(a);
            const auto chanceProbability = double(mFixedStrategies[player].at(infoSet)->averageStrategy()[a]);
            nodeUtil += chanceProbability * CFR(game_cp, playerIndex, pi, po * chanceProbability);
        }
        return nodeUtil;
    }

    // get information set node or create it if nonexistant
    Node *node = mNodeMap[infoSet];
    if (node == nullptr) {
        node = new Node(actionNum);
        mNodeMap[infoSet] = node;
    }

    // get current strategy through regret-matching
    const double *strategy = node->strategy();

    // for each action, recursively call CFR with additional history and probability
    double utils[actionNum];
    double nodeUtil = 0;
    for (int a = 0; a < actionNum; ++a) {
        auto game_cp(game);
        game_cp.step(a);
        if (player == playerIndex) {
            utils[a] = CFR(game_cp, playerIndex, pi * strategy[a], po);
        } else {
            utils[a] = CFR(game_cp, playerIndex, pi, po * strategy[a]);
        }
        nodeUtil += strategy[a] * utils[a];
    }

    if (player == playerIndex) {
        // for each action, compute and accumulate counterfactual regret
        for (int a = 0; a < actionNum; ++a) {
            const double regret = utils[a] - nodeUtil;
            const double regretSum = node->regretSum(a) + po * regret;
            node->regretSum(a, regretSum);
        }
        // update average strategy across all training iterations
        node->strategySum(strategy, pi);
    }

    return nodeUtil;
}

/// @brief Main procedure of chance-sampling MCCFR
/// @param game game
/// @param playerIndex player whose strategy is updated in the current iteration
/// @param pi the probability of reaching the current game node if all players other than the acting player always choose actions leading to the current game node
/// @param po the probability of reaching the current game node if the acting player and the chance player always choose actions leading to the current game node
/// @return estimated expected payoff of the specified player at the current game node
template <typename T>
double Trainer<T>::chanceSamplingCFR(const T &game, const int playerIndex, const double pi, const double po) {
    ++mNodeTouchedCnt;

    // return payoff for terminal states
    if (game.done()) {
        return game.payoff(playerIndex);
    }

    // get information set string representation
    std::string infoSet = game.infoSetStr();

    // treat static player as chance node
    const int actionNum = game.actionNum();
    const int player = game.currentPlayer();
    if (!mUpdate[player]) {
        auto game_cp(game);
        auto strategy = mFixedStrategies[player].at(infoSet)->averageStrategy();
        std::discrete_distribution<int> dist(strategy, strategy + actionNum);
        game_cp.step(dist(mEngine));
        return chanceSamplingCFR(game_cp, playerIndex, pi, po);
    }

    // get information set node or create it if nonexistant
    Node *node = mNodeMap[infoSet];
    if (node == nullptr) {
        node = new Node(actionNum);
        mNodeMap[infoSet] = node;
    }

    // get current strategy through regret-matching
    const double *strategy = node->strategy();

    // for each action, recursively call cfr with additional history and probability
    double utils[actionNum];
    double nodeUtil = 0;
    for (int a = 0; a < actionNum; ++a) {
        auto game_cp(game);
        game_cp.step(a);
        if (player == playerIndex) {
            utils[a] = chanceSamplingCFR(game_cp, playerIndex, pi * strategy[a], po);
        } else {
            utils[a] = chanceSamplingCFR(game_cp, playerIndex, pi, po * strategy[a]);
        }
        nodeUtil += strategy[a] * utils[a];
    }

    if (player == playerIndex) {
        // for each action, compute and accumulate counterfactual regret
        for (int a = 0; a < actionNum; ++a) {
            const double regret = utils[a] - nodeUtil;
            const double regretSum = node->regretSum(a) + po * regret;
            node->regretSum(a, regretSum);
        }
        // update average strategy across all training iterations
        node->strategySum(strategy, pi);
    }

    return nodeUtil;
}

/// @brief Main procedure of external-sampling MCCFR
/// @param game game
/// @param playerIndex player whose strategy is updated in the current iteration
/// @return estimated expected payoff of the specified player at the current game node
template <typename T>
double Trainer<T>::externalSamplingCFR(const T &game, const int playerIndex) {
    ++mNodeTouchedCnt;

    // return payoff for terminal states
    if (game.done()) {
        return game.payoff(playerIndex);
    }

    // get information set string representation
    std::string infoSet = game.infoSetStr();

    // external sampling with stochastically-weighted averaging cannot treat static player
    const int actionNum = game.actionNum();
    const int player = game.currentPlayer();
    assert(mUpdate[player] && "External sampling with stochastically-weighted averaging cannot treat static player.");

    // get information set node or create it if nonexistant
    Node *node = mNodeMap[infoSet];
    if (node == nullptr) {
        node = new Node(actionNum);
        mNodeMap[infoSet] = node;
    }

    // get current strategy through regret-matching
    node->updateStrategy();
    const double *strategy = node->strategy();

    // if current player is not the target player, sample a single action and recursively call cfr
    if (player != playerIndex) {
        auto game_cp(game);
        std::discrete_distribution<int> dist(strategy, strategy + actionNum);
        game_cp.step(dist(mEngine));
        const double util = externalSamplingCFR(game_cp, playerIndex);
        // update average strategy
        node->strategySum(strategy, 1.0);
        return util;
    }

    // for each action, recursively call cfr with additional history and probability
    double utils[actionNum];
    double nodeUtil = 0;
    for (int a = 0; a < actionNum; ++a) {
        auto game_cp(game);
        game_cp.step(a);
        utils[a] = externalSamplingCFR(game_cp, playerIndex);
        nodeUtil += strategy[a] * utils[a];
    }

    // for each action, compute and accumulate counterfactual regret
    for (int a = 0; a < actionNum; ++a) {
        const double regret = utils[a] - nodeUtil;
        const double regretSum = node->regretSum(a) + regret;
        node->regretSum(a, regretSum);
    }

    return nodeUtil;
}

/// @brief Main procedure of outcome-sampling MCCFR
/// @param game game
/// @param playerIndex player whose strategy is updated in the current iteration
/// @param pi the probability of reaching the current game node if all players other than the acting player always choose actions leading to the current game node
/// @param po the probability of reaching the current game node if the acting player and the chance player always choose actions leading to the current game node
/// @param s the probability of reaching the current game node if the chance player always chooses actions leading to the current game node and the other players act according to the sample profile
/// @return estimated expected payoff of the specified player at the current game node, and the probability of reaching the terminal game node if the chance player always chooses actions leading to the terminal game node
template <typename T>
std::tuple<double, double> Trainer<T>::outcomeSamplingCFR(const T &game, const int playerIndex, const int iteration , const double pi, const double po, const double s) {
    ++mNodeTouchedCnt;

    // return payoff for terminal states
    if (game.done()) {
        return std::make_tuple(game.payoff(playerIndex) / s, 1.0);
    }

    // get information set string representation
    std::string infoSet = game.infoSetStr();

    // outcome sampling with stochastically-weighted averaging cannot treat static player
    const int actionNum = game.actionNum();
    const int player = game.currentPlayer();
    assert(mUpdate[player] && "Outcome sampling with stochastically-weighted averaging cannot treat static player.");

    // get information set node or create it if nonexistant
    Node *node = mNodeMap[infoSet];
    if (node == nullptr) {
        node = new Node(actionNum);
        mNodeMap[infoSet] = node;
    }

    // get current strategy through regret-matching
    node->updateStrategy();
    const double *strategy = node->strategy();

    // if current player is the target player, sample a single action according to epsilon-on-policy
    // otherwise, sample a single action according to the player's strategy
    const double epsilon = 0.6;
    double probability[actionNum];
    if (player == playerIndex) {
        for (int a = 0; a < actionNum; ++a) {
            probability[a] = (epsilon / (double) actionNum) + (1.0 - epsilon) * strategy[a];
        }
    } else {
        for (int a = 0; a < actionNum; ++a) {
            probability[a] = strategy[a];
        }
    }
    std::discrete_distribution<int> dist(probability, probability + actionNum);
    const int action = dist(mEngine);

    // for sampled action, recursively call cfr with additional history and probability
    double util, pTail;
    auto game_cp(game);
    game_cp.step(action);
    const double newPi = pi * (player == playerIndex ? strategy[action] : 1.0);
    const double newPo = po * (player == playerIndex ? 1.0 : strategy[action]);
    std::tuple<double, double> ret = outcomeSamplingCFR(game_cp, playerIndex, iteration, newPi, newPo, s * probability[action]);
    util = std::get<0>(ret);
    pTail = std::get<1>(ret);
    if (player == playerIndex) {
        // for each action, compute and accumulate counterfactual regret
        const double W = util * po;
        for (int a = 0; a < actionNum; ++a) {
            const double regret = a == action ? W * (1.0 - strategy[action]) * pTail : -W * pTail * strategy[action];
            const double regretSum = node->regretSum(a) + regret;
            node->regretSum(a, regretSum);
        }
    } else {
        // update average strategy
        node->strategySum(strategy, po / s);
    }
    return std::make_tuple(util, pTail * strategy[action]);
}

/// @brief Save the current average strategy as a binary file
/// @param iteration current iteration
template <typename T>
void Trainer<T>::writeStrategyToBin(const int iteration) const {
    for (auto &itr : mNodeMap) {
        for (char c : itr.first) {
            std::cout << int(c);
        }
        std::cout << ":";
        for (int i = 0; i < itr.second->actionNum(); ++i) {
            std::cout << itr.second->averageStrategy()[i] << ",";
        }
        std::cout << std::endl;
    }
    std::string path = iteration > 0 ? "strategy_" + std::to_string(iteration)
                                     : "strategy";
    path += "_" + mModeStr + ".bin";
    std::ofstream ofs(mFolderPath + "/" + path);
    boost::archive::binary_oarchive oa(ofs);
    oa << mNodeMap;
    ofs.close();
}

} // namespace


#ifndef JHMI_VOLUME_IMAGE_HPP_NRC20150429
#define JHMI_VOLUME_IMAGE_HPP_NRC20150429

#include "cube.hpp"
#include "range/v3/view.hpp"
#include "range/v3/core.hpp"
#include "range/v3/view_facade.hpp"
#include <boost/algorithm/string.hpp>
#include <boost/filesystem.hpp>
#include <boost/iostreams/filtering_stream.hpp>
#include <boost/iostreams/filter/zlib.hpp>
#include <fmt/ostream.h>
#include <fstream>

namespace jhmi {
  namespace jhmi_detail {
    template <typename T, typename P, typename V> class by_location_view;
    inline std::string to_dtype(float) { return "float32"; }
    inline std::string to_dtype(double) { return "float64"; }
    inline std::string to_dtype(std::uint8_t) { return "uint8"; }
    inline std::string to_dtype(std::uint32_t) { return "uint32"; }
    inline std::string to_dtype(std::int32_t) { return "int32"; }
    template <typename U, typename T>
    inline std::string to_dtype(boost::units::quantity<U,T>) { return to_dtype(T{}); }
  }

  template <typename T>
  class volume_image {
    using reference = T&;
    using const_reference = T const&;

  public:
    struct physical_pixel_ref {
      physical_pixel_ref(m3 const& pp, int3 const&, reference v) : loc{pp}, value{v} {}
      m3 const loc;
      reference value;
    };
    struct image_pixel_ref {
      image_pixel_ref(m3 const&, int3 const& ip, reference v) : loc{ip}, value{v} {}
      int3 const loc;
      reference value;
    };
    struct pixel_ref {
      pixel_ref(m3 const& pp, int3 const& ip, reference v) : physical_loc{pp}, image_loc{ip}, value{v} {}
      m3 const physical_loc;
      int3 const image_loc;
      reference value;
    };
    struct physical_pixel {
      physical_pixel(m3 const& pp, int3 const&, const_reference v) : loc{pp}, value{v} {}
      m3 const loc;
      const_reference value;
    };
    struct image_pixel {
      image_pixel(m3 const&, int3 const& ip, const_reference v) : loc{ip}, value{v} {}
      int3 const loc;
      const_reference value;
    };
    struct pixel {
      pixel(m3 const& pp, int3 const& ip, const_reference v) : physical_loc{pp}, image_loc{ip}, value{v} {}
      m3 const physical_loc;
      int3 const image_loc;
      const_reference value;
    };

    volume_image() = default;
    volume_image(int3 const& size, cube<m3> const& physical_extents)
      : physical_extents_(physical_extents),
        w_(size.x), h_(size.y), d_(size.z),
        volume_(new T[w_*h_*d_]()),//Note, we default initialize each element of volume_
        half_pixel_sizes_{element_divide(jhmi::dimensions(physical_extents), 2. * size)}
    {}

    explicit volume_image(boost::filesystem::path const& filename) {
      std::ifstream file(filename.string(), std::ios::binary);
      if (!file.is_open()) {
        throw std::runtime_error(fmt::format(
          "Invalid file \"{}\" provided to volume_image", filename.string()));
      }
      boost::iostreams::filtering_istream in;
      in.push(boost::iostreams::zlib_decompressor{});
      in.push(file);
      std::string line;
      getline(in, line);
      std::vector<std::string> e;
      boost::split(e, line, boost::is_any_of(" "), boost::token_compress_on);
      physical_extents_ = cube<m3>{dbl3{stod(e[0]), stod(e[1]), stod(e[2])}*mm,
                                   dbl3{stod(e[3]), stod(e[4]), stod(e[5])}*mm};
      getline(in, line);
      w_ = stoi(line);
      getline(in, line);
      h_ = stoi(line);
      getline(in, line);
      d_ = stoi(line);
      volume_.reset(new T[w_ * h_ * d_]);
      getline(in, line);
      if (line != jhmi_detail::to_dtype(T{})) {
        throw std::invalid_argument(fmt::format(
          "Invalid attempt to construct {} from {}", jhmi_detail::to_dtype(T()), line));
      }
      in.read(reinterpret_cast<char*>(volume_.get()), w_ * h_ * d_ * sizeof(T));
      half_pixel_sizes_ = element_divide(
        jhmi::dimensions(physical_extents_), 2. * int3{w_,h_,d_});
    }

    dbl3 to_index(m3 const& pt) const {
      return dbl3(element_divide(
        element_multiply(pt - physical_extents_.ul() - half_pixel_sizes_, dbl3(w_,h_,d_)),
        jhmi::dimensions(physical_extents_)));
    }

    reference operator()(m3 const& pt) {
      return (*this)(int3(to_index(pt)));
    }
    const_reference operator()(m3 const& pt) const {
      return (*this)(int3(to_index(pt)));
    }

    reference operator()(int x, int y, int z) {
      assert(x + y * w_ + z * w_ * h_ < w_ * h_ * d_);
      return volume_[x + y * w_ + z * w_ * h_];
    }
    reference operator()(int3 const& pt) { return (*this)(pt.x, pt.y, pt.z); }
    const_reference operator()(int x, int y, int z) const {
      assert(x + y * w_ + z * w_ * h_ < w_ * h_ * d_);
      return volume_[x + y * w_ + z * w_ * h_];
    }
    const_reference operator()(int3 const& pt) const { return (*this)(pt.x, pt.y, pt.z); }

    //TODO: Is this returning a pointer to const?  I don't think so...
    auto begin() const { return volume_.get(); }
    auto begin() { return volume_.get(); }

    auto end() const { return volume_.get() + w_ * h_ * d_; }
    auto end() { return volume_.get() + w_ * h_ * d_; }

    int width() const { return w_; }
    int height() const { return h_; }
    int depth() const { return d_; }

    int3 dimensions() const { return {w_, h_, d_}; }

    void write(boost::filesystem::path const& filename) const {
      std::ofstream file{filename.string(), std::ios::binary};
      boost::iostreams::filtering_ostream out;
      out.push(boost::iostreams::zlib_compressor{});
      out.push(file);
      out << fmt::format("{} {} {} {} {} {}\n{}\n{}\n{}\n{}\n",
        physical_extents_.ul().x.value()*1000,
        physical_extents_.ul().y.value()*1000,
        physical_extents_.ul().z.value()*1000,
        physical_extents_.lr().x.value()*1000,
        physical_extents_.lr().y.value()*1000,
        physical_extents_.lr().z.value()*1000,
        width(), height(), depth(), jhmi_detail::to_dtype(T()));
      write_binary(out);
    }

  private:
    template <typename Stream>
    void write_binary(Stream& out) const {
      out.write(reinterpret_cast<char const*>(volume_.get()),
                w_ * h_ * d_ * sizeof(T));
    }

    template <typename U, typename P, typename V> friend class jhmi_detail::by_location_view;
    friend cube<m3> extents(volume_image<T> const& v) {
      return v.physical_extents_;
    }
    cube<m3> physical_extents_;
    int w_, h_, d_;
    std::unique_ptr<T[]> volume_;
    m3 half_pixel_sizes_;
  };

  namespace jhmi_detail {
  template <typename T, typename P, typename V>
  class by_location_view : public ranges::view_facade<by_location_view<T,P,V>> {
  public:
    friend struct ranges::range_access;
    V* img_;
    m3 step_;
    m3 center_;
    struct cursor {
      V* img_;
      m3 mul_;
      m3 step_;
      int3 iul_;
      int3 ilr_;
      int3 ipt_;

      cursor() = default;

      bool equal(cursor const& other) const { return ipt_ == other.ipt_; }
      auto read() const {
        return P{mul_ + element_multiply(dbl3{ipt_}, step_), ipt_, (*img_)(ipt_)};
      }
      void next() {
        if (++ipt_.x >= ilr_.x) {
          ipt_.x = iul_.x;
          if (++ipt_.y >= ilr_.y) {
            ipt_.y = iul_.y;
            ++ipt_.z;
          }
        }
      }
    };

    cursor make_cursor(bool do_ul) const {
      auto pe = img_->physical_extents_;
      auto d = img_->dimensions();
      int3 iul{0,0,0};
      int3 iend{0,0,d.z};
      auto ipt = do_ul ? iul : iend;

      auto hstep = step_ / 2.;
      auto mul = element_multiply(ceil(element_divide(pe.ul() - center_, hstep)), hstep) + center_;
      return {img_, mul + hstep, step_, iul, d, ipt};
    }

  public:
    cursor begin_cursor() const {
      return make_cursor(true);
    }
    cursor end_cursor() const {
      return make_cursor(false);
    }

    explicit by_location_view(V& img) : img_(&img),
      step_(element_divide(dimensions(extents(img)), dbl3{img.dimensions()})),
      center_{extents(img).ul() + step_ / 2.} {}
    by_location_view(V& img, m3 const& step, m3 const& center)
      : img_(&img), step_(step), center_{center} {}
  };
  }//jhmi_detail

  template <typename T>
  struct by_const_physical_location_view : jhmi_detail::by_location_view<T, typename volume_image<T>::physical_pixel, volume_image<T> const> {
    using jhmi_detail::by_location_view<T, typename volume_image<T>::physical_pixel, volume_image<T> const>::by_location_view;
  };
  template <typename T>
  struct by_physical_location_view : jhmi_detail::by_location_view<T, typename volume_image<T>::physical_pixel_ref, volume_image<T>> {
    using jhmi_detail::by_location_view<T, typename volume_image<T>::physical_pixel_ref, volume_image<T>>::by_location_view;
  };
  template <typename T>
  struct by_const_image_location_view : jhmi_detail::by_location_view<T, typename volume_image<T>::image_pixel, volume_image<T> const> {
    using jhmi_detail::by_location_view<T, typename volume_image<T>::image_pixel, volume_image<T> const>::by_location_view;
  };
  template <typename T>
  struct by_image_location_view : jhmi_detail::by_location_view<T, typename volume_image<T>::image_pixel_ref, volume_image<T>> {
    using jhmi_detail::by_location_view<T, typename volume_image<T>::image_pixel_ref, volume_image<T>>::by_location_view;
  };
  template <typename T>
  struct by_const_location_view : jhmi_detail::by_location_view<T, typename volume_image<T>::pixel, volume_image<T> const> {
    using jhmi_detail::by_location_view<T, typename volume_image<T>::pixel, volume_image<T> const>::by_location_view;
  };
  template <typename T>
  struct by_location_view : jhmi_detail::by_location_view<T, typename volume_image<T>::pixel_ref, volume_image<T>> {
    using jhmi_detail::by_location_view<T, typename volume_image<T>::pixel_ref, volume_image<T>>::by_location_view;
  };

  namespace view {
    struct by_physical_location_fn {
      template <typename T> auto operator()(volume_image<T> const& img) const {
        return by_const_physical_location_view<T>{img};
      }
      template <typename T> auto operator()(volume_image<T>& img) const {
        return by_physical_location_view<T>{img};
      }
    };
    template <typename T> auto operator|(T& img, by_physical_location_fn fn) {
      return fn(img);
    }
    struct by_image_location_fn {
      template <typename T> auto operator()(volume_image<T> const& img) const {
        return by_const_image_location_view<T>{img};
      }
      template <typename T> auto operator()(volume_image<T>& img) const {
        return by_image_location_view<T>{img};
      }
    };
    template <typename T> auto operator|(T& img, by_image_location_fn fn) {
      return fn(img);
    }
    struct by_location_fn {
      template <typename T> auto operator()(volume_image<T> const& img) const {
        return by_const_location_view<T>{img};
      }
      template <typename T> auto operator()(volume_image<T>& img) const {
        return by_location_view<T>{img};
      }
    };
    template <typename T> auto operator|(T& img, by_location_fn fn) {
      return fn(img);
    }
    constexpr by_image_location_fn by_image_location{};
    constexpr by_physical_location_fn by_physical_location{};
    constexpr by_location_fn by_location{};

    struct equal_step {
      m step_;
      m3 center_;
      explicit equal_step(m step, m3 const& center = m3{}) : step_{step}, center_{center} {}
      template <typename T> auto operator()(volume_image<T> const& img) const {
        return by_const_location_view<T>(img, dbl3{1,1,1}*step_, center_);
      }
      template <typename T> auto operator()(volume_image<T>& img) const {
        return by_location_view<T>(img, dbl3{1,1,1}*step_, center_);
      }
    };
    template <typename T> auto operator|(T& img, equal_step fn) {
      return fn(img);
    }

    struct x_slice {
      int x1, x2, y, z;
      x_slice(int x1, int x2, int y, int z) : x1(x1), x2(x2), y(y), z(z) {}

      template <typename T> auto operator()(volume_image<T> const& img) const {
        return ranges::make_iterator_range(&img(x1,y,z), &img(x2,y,z));
      }
      template <typename T> auto operator()(volume_image<T>& img) const {
        return ranges::make_iterator_range(&img(x1,y,z), &img(x2,y,z));
      }
    };
    template <typename T> auto operator|(T& img, x_slice fn) {
      return fn(img);
    }
  }

  template <typename T>
  auto gaussian_filter(volume_image<T>& in, double sigma, int num_dev = 3) {
    volume_image<double> out{in.dimensions(), extents(in)};
    volume_image<double> tmp{in.dimensions(), extents(in)};
    volume_image<double> tmp1{in.dimensions(), extents(in)};
    int range = static_cast<int>(std::ceil(num_dev * sigma));
    auto filter = ranges::view::closed_indices(-range, range)
      | ranges::view::transform([&](int x) {
           return /*1. / (sigma * 2.506628274) */ std::exp(-x*x/(2*sigma*sigma)); })
      | ranges::to_vector;

    auto ds = inflate(cube<int3>{int3{}, in.dimensions()}, -range*int3{1,1,1});
    traverse(ds, 1, [&](int3 const& pt) {
      for (int i = 0; i < filter.size(); ++i) {
        auto new_pt = pt + int3{i-range,0,0};
        tmp1(pt) += in(new_pt) * filter[i];
      }
    });
    traverse(ds, 1, [&](int3 const& pt) {
      for (int i = 0; i < filter.size(); ++i) {
        auto new_pt = pt + int3{0,i-range,0};
        tmp(pt) += tmp1(new_pt) * filter[i];
      }
    });
    traverse(ds, 1, [&](int3 const& pt) {
      for (int i = 0; i < filter.size(); ++i) {
        auto new_pt = pt + int3{0,0,i-range};
        out(pt) += tmp(new_pt) * filter[i];
      }
    });
    return out;
  }
  template <typename T, typename U> auto convolve(
      volume_image<T> const& lhs, volume_image<U> const& rhs) {
    auto half_size = int3(ceil(rhs.dimensions() / 2.));
    cube<int3> ds{int3{}, lhs.dimensions() - rhs.dimensions()};
    using Out = decltype(T{} * U{});
    volume_image<Out> out_img{lhs.dimensions(), extents(lhs)};
    //Reverse rhs once, simplify calculations later.
    auto kernel = rhs;
    traverse(cube<int3>{int3{}, rhs.dimensions()}, 1, [&](int3 const& pt) {
      kernel(rhs.dimensions() - int3{1,1,1} - pt) = rhs(pt);
    });
    traverse(ds, 1, [&](int3 const& ul) {
      Out out;
      traverse(cube<int3>{int3{}, kernel.dimensions()}, 1, [&](int3 const& pt) {
        out += lhs(ul + pt) * kernel(pt);
      });
      out_img(ul+half_size) = out;
    });
    return out_img;
  }
  namespace jhmi_detail {
    struct prev_type{}; static const constexpr prev_type prev{};
    struct next_type{}; static const constexpr next_type next{};
    struct Top {
      int operator()() const { return 0; }
      int operator()(prev_type) const { return 0; }
      int operator()(next_type) const { return 1; }
    };
    struct Mid {
      int v;
      explicit Mid(int v) : v(v) {}
      int operator()() const { return v; }
      int operator()(prev_type) const { return v-1; }
      int operator()(next_type) const { return v+1; }
    };
    struct Bottom {
      int v;
      explicit Bottom(int v) : v(v) {}
      int operator()() const { return v; }
      int operator()(prev_type) const { return v-1; }
      int operator()(next_type) const { return v; }
    };
    template <typename T, typename U1, typename U2, typename F>
    void traverse_x(F f, U1 y, U2 z, volume_image<T> const& in, volume_image<T>& out) {
      out(0, y(), z()) = f(in(0, y(), z()), in(1, y(), z()),
                           in(0, y(prev), z()), in(0, y(next), z()),
                           in(0, y(), z(prev)), in(0, y(), z(next)));
      auto l = in.width()-1;
      for (int x = 1; x < l; ++x)
        out(x, y(), z()) = f(in(x, y(), z()), in(x-1, y(), z()), in(x+1, y(), z()),
                             in(x, y(prev), z()), in(x, y(next), z()),
                             in(x, y(), z(prev)), in(x, y(), z(next)));
      out(l, y(), z()) = f(in(l, y(), z()), in(l-1, y(), z()),
                           in(l, y(prev), z()), in(l, y(next), z()),
                           in(l, y(), z(prev)), in(l, y(), z(next)));
    }
    template <typename T, typename U1, typename F>
    void traverse_y(F f, U1 z, volume_image<T> const& in, volume_image<T>& out) {
      traverse_x(f, Top(), z, in, out);
      for (int y = 1; y < in.height()-1; ++y)
        traverse_x(f, Mid(y), z, in, out);
      traverse_x(f, Bottom(in.height()-1), z, in, out);
    }
    template <typename T, typename F>
    void traverse_z(F f, volume_image<T> const& in, volume_image<T>& out) {
      traverse_y(f, Top(), in, out);
      for (int z = 1; z < in.depth()-1; ++z)
        traverse_y(f, Mid(z), in, out);
      traverse_y(f, Bottom(in.depth()-1), in, out);
    }
  }//jhmi_detail
  template <typename T> auto erode(volume_image<T> const& img) {
    auto out = volume_image<T>{img.dimensions(), extents(img)};
    jhmi_detail::traverse_z([](auto ...vals) { return std::min({vals...}); }, img, out);
    return out;
  }
  template <typename T> auto dilate(volume_image<T> const& img) {
    auto out = volume_image<T>{img.dimensions(), extents(img)};
    jhmi_detail::traverse_z([](auto ...vals) { return std::max({vals...}); }, img, out);
    return out;
  }
}

#endif


#include "MBTB_JointR.hpp"
#include "NewtonEulerJointR.hpp"
#include <boost/math/quaternion.hpp>
#include "op3x3.h"
#include "RotationQuaternion.hpp"
//#define MBTB_JOINTR_DEBUG

MBTB_JointR::MBTB_JointR()
{
  _M.reset(new SimpleMatrix(6,6));
  _F.reset(new SiconosVector(6));
}

/*
 *
 *
 * ML_A_abs = M_FA_A + M_FB_A = F2/\BA
 * the system is :
 * FL=F1+F2
 * F1.e0 = F2.e0
 * ML_A_abs . ei = F2 /\ BA .ei for i=1,2
 *
 */

void MBTB_JointR::computeEquivalentForces()
{
  if(!_G0C1)
    return;
  double q1=_ds1->q()->getValue(3);
  double q2=_ds1->q()->getValue(4);
  double q3=_ds1->q()->getValue(5);
  double q4=_ds1->q()->getValue(6);
  ::boost::math::quaternion<double>    quattrf(q1,q2,q3,q4);
  ::boost::math::quaternion<double>    cquattrf(q1,-q2,-q3,-q4);
  ::boost::math::quaternion<double>    quatbuff;
  SP::SiconosVector Blambda=_jointR->contactForce();
  SiconosVector FL(3);
  FL.setValue(0,Blambda->getValue(0));
  FL.setValue(1,Blambda->getValue(1));
  FL.setValue(2,Blambda->getValue(2));

  SiconosVector ML_G(3);
  SiconosVector ML_G_abs(3);
  ML_G.setValue(0,Blambda->getValue(3));
  ML_G.setValue(1,Blambda->getValue(4));
  ML_G.setValue(2,Blambda->getValue(5));
  SP::SiconosVector spML_G_abs(new SiconosVector(3));
  *spML_G_abs = ML_G;
  changeFrameBodyToAbs(_ds1->q(),spML_G_abs);
  ML_G_abs = * spML_G_abs;
#ifdef MBTB_JOINTR_DEBUG
  printf("MBTB_JointR::computeEquivalentForces Blambda\n");
  Blambda->display();
  printf("MBTB_JointR::computeEquivalentForces ML_G_abs (in abs frame):\n");
  ML_G_abs.display();
#endif

  SiconosVector AB0(3);
  AB0=(*_G0C2)-(*_G0C1);
  ::boost::math::quaternion<double>    quatAB0(0,AB0.getValue(0),AB0.getValue(1),AB0.getValue(2));
  quatbuff = quattrf*quatAB0*cquattrf;
  SiconosVector AB(3);
  AB.setValue(0,quatbuff.R_component_2());
  AB.setValue(1,quatbuff.R_component_3());
  AB.setValue(2,quatbuff.R_component_4());
#ifdef MBTB_JOINTR_DEBUG
  printf("MBTB_JointR::computeEquivalentForces AB0:\n");
  AB0.display();
  printf("MBTB_JointR::computeEquivalentForces AB:\n");
  AB.display();
#endif

  ::boost::math::quaternion<double>    quatG0C1(0,_G0C1->getValue(0),_G0C1->getValue(1),_G0C1->getValue(2));
  quatbuff = quattrf*quatG0C1*cquattrf;
  SiconosVector GA(3);
  GA.setValue(0,quatbuff.R_component_2());
  GA.setValue(1,quatbuff.R_component_3());
  GA.setValue(2,quatbuff.R_component_4());
#ifdef MBTB_JOINTR_DEBUG
  printf("MBTB_JointR::computeEquivalentForces GA:\n");
  GA.display();
#endif
  SiconosVector ML_A_abs(3);
  SiconosVector GA_FL(3);
  cross_product(GA,FL,GA_FL);
  ML_A_abs=ML_G_abs - GA_FL;
#ifdef MBTB_JOINTR_DEBUG
  printf("MBTB_JointR::computeEquivalentForces GA_FL:\n");
  GA_FL.display();
  printf("MBTB_JointR::computeEquivalentForces ML_A_abs :\n");
  ML_A_abs.display();
#endif
  double normAB=1.0/AB.norm2();
  double e0_x =normAB*AB.getValue(0);
  double e0_y =normAB*AB.getValue(1);
  double e0_z =normAB*AB.getValue(2);
#ifdef MBTB_JOINTR_DEBUG
  printf("MBTB_JointR::computeEquivalentForces e0_x . ML_A_abs (must be zero):%e\n",
         e0_x*ML_A_abs.getValue(0)+e0_y*ML_A_abs.getValue(1)+e0_z*ML_A_abs.getValue(2));
#endif
  double  e1_x,e1_y,e1_z,e2_x,e2_y,e2_z;
  int info = orthoBaseFromVector(&e0_x,&e0_y,&e0_z,&e1_x,&e1_y,&e1_z,&e2_x,&e2_y,&e2_z);
  if(info)
  {
    std::cout << "something wrong happened in  orthoBaseFromVector"<<std::endl;
  }
  /*
   * ML_A_abs = M_FA_A + M_FB_A = F2/\BA
   * the system is :
   * FL=F1+F2
   * F1.e0 = F2.e0
   * ML_A_abs . ei = F2 /\ BA .ei for i=1,2
   *
   */
  /*Fill _M*/
  _M->zero();
  //FL= F1+F2.
  _M->setValue(0,0,1);
  _M->setValue(0,3,1);
  _M->setValue(1,1,1);
  _M->setValue(1,4,1);
  _M->setValue(2,2,1);
  _M->setValue(2,5,1);
  /*F1.e0=F2.e0*/
  _M->setValue(3,0,e0_x);
  _M->setValue(3,1,e0_y);
  _M->setValue(3,2,e0_z);
  _M->setValue(3,3,-e0_x);
  _M->setValue(3,4,-e0_y);
  _M->setValue(3,5,-e0_z);

  /*
   *             F2x  BAx    F2y*BAz-F2z*BAy
   * FB/\BA . ei=F2y/\BAy.ei=F2z*BAx-F2x*BAz .ei = F2x*(BAy *ei_z-BAz*ei_y)+F2y*(BAz*ei_x-BAx*ei_z)+F2z*(BAx*ei_y-BAy*ei_x)
   *             F2z  BAz    F2x*BAy-F2y*BAx
   *
   */
  _M->setValue(4,3,-AB.getValue(1) *e1_z+AB.getValue(2)*e1_y);
  _M->setValue(4,4,-AB.getValue(2) *e1_x+AB.getValue(0)*e1_z);
  _M->setValue(4,5,-AB.getValue(0) *e1_y+AB.getValue(1)*e1_x);

  _M->setValue(5,3,-AB.getValue(1) *e2_z+AB.getValue(2)*e2_y);
  _M->setValue(5,4,-AB.getValue(2) *e2_x+AB.getValue(0)*e2_z);
  _M->setValue(5,5,-AB.getValue(0) *e2_y+AB.getValue(1)*e2_x);
#ifdef MBTB_JOINTR_DEBUG
  printf("MBTB_JointR::computeEquivalentForces the sytem M is:\n");
  _M->display();
#endif
  _F->setValue(0,FL.getValue(0));
  _F->setValue(1,FL.getValue(1));
  _F->setValue(2,FL.getValue(2));
  _F->setValue(3,0);
  _F->setValue(4,e1_x * ML_A_abs.getValue(0) + e1_y * ML_A_abs.getValue(1) +e1_z * ML_A_abs.getValue(2));
  _F->setValue(5,e2_x * ML_A_abs.getValue(0) + e2_y * ML_A_abs.getValue(1) +e2_z * ML_A_abs.getValue(2));
#ifdef MBTB_JOINTR_DEBUG
  printf("MBTB_JointR::computeEquivalentForces Mx=b, b:");
  _F->display();
#endif

  /*Solve the system.*/
  try
  {
    _M->PLUFactorizationInPlace();
    _M->PLUSolve(*_F);
#ifdef MBTB_JOINTR_DEBUG
    printf("MBTB_JointR::computeEquivalentForces Forces equivalent:");
    _F->display();
    printf("MBTB_JointR::computeEquivalentForces checking ML_G_abs = MF1_G+MF2_G:");
    SiconosVector Maux1(3),Maux2(3),F1(3),F2(3),GC1(3),GC2(3),dif(3);
    F1.setValue(0,_F->getValue(0));
    F1.setValue(1,_F->getValue(1));
    F1.setValue(2,_F->getValue(2));
    F2.setValue(0,_F->getValue(3));
    F2.setValue(1,_F->getValue(4));
    F2.setValue(2,_F->getValue(5));
    //FL=F1+F2
    dif = FL-F1-F2;
    printf("MBTB_JointR::computeEquivalentForces  FL-F1-F2(must be zero):\n");
    dif.display();
    //MF1_G=F1/\C1G
    cross_product(GA,F1,Maux1);
    ::boost::math::quaternion<double>    quatG0C2(0,_G0C2->getValue(0),_G0C2->getValue(1),_G0C2->getValue(2));
    quatbuff = quattrf*quatG0C2*cquattrf;
    SiconosVector GB(3);
    GB.setValue(0,quatbuff.R_component_2());
    GB.setValue(1,quatbuff.R_component_3());
    GB.setValue(2,quatbuff.R_component_4());
    cross_product(GB,F2,Maux2);
    dif=Maux1+Maux2;
    printf("MBTB_JointR::computeEquivalentForces  momentum (must be ML_G_abs):\n");
    dif.display();
    dif=Maux1+Maux2 - ML_G_abs;
    printf("MBTB_JointR::computeEquivalentForces  dif momentum(must be zero):\n");
    dif.display();
#endif
  }
  catch(const std::exception& e)
  {
    printf("MBTB_JointR: exception caught.\n");
  }

}


#ifndef STAN_MATH_PRIM_SCAL_PROB_FRECHET_LPDF_HPP
#define STAN_MATH_PRIM_SCAL_PROB_FRECHET_LPDF_HPP

#include <boost/random/weibull_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <stan/math/prim/scal/meta/operands_and_partials.hpp>
#include <stan/math/prim/scal/err/check_consistent_sizes.hpp>
#include <stan/math/prim/scal/err/check_nonnegative.hpp>
#include <stan/math/prim/scal/err/check_not_nan.hpp>
#include <stan/math/prim/scal/err/check_positive.hpp>
#include <stan/math/prim/scal/err/check_positive_finite.hpp>
#include <stan/math/prim/scal/fun/size_zero.hpp>
#include <stan/math/prim/scal/fun/log1m.hpp>
#include <stan/math/prim/scal/fun/multiply_log.hpp>
#include <stan/math/prim/scal/fun/value_of.hpp>
#include <stan/math/prim/scal/meta/length.hpp>
#include <stan/math/prim/scal/meta/is_constant_struct.hpp>
#include <stan/math/prim/scal/meta/scalar_seq_view.hpp>
#include <stan/math/prim/scal/meta/VectorBuilder.hpp>
#include <stan/math/prim/scal/meta/partials_return_type.hpp>
#include <stan/math/prim/scal/meta/return_type.hpp>
#include <stan/math/prim/scal/fun/constants.hpp>
#include <stan/math/prim/scal/meta/include_summand.hpp>
#include <cmath>

namespace stan {
namespace math {

// Frechet(y|alpha, sigma)     [y > 0;  alpha > 0;  sigma > 0]
// FIXME: document
template <bool propto, typename T_y, typename T_shape, typename T_scale>
typename return_type<T_y, T_shape, T_scale>::type frechet_lpdf(
    const T_y& y, const T_shape& alpha, const T_scale& sigma) {
  static const char* function = "frechet_lpdf";
  typedef typename stan::partials_return_type<T_y, T_shape, T_scale>::type
      T_partials_return;

  using std::log;

  if (size_zero(y, alpha, sigma))
    return 0.0;

  T_partials_return logp(0.0);
  check_positive(function, "Random variable", y);
  check_positive_finite(function, "Shape parameter", alpha);
  check_positive_finite(function, "Scale parameter", sigma);
  check_consistent_sizes(function, "Random variable", y, "Shape parameter",
                         alpha, "Scale parameter", sigma);

  if (!include_summand<propto, T_y, T_shape, T_scale>::value)
    return 0.0;

  scalar_seq_view<T_y> y_vec(y);
  scalar_seq_view<T_shape> alpha_vec(alpha);
  scalar_seq_view<T_scale> sigma_vec(sigma);
  size_t N = max_size(y, alpha, sigma);

  VectorBuilder<include_summand<propto, T_shape>::value, T_partials_return,
                T_shape>
      log_alpha(length(alpha));
  for (size_t i = 0; i < length(alpha); i++)
    if (include_summand<propto, T_shape>::value)
      log_alpha[i] = log(value_of(alpha_vec[i]));

  VectorBuilder<include_summand<propto, T_y, T_shape>::value, T_partials_return,
                T_y>
      log_y(length(y));
  for (size_t i = 0; i < length(y); i++)
    if (include_summand<propto, T_y, T_shape>::value)
      log_y[i] = log(value_of(y_vec[i]));

  VectorBuilder<include_summand<propto, T_shape, T_scale>::value,
                T_partials_return, T_scale>
      log_sigma(length(sigma));
  for (size_t i = 0; i < length(sigma); i++)
    if (include_summand<propto, T_shape, T_scale>::value)
      log_sigma[i] = log(value_of(sigma_vec[i]));

  VectorBuilder<include_summand<propto, T_y, T_shape, T_scale>::value,
                T_partials_return, T_y>
      inv_y(length(y));
  for (size_t i = 0; i < length(y); i++)
    if (include_summand<propto, T_y, T_shape, T_scale>::value)
      inv_y[i] = 1.0 / value_of(y_vec[i]);

  VectorBuilder<include_summand<propto, T_y, T_shape, T_scale>::value,
                T_partials_return, T_y, T_shape, T_scale>
      sigma_div_y_pow_alpha(N);
  for (size_t i = 0; i < N; i++)
    if (include_summand<propto, T_y, T_shape, T_scale>::value) {
      const T_partials_return alpha_dbl = value_of(alpha_vec[i]);
      sigma_div_y_pow_alpha[i]
          = pow(inv_y[i] * value_of(sigma_vec[i]), alpha_dbl);
    }

  operands_and_partials<T_y, T_shape, T_scale> ops_partials(y, alpha, sigma);
  for (size_t n = 0; n < N; n++) {
    const T_partials_return alpha_dbl = value_of(alpha_vec[n]);
    if (include_summand<propto, T_shape>::value)
      logp += log_alpha[n];
    if (include_summand<propto, T_y, T_shape>::value)
      logp -= (alpha_dbl + 1.0) * log_y[n];
    if (include_summand<propto, T_shape, T_scale>::value)
      logp += alpha_dbl * log_sigma[n];
    if (include_summand<propto, T_y, T_shape, T_scale>::value)
      logp -= sigma_div_y_pow_alpha[n];

    if (!is_constant_struct<T_y>::value) {
      const T_partials_return inv_y_dbl = value_of(inv_y[n]);
      ops_partials.edge1_.partials_[n]
          += -(alpha_dbl + 1.0) * inv_y_dbl
             + alpha_dbl * sigma_div_y_pow_alpha[n] * inv_y_dbl;
    }
    if (!is_constant_struct<T_shape>::value)
      ops_partials.edge2_.partials_[n]
          += 1.0 / alpha_dbl
             + (1.0 - sigma_div_y_pow_alpha[n]) * (log_sigma[n] - log_y[n]);
    if (!is_constant_struct<T_scale>::value)
      ops_partials.edge3_.partials_[n] += alpha_dbl / value_of(sigma_vec[n])
                                          * (1 - sigma_div_y_pow_alpha[n]);
  }
  return ops_partials.build(logp);
}

template <typename T_y, typename T_shape, typename T_scale>
inline typename return_type<T_y, T_shape, T_scale>::type frechet_lpdf(
    const T_y& y, const T_shape& alpha, const T_scale& sigma) {
  return frechet_lpdf<false>(y, alpha, sigma);
}

}  // namespace math
}  // namespace stan
#endif


/**
 * Parrot Drones Awesome Video Viewer
 * Desktop application
 *
 * Copyright (c) 2018 Parrot Drones SAS
 * Copyright (c) 2016 Aurelien Barre
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above copyright
 *     notice, this list of conditions and the following disclaimer in the
 *     documentation and/or other materials provided with the distribution.
 *   * Neither the name of the copyright holders nor the names of its
 *     contributors may be used to endorse or promote products derived from
 *     this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include <Eigen/Eigen>

#include "pdraw_desktop.h"

void pdraw_desktop_view_create_matrices(struct pdraw_desktop *self,
					unsigned int width,
					unsigned int height,
					float *view_mat,
					float *proj_mat,
					float near,
					float far)
{
	float w = 1.f;
	float h = (float)width / (float)height;
	float a = (far + near) / (far - near);
	float b = -((2 * far * near) / (far - near));

	Eigen::Matrix4f p_mat;
	p_mat << w, 0, 0, 0, 0, h, 0, 0, 0, 0, a, b, 0, 0, 1, 0;

	Eigen::Matrix4f v_mat = Eigen::Matrix4f::Identity();

	unsigned int i, j;
	for (i = 0; i < 4; i++) {
		for (j = 0; j < 4; j++)
			view_mat[j * 4 + i] = v_mat(i, j);
	}
	for (i = 0; i < 4; i++) {
		for (j = 0; j < 4; j++)
			proj_mat[j * 4 + i] = p_mat(i, j);
	}
}


#include "QAlg/QITE/QITE.h"
#include "Core/QuantumMachine/QuantumMachineFactory.h"
#include <Eigen/Eigenvalues>

QPANDA_BEGIN

QITE::QITE()
{
}

int QITE::exec()
{
    initEnvironment();
    srand((int)time(0));

    auto tmp_ansatz = m_ansatz;
    for (int t = 0; t < m_upthrow_num; t++)
    {
        for (int i = 0; i < m_iter_num; i++)
        {
            calcParaA();
            calcParaC();

            Eigen::MatrixXd A_inverse = pseudoinverse(m_A);
            Eigen::VectorXd theta_dot = -A_inverse * m_C;
            double delta_k = m_delta_tau * pow(m_Q, i);

            for (int j = 0; j < m_theta_index_vec.size(); j++)
            {
                auto t_j = m_theta_index_vec[j];
                double r = (rand() / double(RAND_MAX));
                if (m_update_mode == UpdateMode::GD_VALUE)
                {
                    m_ansatz[t_j].theta += theta_dot[j];
                }
                else if(m_update_mode == UpdateMode::GD_DIRECTION)
                {
                    if (theta_dot[j] > 0)
                    {
                        m_ansatz[t_j].theta -= delta_k * r;
                    }
                    else
                    {
                        m_ansatz[t_j].theta += delta_k * r;
                    }
                }
                else
                {
                    return -1;
                }

                int n_period = int(m_ansatz[t_j].theta / (2 * PI));
                if (m_ansatz[t_j].theta < 0)
                {
                    m_ansatz[t_j].theta += (n_period + 1) * 2 * PI;
                }
                else
                {
                    m_ansatz[t_j].theta -= n_period * 2 * PI;
                }
            }

            double tmp_exp = getExpectation(m_ansatz);
            m_log_writer << tmp_exp << std::endl;

            if (tmp_exp < m_expectation)
            {
                m_expectation = tmp_exp;
                m_best_ansatz = m_ansatz;
            }
        }
    }

    std::fstream para_out;
    para_out.open(m_log_file + "_para.txt", std::ios::out);
    para_out << "Last_para:" << std::endl;
    for (int i = 0; i < m_theta_index_vec.size(); i++)
    {
        para_out << m_ansatz[m_theta_index_vec[i]].theta << ", ";
    }

    para_out << std::endl << "Best_para:" << std::endl;
    for (int i = 0; i < m_theta_index_vec.size(); i++)
    {
        para_out << m_best_ansatz[m_theta_index_vec[i]].theta << ", ";
    }
    para_out.close();
    m_log_writer.close();

    m_ansatz = tmp_ansatz;
    //m_machine->Free_Qubits(m_qlist);

    return 0;
}

prob_tuple QITE::getResult()
{
    QProg prog;
    prog << constructCircuit(m_best_ansatz);
    m_machine->directlyRun(prog);

    auto temp = dynamic_cast<IdealMachineInterface*>(m_machine.get());
    if (nullptr == temp)
    {
        QCERR("m_machine is not ideal machine");
        throw std::runtime_error("m_machine is not ideal machine");
    }
    auto measure_qubits = m_qlist;
    measure_qubits.pop_back();
    auto result = temp->PMeasure(measure_qubits, -1);

    std::fstream fout;
    fout.open(m_log_file + "_measure.txt", std::ios::out);

    std::cout << "Measure result: " << std::endl;
    for (auto& i : result)
    {
        if (fabs(i.second) < 1e-4)
        {
            break;
        }
        std::cout << i.first << " " << i.second << std::endl;
        fout << i.first << " " << i.second << std::endl;
    }

    fout.close();

    return result;
}

void QITE::initEnvironment()
{
    if (!m_log_file.empty())
    {
        m_log_writer.open(m_log_file, std::ios::out);
    }
    else
    {
        m_log_writer.open("test.log", std::ios::out);
    }

    m_machine.reset(QuantumMachineFactory::GetFactoryInstance()
        .CreateByType(m_quantum_machine_type));
    m_machine->init();

    m_theta_index_vec.clear();
    m_hamiltonian = m_pauli.toHamiltonian();
    auto qubits_num = 0;
    for (int i = 0; i < m_ansatz.size(); i++)
    {
        if (m_ansatz[i].target > qubits_num)
        {
            qubits_num = m_ansatz[i].target;
        }

        if (m_ansatz[i].control > qubits_num)
        {
            qubits_num = m_ansatz[i].control;
        }

        if (m_ansatz[i].type == AnsatzGateType::AGT_RX ||
            m_ansatz[i].type == AnsatzGateType::AGT_RY ||
            m_ansatz[i].type == AnsatzGateType::AGT_RZ)
        {
            m_theta_index_vec.push_back(i);
        }
    }
    qubits_num++;

    m_best_ansatz = m_ansatz;
    m_qlist = m_machine->allocateQubits(qubits_num+1);
    m_expectation = getExpectation(m_ansatz);
}

double QITE::getExpectation(const std::vector<AnsatzGate>& ansatz)
{
    QCircuit circuit = constructCircuit(ansatz);
    double expectation = 0.0;
    for (size_t i = 0; i < m_hamiltonian.size(); i++)
    {
        expectation += getExpectationOneTerm(circuit, m_hamiltonian[i]);
    }

    return expectation;
}

double QITE::getExpectationOneTerm( QCircuit c, const QHamiltonianItem& component)
{
    if (component.first.empty())
    {
        return component.second;
    }

    QProg prog;
    prog << c;

    for (auto iter : component.first)
    {
        if (iter.second == 'X')
        {
            prog << H(m_qlist[iter.first]);
        }
        else if (iter.second == 'Y')
        {
            prog << RX(m_qlist[iter.first], PI / 2);
        }
    }

    m_machine->directlyRun(prog);
    double expectation = 0;

    auto temp = dynamic_cast<IdealMachineInterface*>(m_machine.get());
    if (nullptr == temp)
    {
        QCERR("m_machine is not ideal machine");
        throw std::runtime_error("m_machine is not ideal machine");
    }
    auto measure_qubit = m_qlist;
    measure_qubit.pop_back();
    auto result = temp->PMeasure(measure_qubit, -1);

    for (auto i = 0u; i < result.size(); i++)
    {
        if (ParityCheck(result[i].first, component.first))
        {
            expectation -= result[i].second;
        }
        else
        {
            expectation += result[i].second;
        }
    }

    return expectation * component.second;
}

bool QITE::ParityCheck(size_t state, const QTerm& paulis) const
{
    size_t check = 0;
    for (auto iter = paulis.begin(); iter != paulis.end(); iter++)
    {
        auto value = state >> iter->first;
        if ((value % 2) == 1)
        {
            check++;
        }
    }

    return 1 == check % 2;
}

void QITE::calcParaA()
{
    int theta_num = m_theta_index_vec.size();
    m_A = Eigen::MatrixXd(theta_num, theta_num);
    for (int i = 0; i < theta_num; i++)
    {
        for (int j = 0; j < theta_num; j++)
        {
            auto t_i = m_theta_index_vec[i];
            auto t_j = m_theta_index_vec[j];
            if (i > j)
            {
                m_A.row(i)[j] = m_A.row(j)[i];
                continue;
            }

            int k = getAnsatzDerivativeParaNum(t_i);
            int l = getAnsatzDerivativeParaNum(t_j);

            double sum = 0;
            for (int p = 0; p < k; p++)
            {
                for (int q = 0; q < l; q++)
                {
                    auto f_ik = getAnsatzDerivativePara(t_i, p);
                    auto f_jl = getAnsatzDerivativePara(t_j, q);
                    auto ff = complexDagger(f_ik) * f_jl;
                    auto die_len = std::abs(ff);

                    if ((i ==j) &&(p == q))
                    {
                        // Identity
                        sum += die_len;
                    }
                    else
                    {
                        auto phase = std::arg(ff);
                        auto value = calcSubCircuit(
                            t_i,
                            t_j,
                            phase,
                            getAnsatzDerivativeCircuit(t_i, p),
                            getAnsatzDerivativeCircuit(t_j, q));

                        sum += die_len * value;
                    }
                }
            }

            m_A.row(i)[j] = sum;
        }
    }
}

void QITE::calcParaC()
{
    int theta_num = m_theta_index_vec.size();
    m_C = Eigen::VectorXd(theta_num);
    for (int i = 0; i < theta_num; i++)
    {
        auto t_i = m_theta_index_vec[i];
        double sum = 0;
        int k = getAnsatzDerivativeParaNum(t_i);
        int l = getHamiltonianItemNum();
        for (int p = 0; p < k; p++)
        {
            for (int q = 0; q < l; q++)
            {
                auto f_ik = getAnsatzDerivativePara(t_i, p);
                auto f_l = getHamiltonianItemPara(q);
                auto ff = complexDagger(f_ik) * f_l;
                auto die_len = std::abs(ff);
                auto phase = std::arg(ff);

                auto value = calcSubCircuit(
                    t_i,
                    m_ansatz.size(),
                    phase,
                    getAnsatzDerivativeCircuit(t_i, p),
                    getHamiltonianItemCircuit(q));

                sum += die_len * value;
            }
        }
        m_C[i] = sum;
    }
    m_C *= -1;
}

QCircuit QITE::constructCircuit(const std::vector<AnsatzGate>& ansatz)
{
    QCircuit circuit;
    for (int i = 0; i < ansatz.size(); i++)
    {
        circuit << convertAnsatzToCircuit(ansatz[i]);
    }

    return circuit;
}

std::complex<double> QITE::complexDagger(std::complex<double>& value)
{
    return std::complex<double>(value.real(), -value.imag());
}

int QITE::getAnsatzDerivativeParaNum(int i)
{
    if (i < 0 ||
        i >= m_ansatz.size())
    {
        QCERR_AND_THROW_ERRSTR(std::runtime_error,
            "bad para of i in getAnsatzDerivativeParaNum");
    }

    auto& u = m_ansatz[i];

    return u.control == -1 ? 1 : 2;
}

int QITE::getHamiltonianItemNum()
{
    return m_hamiltonian.size();
}

std::complex<double> QITE::getAnsatzDerivativePara(int i, int cnt)
{
    if (i < 0 ||
        i >= m_ansatz.size())
    {
        QCERR_AND_THROW_ERRSTR(std::runtime_error,
            "bad para of i in getAnsatzDerivativePara");
    }

    auto& u = m_ansatz[i];

    if (u.control != -1)
    {
        if (cnt < 0 ||
            cnt > 1)
        {
            QCERR_AND_THROW_ERRSTR(std::runtime_error,
                "bad para of cnt in getAnsatzDerivativePara");
        }

        return cnt == 0 ? std::complex<double>(0, -0.25) :
            std::complex<double>(0, 0.25);
    }
    else
    {
        if (cnt != 0)
        {
            QCERR_AND_THROW_ERRSTR(std::runtime_error,
                "bad para of cnt in getAnsatzDerivativePara");
        }

        return std::complex<double>(0, -0.5);
    }
}

double QITE::getHamiltonianItemPara(int i)
{
    if (i < 0 ||
        i >= m_hamiltonian.size())
    {
        QCERR_AND_THROW_ERRSTR(std::runtime_error,
            "bad para of i in getHamiltonianItemPara");
    }

    return m_hamiltonian[i].second;
}

QCircuit QITE::getAnsatzDerivativeCircuit(int i, int cnt)
{
    if (i < 0 ||
        i >= m_ansatz.size())
    {
        QCERR_AND_THROW_ERRSTR(std::runtime_error,
            "bad para of i in getAnsatzDerivativePara");
    }

    QCircuit sub_cir;
    int anc = m_qlist.size() - 1;

    auto& u = m_ansatz[i];

    if (u.control != -1)
    {
        if (cnt < 0 ||
            cnt > 1)
        {
            QCERR_AND_THROW_ERRSTR(std::runtime_error,
                "bad para of cnt in getAnsatzDerivativePara");
        }

        sub_cir << (cnt == 0 ? I(m_qlist[u.control]) : Z(m_qlist[u.control]));
    }

    switch (u.type)
    {
    case AnsatzGateType::AGT_RX:
        sub_cir << X(m_qlist[u.target]);
        break;
    case AnsatzGateType::AGT_RY:
        sub_cir << Y(m_qlist[u.target]);
        break;
    case AnsatzGateType::AGT_RZ:
        sub_cir << Z(m_qlist[u.target]);
        break;
    }

    return sub_cir.control({ m_qlist[anc] });
}

QCircuit QITE::getHamiltonianItemCircuit(int cnt)
{
    if (cnt < 0 ||
        cnt >= m_hamiltonian.size())
    {
        QCERR_AND_THROW_ERRSTR(std::runtime_error,
            "bad para of cnt in getHamiltonianItemPara");
    }

    QCircuit cir;
    auto term = m_hamiltonian[cnt].first;

    for (auto& iter : term)
    {
        switch (iter.second)
        {
        case 'X':
            cir << X(m_qlist[iter.first]);
            break;
        case 'Y':
            cir << Y(m_qlist[iter.first]);
            break;
        case 'Z':
            cir << Z(m_qlist[iter.first]);
            break;
        default:
            cir << I(m_qlist[iter.first]);
            break;
        }
    }

    return cir.control({ m_qlist[m_qlist.size() - 1] });
}

double QITE::calcSubCircuit(
    int index1,
    int index2,
    double theta,
    QCircuit cir1,
    QCircuit cir2)
{
    auto anc_quibit = m_qlist[m_qlist.size() - 1];

    QProg prog;
    for (int i = 0; i < index1; i++)
    {
        prog << convertAnsatzToCircuit(m_ansatz[i]);
    }

    prog << H(anc_quibit)
        << U1(anc_quibit, theta)
        << X(anc_quibit)
        << cir1
        << X(anc_quibit);

    for (int i = index1; i < index2; i++)
    {
        prog << convertAnsatzToCircuit(m_ansatz[i]);
    }

    prog << cir2
        << H(anc_quibit);

    auto temp = dynamic_cast<IdealMachineInterface*>(m_machine.get());
    auto result = temp->probRunDict(prog, { anc_quibit }, -1);

    return 2 * result["0"] - 1;
}

QCircuit QITE::convertAnsatzToCircuit(const AnsatzGate& u)
{
    if (u.target < 0 || u.target >= m_qlist.size())
    {
        QCERR_AND_THROW_ERRSTR(std::runtime_error,
            "bad para of target in convertAnsatzToCircuit");
    }

    QCircuit sub_cir;
    switch (u.type)
    {
    case AnsatzGateType::AGT_NOT:
        sub_cir << X(m_qlist[u.target]);
        break;
    case AnsatzGateType::AGT_H:
        sub_cir << H(m_qlist[u.target]);
        break;
    case AnsatzGateType::AGT_RX:
        sub_cir << RX(m_qlist[u.target], u.theta);
        break;
    case AnsatzGateType::AGT_RY:
        sub_cir << RY(m_qlist[u.target], u.theta);
        break;
    case AnsatzGateType::AGT_RZ:
        sub_cir << RZ(m_qlist[u.target], u.theta);
        break;
    default:
        break;
    }

    if (u.control != -1)
    {
        sub_cir.setControl({ m_qlist[u.control] });
    }

    return sub_cir;
}

Eigen::MatrixXd QITE::pseudoinverse(Eigen::MatrixXd matrix)
{
    auto svd = matrix.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);
    const auto& singularValues = svd.singularValues();
    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>
        singularValuesInv(matrix.cols(), matrix.rows());
    singularValuesInv.setZero();
    double  pinvtoler = m_arbitary_cofficient;
    for (unsigned int i = 0; i < singularValues.size(); ++i) {
        if (singularValues(i) > pinvtoler)
            singularValuesInv(i, i) = 1.0 / singularValues(i);
        else
            singularValuesInv(i, i) = 0.0;
    }
    Eigen::MatrixXd pinvmat =
        svd.matrixV() * singularValuesInv * svd.matrixU().transpose();

    return pinvmat;
}

prob_tuple qite(
    const PauliOperator& h,
    const std::vector<AnsatzGate>& ansatz_gate,
    size_t iter_num,
    std::string log_file,
    QITE::UpdateMode mode,
    size_t up_throw_num,
    double delta_tau,
    double convergence_factor_Q,
    double arbitary_cofficient,
    QMachineType type)
{
    QITE alg;
    alg.setHamiltonian(h);
    alg.setAnsatzGate(ansatz_gate);
    alg.setIterNum(iter_num);
    alg.setLogFile(log_file);
    alg.setParaUpdateMode(mode);
    alg.setUpthrowNum(up_throw_num);
    alg.setDeltaTau(delta_tau);
    alg.setConvergenceFactorQ(convergence_factor_Q);
    alg.setArbitaryCofficient(arbitary_cofficient);
    alg.setQuantumMachineType(type);

    if (alg.exec() != 0)
    {
        return prob_tuple();
    }

    return alg.getResult();
}

QPANDA_END