// Copyright (c) 2021, The Monero Project
//
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without modification, are
// permitted provided that the following conditions are met:
//
// 1. Redistributions of source code must retain the above copyright notice, this list of
//    conditions and the following disclaimer.
//
// 2. 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.
//
// 3. Neither the name of the copyright holder 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 HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
// STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF
// THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#include "multisig_account.h"

#include "crypto/crypto.h"
#include "cryptonote_config.h"
#include "include_base_utils.h"
#include "multisig.h"
#include "multisig_kex_msg.h"
#include "ringct/rctOps.h"

#include <boost/math/special_functions/binomial.hpp>

#include <algorithm>
#include <cmath>
#include <cstdint>
#include <limits>
#include <memory>
#include <unordered_map>
#include <unordered_set>
#include <utility>
#include <vector>

#undef MONERO_DEFAULT_LOG_CATEGORY
#define MONERO_DEFAULT_LOG_CATEGORY "multisig"

namespace multisig
{
  //----------------------------------------------------------------------------------------------------------------------
  /**
  * INTERNAL
  *
  * brief: calculate_multisig_keypair_from_derivation - wrapper on calculate_multisig_keypair() for an input public key
  *    Converts an input public key into a crypto private key (type cast, does not change serialization),
  *    then passes it to get_multisig_blinded_secret_key().
  *
  *    Result:
  *      - privkey = H(derivation)
  *      - pubkey = privkey * G
  * param: derivation - a curve point
  * outparam: derived_pubkey_out - public key of the resulting privkey
  * return: multisig private key
  */
  //----------------------------------------------------------------------------------------------------------------------
  static crypto::secret_key calculate_multisig_keypair_from_derivation(const crypto::public_key_memsafe &derivation,
    crypto::public_key &derived_pubkey_out)
  {
    crypto::secret_key blinded_skey = get_multisig_blinded_secret_key(rct::rct2sk(rct::pk2rct(derivation)));
    CHECK_AND_ASSERT_THROW_MES(crypto::secret_key_to_public_key(blinded_skey, derived_pubkey_out), "Failed to derive public key");

    return blinded_skey;
  }
  //----------------------------------------------------------------------------------------------------------------------
  /**
  * INTERNAL
  *
  * brief: make_multisig_common_privkey - Create the 'common' multisig privkey, owned by all multisig participants.
  *    - common privkey = H(sorted base common privkeys)
  * param: participant_base_common_privkeys - Base common privkeys contributed by multisig participants.
  * outparam: common_privkey_out - result
  */
  //----------------------------------------------------------------------------------------------------------------------
  static void make_multisig_common_privkey(std::vector<crypto::secret_key> participant_base_common_privkeys,
    crypto::secret_key &common_privkey_out)
  {
    // sort the privkeys for consistency
    //TODO: need a constant-time operator< for sorting secret keys
    std::sort(participant_base_common_privkeys.begin(), participant_base_common_privkeys.end(),
        [](const crypto::secret_key &key1, const crypto::secret_key &key2) -> bool
        {
          return memcmp(&key1, &key2, sizeof(crypto::secret_key)) < 0;
        }
      );

    // privkey = H(sorted ancillary base privkeys)
    crypto::hash_to_scalar(participant_base_common_privkeys.data(),
      participant_base_common_privkeys.size()*sizeof(crypto::secret_key),
      common_privkey_out);

    CHECK_AND_ASSERT_THROW_MES(common_privkey_out != crypto::null_skey, "Unexpected null secret key (danger!).");
  }
  //----------------------------------------------------------------------------------------------------------------------
  /**
  * INTERNAL
  *
  * brief: compute_multisig_aggregation_coefficient - creates aggregation coefficient for a specific public key in a set
  *    of public keys
  *
  *    WARNING: The coefficient will only be deterministic if...
  *      1) input keys are pre-sorted
  *         - tested here
  *      2) input keys are in canonical form (compressed points in the prime-order subgroup of Ed25519)
  *         - untested here for performance
  * param: sorted_keys - set of component public keys that will be merged into a multisig public spend key
  * param: aggregation_key - one of the component public keys
  * return: aggregation coefficient
  */
  //----------------------------------------------------------------------------------------------------------------------
  static rct::key compute_multisig_aggregation_coefficient(const std::vector<crypto::public_key> &sorted_keys,
    const crypto::public_key &aggregation_key)
  {
    CHECK_AND_ASSERT_THROW_MES(std::is_sorted(sorted_keys.begin(), sorted_keys.end()),
      "Keys for aggregation coefficient aren't sorted.");

    // aggregation key must be in sorted_keys
    CHECK_AND_ASSERT_THROW_MES(std::find(sorted_keys.begin(), sorted_keys.end(), aggregation_key) != sorted_keys.end(),
      "Aggregation key expected to be in input keyset.");

    // aggregation coefficient salt
    rct::key salt = rct::zero();
    static_assert(sizeof(rct::key) >= sizeof(config::HASH_KEY_MULTISIG_KEY_AGGREGATION), "Hash domain separator is too big.");
    memcpy(salt.bytes, config::HASH_KEY_MULTISIG_KEY_AGGREGATION, sizeof(config::HASH_KEY_MULTISIG_KEY_AGGREGATION));

    // coeff = H(aggregation_key, sorted_keys, domain-sep)
    rct::keyV data;
    data.reserve(sorted_keys.size() + 2);
    data.push_back(rct::pk2rct(aggregation_key));
    for (const auto &key : sorted_keys)
      data.push_back(rct::pk2rct(key));
    data.push_back(salt);

    // note: coefficient is considered public knowledge, no need to memwipe data
    return rct::hash_to_scalar(data);
  }
  //----------------------------------------------------------------------------------------------------------------------
  /**
  * INTERNAL
  *
  * brief: generate_multisig_aggregate_key - generates a multisig public spend key via key aggregation
  *    Key aggregation via aggregation coefficients prevents key cancellation attacks.
  *    See: https://www.getmonero.org/resources/research-lab/pubs/MRL-0009.pdf
  * param: final_keys - address components (public keys) obtained from other participants (not shared with local)
  * param: privkeys_inout - private keys of address components known by local; each key will be multiplied by an aggregation coefficient (return by reference)
  * return: final multisig public spend key for the account
  */
  //----------------------------------------------------------------------------------------------------------------------
  static crypto::public_key generate_multisig_aggregate_key(std::vector<crypto::public_key> final_keys,
    std::vector<crypto::secret_key> &privkeys_inout)
  {
    // collect all public keys that will go into the spend key (these don't need to be memsafe)
    final_keys.reserve(final_keys.size() + privkeys_inout.size());

    // 1. convert local multisig private keys to pub keys
    // 2. insert to final keyset if not there yet
    // 3. save the corresponding index of input priv key set for later reference
    std::unordered_map<crypto::public_key, std::size_t> own_keys_mapping;

    for (std::size_t multisig_keys_index{0}; multisig_keys_index < privkeys_inout.size(); ++multisig_keys_index)
    {
      crypto::public_key pubkey;
      CHECK_AND_ASSERT_THROW_MES(crypto::secret_key_to_public_key(privkeys_inout[multisig_keys_index], pubkey), "Failed to derive public key");

      own_keys_mapping[pubkey] = multisig_keys_index;

      final_keys.push_back(pubkey);
    }

    // sort input final keys for computing aggregation coefficients (lowest to highest)
    // note: input should be sanitized (no duplicates)
    std::sort(final_keys.begin(), final_keys.end());
    CHECK_AND_ASSERT_THROW_MES(std::adjacent_find(final_keys.begin(), final_keys.end()) == final_keys.end(),
        "Unexpected duplicate found in input list.");

    // key aggregation
    rct::key aggregate_key = rct::identity();

    for (const crypto::public_key &key : final_keys)
    {
      // get aggregation coefficient
      rct::key coeff = compute_multisig_aggregation_coefficient(final_keys, key);

      // convert private key if possible
      // note: retain original priv key index in input list, in case order matters upstream
      auto found_key = own_keys_mapping.find(key);
      if (found_key != own_keys_mapping.end())
      {
        // k_agg = coeff*k_base
        sc_mul((unsigned char*)&(privkeys_inout[found_key->second]),
          coeff.bytes,
          (const unsigned char*)&(privkeys_inout[found_key->second]));

        CHECK_AND_ASSERT_THROW_MES(privkeys_inout[found_key->second] != crypto::null_skey,
          "Multisig privkey with aggregation coefficient unexpectedly null.");
      }

      // convert public key (pre-merge operation)
      // K_agg = coeff*K_base
      rct::key converted_pubkey = rct::scalarmultKey(rct::pk2rct(key), coeff);

      // build aggregate key (merge operation)
      rct::addKeys(aggregate_key, aggregate_key, converted_pubkey);
    }

    return rct::rct2pk(aggregate_key);
  }
  //----------------------------------------------------------------------------------------------------------------------
  /**
  * INTERNAL
  *
  * brief: multisig_kex_make_next_msg - Construct a kex msg for any round > 1 of multisig key construction.
  *    - Involves DH exchanges with pubkeys provided by other participants.
  *    - Conserves mapping [pubkey -> DH derivation] : [origin keys of participants that share this secret with you].
  * param: base_privkey - account's base private key, for performing DH exchanges and signing messages
  * param: round - the round of the message that should be produced
  * param: threshold - threshold for multisig (M in M-of-N)
  * param: num_signers - number of participants in multisig (N)
  * param: pubkey_origins_map - map between pubkeys to produce DH derivations with and identity keys of
  *    participants who will share each derivation with you
  * outparam: derivation_origins_map_out - map between DH derivations (shared secrets) and identity keys
  *        - If msg is not for the last round, then these derivations are also stored in the output message
  *          so they can be sent to other participants, who will make more DH derivations for the next kex round.
  *        - If msg is for the last round, then these derivations won't be sent to other participants.
  *          Instead, they are converted to share secrets (i.e. s = H(derivation)) and multiplied by G.
  *          The keys s*G are sent to other participants in the message, so they can be used to produce the final
  *          multisig key via generate_multisig_spend_public_key().
  *            - The values s are the local account's shares of the final multisig key's private key. The caller can
  *              compute those values with calculate_multisig_keypair_from_derivation() (or compute them directly).
  * return: multisig kex message for the specified round
  */
  //----------------------------------------------------------------------------------------------------------------------
  static multisig_kex_msg multisig_kex_make_next_msg(const crypto::secret_key &base_privkey,
    const std::uint32_t round,
    const std::uint32_t threshold,
    const std::uint32_t num_signers,
    const std::unordered_map<crypto::public_key_memsafe, std::unordered_set<crypto::public_key>> &pubkey_origins_map,
    std::unordered_map<crypto::public_key_memsafe, std::unordered_set<crypto::public_key>> &derivation_origins_map_out)
  {
    CHECK_AND_ASSERT_THROW_MES(num_signers > 1, "Must be at least one other multisig signer.");
    CHECK_AND_ASSERT_THROW_MES(num_signers <= config::MULTISIG_MAX_SIGNERS,
      "Too many multisig signers specified (limit = 16 to prevent dangerous combinatorial explosion during key exchange).");
    CHECK_AND_ASSERT_THROW_MES(num_signers >= threshold,
      "Multisig threshold may not be larger than number of signers.");
    CHECK_AND_ASSERT_THROW_MES(threshold > 0, "Multisig threshold must be > 0.");
    CHECK_AND_ASSERT_THROW_MES(round > 1, "Round for next msg must be > 1.");
    CHECK_AND_ASSERT_THROW_MES(round <= multisig_kex_rounds_required(num_signers, threshold),
      "Trying to make key exchange message for an invalid round.");

    // make shared secrets with input pubkeys
    std::vector<crypto::public_key> msg_pubkeys;
    msg_pubkeys.reserve(pubkey_origins_map.size());
    derivation_origins_map_out.clear();

    for (const auto &pubkey_and_origins : pubkey_origins_map)
    {
      // D = 8 * k_base * K_pubkey
      // note: must be mul8 (cofactor), otherwise it is possible to leak to a malicious participant if the local
      //       base_privkey is a multiple of 8 or not
      // note2: avoid making temporaries that won't be memwiped
      rct::key derivation_rct;
      auto a_wiper = epee::misc_utils::create_scope_leave_handler([&]{
        memwipe(&derivation_rct, sizeof(rct::key));
      });

      rct::scalarmultKey(derivation_rct, rct::pk2rct(pubkey_and_origins.first), rct::sk2rct(base_privkey));
      rct::scalarmultKey(derivation_rct, derivation_rct, rct::EIGHT);

      crypto::public_key_memsafe derivation{rct::rct2pk(derivation_rct)};

      // retain mapping between pubkey's origins and the DH derivation
      // note: if msg for last round, then caller must know how to handle these derivations properly
      derivation_origins_map_out[derivation] = pubkey_and_origins.second;

      // if the last round, convert derivations to public keys for the output message
      if (round == multisig_kex_rounds_required(num_signers, threshold))
      {
        // derived_pubkey = H(derivation)*G
        crypto::public_key derived_pubkey;
        calculate_multisig_keypair_from_derivation(derivation, derived_pubkey);
        msg_pubkeys.push_back(derived_pubkey);
      }
      // otherwise, put derivations in message directly, so other signers can in turn create derivations (shared secrets)
      //  with them for the next round
      else
        msg_pubkeys.push_back(derivation);
    }

    return multisig_kex_msg{round, base_privkey, std::move(msg_pubkeys)};
  }
  //----------------------------------------------------------------------------------------------------------------------
  /**
  * INTERNAL
  *
  * brief: multisig_kex_msgs_sanitize_pubkeys - Sanitize multisig kex messages.
  *    - Removes duplicates from msg pubkeys, ignores pubkeys equal to the local account's signing key,
  *      ignores messages signed by the local account, ignores keys found in input 'exclusion set',
  *      constructs map of pubkey:origins.
  *    - Requires that all input msgs have the same round number.
  *
  *    origins = all the signing pubkeys that recommended a given pubkey found in input msgs
  *
  *    - If the messages' round numbers are all '1', then only the message signing pubkey is considered
  *      'recommended'. Furthermore, the 'exclusion set' is ignored.
  * param: own_pubkey - local account's signing key (key used to sign multisig messages)
  * param: expanded_msgs - set of multisig kex messages to process
  * param: exclude_pubkeys - pubkeys to exclude from output set
  * outparam: sanitized_pubkeys_out - processed pubkeys obtained from msgs, mapped to their origins
  * return: round number shared by all input msgs
  */
  //----------------------------------------------------------------------------------------------------------------------
  static std::uint32_t multisig_kex_msgs_sanitize_pubkeys(const crypto::public_key &own_pubkey,
    const std::vector<multisig_kex_msg> &expanded_msgs,
    const std::vector<crypto::public_key> &exclude_pubkeys,
    std::unordered_map<crypto::public_key_memsafe, std::unordered_set<crypto::public_key>> &sanitized_pubkeys_out)
  {
    CHECK_AND_ASSERT_THROW_MES(expanded_msgs.size() > 0, "At least one input message expected.");

    std::uint32_t round = expanded_msgs[0].get_round();
    sanitized_pubkeys_out.clear();

    // get all pubkeys from input messages, add them to pubkey:origins map
    // - origins = all the signing pubkeys that recommended a given msg pubkey
    for (const auto &expanded_msg : expanded_msgs)
    {
      CHECK_AND_ASSERT_THROW_MES(expanded_msg.get_round() == round, "All messages must have the same kex round number.");

      // ignore messages from self
      if (expanded_msg.get_signing_pubkey() == own_pubkey)
        continue;

      // in round 1, only the signing pubkey is treated as a msg pubkey
      if (round == 1)
      {
        // note: ignores duplicates
        sanitized_pubkeys_out[expanded_msg.get_signing_pubkey()].insert(expanded_msg.get_signing_pubkey());
      }
      // in other rounds, only the msg pubkeys are treated as msg pubkeys
      else
      {
        // copy all pubkeys from message into list
        for (const auto &pubkey : expanded_msg.get_msg_pubkeys())
        {
          // ignore own pubkey
          if (pubkey == own_pubkey)
            continue;

          // ignore pubkeys in 'ignore' set
          if (std::find(exclude_pubkeys.begin(), exclude_pubkeys.end(), pubkey) != exclude_pubkeys.end())
            continue;

          // note: ignores duplicates
          sanitized_pubkeys_out[pubkey].insert(expanded_msg.get_signing_pubkey());
        }
      }
    }

    return round;
  }
  //----------------------------------------------------------------------------------------------------------------------
  /**
  * INTERNAL
  *
  * brief: evaluate_multisig_kex_round_msgs - Evaluate pubkeys from a kex round in order to prepare for the next round.
  *    - Sanitizes input msgs.
  *    - Require uniqueness in: 'signers', 'exclude_pubkeys'.
  *    - Requires each input pubkey be recommended by 'num_recommendations = expected_round' msg signers.
  *      - For a final multisig key to be truly 'M-of-N', each of the the private key's components must be
  *        shared by (N - M + 1) signers.
  *    - Requires that msgs are signed by only keys in 'signers'.
  *    - Requires that each key in 'signers' recommends [num_signers - 2 CHOOSE (expected_round - 1)] pubkeys.
  *      - These should be derivations each signer recommends for round 'expected_round', excluding derivations shared
  *        with the local account.
  *    - Requires that 'exclude_pubkeys' has [num_signers - 1 CHOOSE (expected_round - 1)] pubkeys.
  *      - These should be derivations the local account has corresponding to round 'expected_round'.
  * param: base_privkey - multisig account's base private key
  * param: expected_round - expected kex round of input messages
  * param: threshold - threshold for multisig (M in M-of-N)
  * param: signers - expected participants in multisig kex
  * param: expanded_msgs - set of multisig kex messages to process
  * param: exclude_pubkeys - derivations held by the local account corresponding to round 'expected_round'
  * return: fully sanitized and validated pubkey:origins map for building the account's next kex round message
  */
  //----------------------------------------------------------------------------------------------------------------------
  static std::unordered_map<crypto::public_key_memsafe, std::unordered_set<crypto::public_key>> evaluate_multisig_kex_round_msgs(
    const crypto::public_key &base_pubkey,
    const std::uint32_t expected_round,
    const std::uint32_t threshold,
    const std::vector<crypto::public_key> &signers,
    const std::vector<multisig_kex_msg> &expanded_msgs,
    const std::vector<crypto::public_key> &exclude_pubkeys)
  {
    CHECK_AND_ASSERT_THROW_MES(signers.size() > 1, "Must be at least one other multisig signer.");
    CHECK_AND_ASSERT_THROW_MES(signers.size() <= config::MULTISIG_MAX_SIGNERS,
      "Too many multisig signers specified (limit = 16 to prevent dangerous combinatorial explosion during key exchange).");
    CHECK_AND_ASSERT_THROW_MES(signers.size() >= threshold, "Multisig threshold may not be larger than number of signers.");
    CHECK_AND_ASSERT_THROW_MES(threshold > 0, "Multisig threshold must be > 0.");
    CHECK_AND_ASSERT_THROW_MES(expected_round > 0, "Expected round must be > 0.");
    CHECK_AND_ASSERT_THROW_MES(expected_round <= multisig_kex_rounds_required(signers.size(), threshold),
      "Expecting key exchange messages for an invalid round.");

    std::unordered_map<crypto::public_key_memsafe, std::unordered_set<crypto::public_key>> pubkey_origins_map;

    // leave early in the last round of 1-of-N, where all signers share a key so the local signer doesn't care about
    // recommendations from other signers
    if (threshold == 1 && expected_round == multisig_kex_rounds_required(signers.size(), threshold))
      return pubkey_origins_map;

    // exclude_pubkeys should all be unique
    for (auto it = exclude_pubkeys.begin(); it != exclude_pubkeys.end(); ++it)
    {
      CHECK_AND_ASSERT_THROW_MES(std::find(exclude_pubkeys.begin(), it, *it) == it,
        "Found duplicate pubkeys for exclusion unexpectedly.");
    }

    // sanitize input messages
    std::uint32_t round = multisig_kex_msgs_sanitize_pubkeys(base_pubkey, expanded_msgs, exclude_pubkeys, pubkey_origins_map);
    CHECK_AND_ASSERT_THROW_MES(round == expected_round,
      "Kex messages were for round [" << round << "], but expected round is [" << expected_round << "]");

    // evaluate pubkeys collected
    std::unordered_map<crypto::public_key, std::unordered_set<crypto::public_key>> origin_pubkeys_map;

    // 1. each pubkey should be recommended by a precise number of signers
    for (const auto &pubkey_and_origins : pubkey_origins_map)
    {
      // expected amount = round_num
      // With each successive round, pubkeys are shared by incrementally larger groups,
      //  starting at 1 in round 1 (i.e. the local multisig key to start kex with).
      CHECK_AND_ASSERT_THROW_MES(pubkey_and_origins.second.size() == round,
        "A pubkey recommended by multisig kex messages had an unexpected number of recommendations.");

      // map (sanitized) pubkeys back to origins
      for (const auto &origin : pubkey_and_origins.second)
        origin_pubkeys_map[origin].insert(pubkey_and_origins.first);
    }

    // 2. the number of unique signers recommending pubkeys should equal the number of signers passed in (minus the local signer)
    CHECK_AND_ASSERT_THROW_MES(origin_pubkeys_map.size() == signers.size() - 1,
      "Number of unique other signers does not equal number of other signers that recommended pubkeys.");

    // 3. each origin should recommend a precise number of pubkeys

    // TODO: move to a 'math' library, with unit tests
    auto n_choose_k_f =
      [](const std::uint32_t n, const std::uint32_t k) -> std::uint32_t
      {
        static_assert(std::numeric_limits<std::int32_t>::digits <= std::numeric_limits<double>::digits,
          "n_choose_k requires no rounding issues when converting between int32 <-> double.");

        if (n < k)
          return 0;

        double fp_result = boost::math::binomial_coefficient<double>(n, k);

        if (fp_result < 0)
          return 0;

        if (fp_result > std::numeric_limits<std::int32_t>::max())  // note: std::round() returns std::int32_t
          return 0;

        return static_cast<std::uint32_t>(std::round(fp_result));
      };

    // other signers: (N - 2) choose (msg_round_num - 1)
      // - Each signer recommends keys they share with other signers.
      // - In each round, a signer shares a key with 'round num - 1' other signers.
      // - Since 'origins pubkey map' excludes keys shared with the local account,
      //   only keys shared with participants 'other than local and self' will be in the map (e.g. N - 2 signers).
      // - So other signers will recommend (N - 2) choose (msg_round_num - 1) pubkeys (after removing keys shared with local).
      // - Each origin should have a shared key with each group of size 'round - 1'.
      // Note: Keys shared with local are ignored to facilitate kex round boosting, where one or more signers may
      //       have boosted the local signer (implying they didn't have access to the local signer's previous round msg).
    std::uint32_t expected_recommendations_others = n_choose_k_f(signers.size() - 2, round - 1);

    // local: (N - 1) choose (msg_round_num - 1)
    std::uint32_t expected_recommendations_self = n_choose_k_f(signers.size() - 1, round - 1);

    // note: expected_recommendations_others would be 0 in the last round of 1-of-N, but we return early for that case
    CHECK_AND_ASSERT_THROW_MES(expected_recommendations_self > 0 && expected_recommendations_others > 0,
      "Bad num signers or round num (possibly numerical limits exceeded).");

    // check that local account recommends expected number of keys
    CHECK_AND_ASSERT_THROW_MES(exclude_pubkeys.size() == expected_recommendations_self,
      "Local account did not recommend expected number of multisig keys.");

    // check that other signers recommend expected number of keys
    for (const auto &origin_and_pubkeys : origin_pubkeys_map)
    {
      CHECK_AND_ASSERT_THROW_MES(origin_and_pubkeys.second.size() == expected_recommendations_others,
        "A pubkey recommended by multisig kex messages had an unexpected number of recommendations.");

      // 2 (continued). only expected signers should be recommending keys
      CHECK_AND_ASSERT_THROW_MES(std::find(signers.begin(), signers.end(), origin_and_pubkeys.first) != signers.end(),
        "Multisig kex message with unexpected signer encountered.");
    }

    // note: above tests implicitly detect if the total number of recommended keys is correct or not
    return pubkey_origins_map;
  }
  //----------------------------------------------------------------------------------------------------------------------
  /**
  * INTERNAL
  *
  * brief: multisig_kex_process_round - Process kex messages for the active kex round.
  *    - A wrapper around evaluate_multisig_kex_round_msgs() -> multisig_kex_make_next_msg().
  *      - In other words, evaluate the input messages and try to make a message for the next round.
  *    - Note: Must be called on the final round's msgs to evaluate the final key components
  *            recommended by other participants.
  * param: base_privkey - multisig account's base private key
  * param: current_round - round of kex the input messages should be designed for
  * param: threshold - threshold for multisig (M in M-of-N)
  * param: signers - expected participants in multisig kex
  * param: expanded_msgs - set of multisig kex messages to process
  * param: exclude_pubkeys - keys held by the local account corresponding to round 'current_round'
  *    - If 'current_round' is the final round, these are the local account's shares of the final aggregate key.
  * outparam: keys_to_origins_map_out - map between round keys and identity keys
  *    - If in the final round, these are key shares recommended by other signers for the final aggregate key.
  *    - Otherwise, these are the local account's DH derivations for the next round.
  *      - See multisig_kex_make_next_msg() for an explanation.
  * return: multisig kex message for next round, or empty message if 'current_round' is the final round
  */
  //----------------------------------------------------------------------------------------------------------------------
  static multisig_kex_msg multisig_kex_process_round(const crypto::secret_key &base_privkey,
    const crypto::public_key &base_pubkey,
    const std::uint32_t current_round,
    const std::uint32_t threshold,
    const std::vector<crypto::public_key> &signers,
    const std::vector<multisig_kex_msg> &expanded_msgs,
    const std::vector<crypto::public_key> &exclude_pubkeys,
    std::unordered_map<crypto::public_key_memsafe, std::unordered_set<crypto::public_key>> &keys_to_origins_map_out)
  {
    // evaluate messages
    std::unordered_map<crypto::public_key_memsafe, std::unordered_set<crypto::public_key>> evaluated_pubkeys =
      evaluate_multisig_kex_round_msgs(base_pubkey, current_round, threshold, signers, expanded_msgs, exclude_pubkeys);

    // produce message for next round (if there is one)
    if (current_round < multisig_kex_rounds_required(signers.size(), threshold))
    {
      return multisig_kex_make_next_msg(base_privkey,
        current_round + 1,
        threshold,
        signers.size(),
        evaluated_pubkeys,
        keys_to_origins_map_out);
    }
    else
    {
      // no more rounds, so collect the key shares recommended by other signers for the final aggregate key
      keys_to_origins_map_out.clear();
      keys_to_origins_map_out = std::move(evaluated_pubkeys);

      return multisig_kex_msg{};
    }
  }
  //----------------------------------------------------------------------------------------------------------------------
  // multisig_account: INTERNAL
  //----------------------------------------------------------------------------------------------------------------------
  void multisig_account::initialize_kex_update(const std::vector<multisig_kex_msg> &expanded_msgs,
    const std::uint32_t rounds_required,
    std::vector<crypto::public_key> &exclude_pubkeys_out)
  {
    if (m_kex_rounds_complete == 0)
    {
      // the first round of kex msgs will contain each participant's base pubkeys and ancillary privkeys

      // collect participants' base common privkey shares
      // note: duplicate privkeys are acceptable, and duplicates due to duplicate signers
      //       will be blocked by duplicate-signer errors after this function is called
      std::vector<crypto::secret_key> participant_base_common_privkeys;
      participant_base_common_privkeys.reserve(expanded_msgs.size() + 1);

      // add local ancillary base privkey
      participant_base_common_privkeys.emplace_back(m_base_common_privkey);

      // add other signers' base common privkeys
      for (const auto &expanded_msg : expanded_msgs)
      {
        if (expanded_msg.get_signing_pubkey() != m_base_pubkey)
        {
          participant_base_common_privkeys.emplace_back(expanded_msg.get_msg_privkey());
        }
      }

      // make common privkey
      make_multisig_common_privkey(std::move(participant_base_common_privkeys), m_common_privkey);

      // set common pubkey
      CHECK_AND_ASSERT_THROW_MES(crypto::secret_key_to_public_key(m_common_privkey, m_common_pubkey),
        "Failed to derive public key");

      // if N-of-N, then the base privkey will be used directly to make the account's share of the final key
      if (rounds_required == 1)
      {
        m_multisig_privkeys.clear();
        m_multisig_privkeys.emplace_back(m_base_privkey);
      }

      // exclude all keys the local account recommends
      // - in the first round, only the local pubkey is recommended by the local signer
      exclude_pubkeys_out.emplace_back(m_base_pubkey);
    }
    else
    {
      // in other rounds, kex msgs will contain participants' shared keys

      // ignore shared keys the account helped create for this round
      for (const auto &shared_key_with_origins : m_kex_keys_to_origins_map)
      {
        exclude_pubkeys_out.emplace_back(shared_key_with_origins.first);
      }
    }
  }
  //----------------------------------------------------------------------------------------------------------------------
  // multisig_account: INTERNAL
  //----------------------------------------------------------------------------------------------------------------------
  void multisig_account::finalize_kex_update(const std::uint32_t rounds_required,
    std::unordered_map<crypto::public_key_memsafe, std::unordered_set<crypto::public_key>> result_keys_to_origins_map)
  {
    // prepare for next round (or complete the multisig account fully)
    if (rounds_required == m_kex_rounds_complete + 1)
    {
      // finished (have set of msgs to complete address)

      // when 'completing the final round', result keys are other signers' shares of the final key
      std::vector<crypto::public_key> result_keys;
      result_keys.reserve(result_keys_to_origins_map.size());

      for (const auto &result_key_and_origins : result_keys_to_origins_map)
      {
        result_keys.emplace_back(result_key_and_origins.first);
      }

      // compute final aggregate key, update local multisig privkeys with aggregation coefficients applied
      m_multisig_pubkey = generate_multisig_aggregate_key(std::move(result_keys), m_multisig_privkeys);

      // no longer need the account's pubkeys saved for this round (they were only used to build exclude_pubkeys)
      // TODO: record [pre-aggregation pubkeys : origins] map for aggregation-style signing
      m_kex_keys_to_origins_map.clear();
    }
    else if (rounds_required == m_kex_rounds_complete + 2)
    {
      // one more round (must send/receive one more set of kex msgs)
      // - at this point, have local signer's pre-aggregation private key shares of the final address

      // result keys are the local signer's DH derivations for the next round

      // derivations are shared secrets between each group of N - M + 1 signers of which the local account is a member
      // - convert them to private keys: multisig_key = H(derivation)
      // - note: shared key = multisig_key[i]*G is recorded in the kex msg for sending to other participants
      //   instead of the original 'derivation' value (which MUST be kept secret!)
      m_multisig_privkeys.clear();
      m_multisig_privkeys.reserve(result_keys_to_origins_map.size());

      m_kex_keys_to_origins_map.clear();

      for (const auto &derivation_and_origins : result_keys_to_origins_map)
      {
        // multisig_privkey = H(derivation)
        // derived pubkey = multisig_key * G
        crypto::public_key_memsafe derived_pubkey;
        m_multisig_privkeys.push_back(
          calculate_multisig_keypair_from_derivation(derivation_and_origins.first, derived_pubkey));

        // save the account's kex key mappings for this round [derived pubkey : other signers who will have the same key]
        m_kex_keys_to_origins_map[derived_pubkey] = std::move(derivation_and_origins.second);
      }
    }
    else
    {
      // next round is an 'intermediate' key exchange round, so there is nothing special to do here

      // save the account's kex keys for this round [DH derivation : other signers who will have the same derivation]
      m_kex_keys_to_origins_map = std::move(result_keys_to_origins_map);
    }

    // a full set of msgs has been collected and processed, so the 'round is complete'
    ++m_kex_rounds_complete;
  }
  //----------------------------------------------------------------------------------------------------------------------
  // multisig_account: INTERNAL
  //----------------------------------------------------------------------------------------------------------------------
  void multisig_account::kex_update_impl(const std::vector<multisig_kex_msg> &expanded_msgs)
  {
    CHECK_AND_ASSERT_THROW_MES(expanded_msgs.size() > 0, "No key exchange messages passed in.");

    const std::uint32_t rounds_required = multisig_kex_rounds_required(m_signers.size(), m_threshold);
    CHECK_AND_ASSERT_THROW_MES(rounds_required > 0, "Multisig kex rounds required unexpectedly 0.");

    // initialize account update
    std::vector<crypto::public_key> exclude_pubkeys;
    initialize_kex_update(expanded_msgs, rounds_required, exclude_pubkeys);

    // evaluate messages and get this account's kex msg for the next round
    std::unordered_map<crypto::public_key_memsafe, std::unordered_set<crypto::public_key>> result_keys_to_origins_map;

    m_next_round_kex_message = multisig_kex_process_round(
      m_base_privkey,
      m_base_pubkey,
      m_kex_rounds_complete + 1,
      m_threshold,
      m_signers,
      expanded_msgs,
      exclude_pubkeys,
      result_keys_to_origins_map).get_msg();

    // finish account update
    finalize_kex_update(rounds_required, std::move(result_keys_to_origins_map));
  }
  //----------------------------------------------------------------------------------------------------------------------
} //namespace multisig


#include "nmos/capabilities.h"

#include <boost/range/adaptor/transformed.hpp>
#include "nmos/json_fields.h"

namespace nmos
{
    web::json::value make_caps_string_constraint(const std::vector<utility::string_t>& enum_values)
    {
        using web::json::value_of;
        using web::json::value_from_elements;
        return value_of({
            { !enum_values.empty() ? nmos::fields::constraint_enum.key : U(""), value_from_elements(enum_values) }
        });
    }

    web::json::value make_caps_integer_constraint(const std::vector<int64_t>& enum_values, int64_t minimum, int64_t maximum)
    {
        using web::json::value_of;
        using web::json::value_from_elements;
        return value_of({
            { !enum_values.empty() ? nmos::fields::constraint_enum.key : U(""), value_from_elements(enum_values) },
            { minimum != no_minimum<int64_t>() ? nmos::fields::constraint_minimum.key : U(""), minimum },
            { maximum != no_maximum<int64_t>() ? nmos::fields::constraint_maximum.key : U(""), maximum }
        });
    }

    web::json::value make_caps_number_constraint(const std::vector<double>& enum_values, double minimum, double maximum)
    {
        using web::json::value_of;
        using web::json::value_from_elements;
        return value_of({
            { !enum_values.empty() ? nmos::fields::constraint_enum.key : U(""), value_from_elements(enum_values) },
            { minimum != no_minimum<double>() ? nmos::fields::constraint_minimum.key : U(""), minimum },
            { maximum != no_maximum<double>() ? nmos::fields::constraint_maximum.key : U(""), maximum }
        });
    }

    web::json::value make_caps_boolean_constraint(const std::vector<bool>& enum_values)
    {
        using web::json::value_of;
        using web::json::value_from_elements;
        return value_of({
            { !enum_values.empty() ? nmos::fields::constraint_enum.key : U(""), value_from_elements(enum_values) }
        });
    }

    web::json::value make_caps_rational_constraint(const std::vector<nmos::rational>& enum_values, const nmos::rational& minimum, const nmos::rational& maximum)
    {
        using web::json::value_of;
        using web::json::value_from_elements;
        return value_of({
            { !enum_values.empty() ? nmos::fields::constraint_enum.key : U(""), value_from_elements(enum_values | boost::adaptors::transformed([](const nmos::rational& r) { return make_rational(r); })) },
            { minimum != no_minimum<nmos::rational>() ? nmos::fields::constraint_minimum.key : U(""), make_rational(minimum) },
            { maximum != no_maximum<nmos::rational>() ? nmos::fields::constraint_maximum.key : U(""), make_rational(maximum) }
        });
    }

    namespace details
    {
        // cf. nmos::details::make_constraints_schema in nmos/connection_api.cpp
        template <typename T, typename Parse>
        bool match_constraint(const T& value, const web::json::value& constraint, Parse parse)
        {
            if (constraint.has_field(nmos::fields::constraint_enum))
            {
                const auto& enum_values = nmos::fields::constraint_enum(constraint).as_array();
                if (enum_values.end() == std::find_if(enum_values.begin(), enum_values.end(), [&parse, &value](const web::json::value& enum_value)
                {
                    return parse(enum_value) == value;
                }))
                {
                    return false;
                }
            }
            if (constraint.has_field(nmos::fields::constraint_minimum))
            {
                const auto& minimum = nmos::fields::constraint_minimum(constraint);
                if (parse(minimum) > value)
                {
                    return false;
                }
            }
            if (constraint.has_field(nmos::fields::constraint_maximum))
            {
                const auto& maximum = nmos::fields::constraint_maximum(constraint);
                if (parse(maximum) < value)
                {
                    return false;
                }
            }
            return true;
        }
    }

    bool match_string_constraint(const utility::string_t& value, const web::json::value& constraint)
    {
        return details::match_constraint(value, constraint, [](const web::json::value& enum_value)
        {
            return enum_value.as_string();
        });
    }

    bool match_integer_constraint(int64_t value, const web::json::value& constraint)
    {
        return details::match_constraint(value, constraint, [&value](const web::json::value& enum_value)
        {
            return enum_value.as_integer();
        });
    }

    bool match_number_constraint(double value, const web::json::value& constraint)
    {
        return details::match_constraint(value, constraint, [&value](const web::json::value& enum_value)
        {
            return enum_value.as_double();
        });
    }

    bool match_boolean_constraint(bool value, const web::json::value& constraint)
    {
        return details::match_constraint(value, constraint, [&value](const web::json::value& enum_value)
        {
            return enum_value.as_bool();
        });
    }

    bool match_rational_constraint(const nmos::rational& value, const web::json::value& constraint)
    {
        return details::match_constraint(value, constraint, [&value](const web::json::value& enum_value)
        {
            return nmos::parse_rational(enum_value);
        });
    }
}


//
// ini_settings.cpp - ini settings implementation
//
// leccore library, part of the liblec library
// Copyright (c) 2019 Alec Musasa (alecmus at live dot com)
//
// Released under the MIT license. For full details see the
// file LICENSE.txt
//

#include "../settings.h"
#include "../leccore_common.h"
#include "../app_version_info.h"
#include "../encode.h"
#include "../hash.h"
#include "../encrypt.h"
#include "../encode.h"

#include <filesystem>

#include <Windows.h>
#include <ShlObj.h>	// for SHGetFolderPathA
#include <comdef.h>	// for _com_error

#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/ini_parser.hpp>

using namespace liblec::leccore;

class ini_settings::impl {
public:
	const std::string _default_file_name = "config.ini";
	const std::string _file_name;
	const std::string _key;
	const std::string _iv;
	const bool _encrypted;
	const int _salt_length = 4;
	std::string _ini_path;
	bool _ini_path_set;

	impl(const std::string& file_name,
		const std::string& key,
		const std::string& iv) :
		_file_name(file_name.empty() ? _default_file_name : file_name),
		_key(key),
		_iv(iv),
		_encrypted(!_key.empty() && !_iv.empty()),
		_ini_path_set(false) {}
	~impl() {}

	bool encrypt_string(const std::string& plain,
		std::string& encrypted,
		std::string& error) {
		error.clear();
		encrypted.clear();
		// make random salt (makes the encoded and encrypted results different for the same input)
		const std::string salt = hash_string::random_string(_salt_length);

		// encode the salt and plain text to base64
		std::string encoded = base64::encode(salt + plain);

		// encrypt the encoded text
		aes enc(_key, _iv);
		return enc.encrypt(encoded, encrypted, error);
	}

	bool decrypt_string(const std::string& encrypted,
		std::string& decrypted,
		std::string& error) {
		error.clear();
		decrypted.clear();
		// decrypt the text
		std::string encoded;
		aes enc(_key, _iv);
		if (!enc.decrypt(encrypted, encoded, error))
			return false;

		// decode the decrypted text
		base64 base64_encoder;
		decrypted = base64_encoder.decode(encoded);

		// step over the salt
		decrypted = decrypted.substr(_salt_length);
		return true;
	}

	static bool erase_path(boost::property_tree::ptree& pt,
		const boost::property_tree::ptree::key_type& key_path,
		size_t& erased_subkeys,
		std::string& error) {
		error.clear();
		try {
			boost::property_tree::ptree* p_sub_tree = &pt;
			boost::property_tree::ptree::path_type path(key_path);
			boost::property_tree::ptree::key_type parent_path;
			boost::property_tree::ptree::key_type sub_key;

			while (!path.single()) {
				sub_key = path.reduce();
				parent_path = parent_path.empty() ? sub_key :
					parent_path + path.separator() + sub_key;
				p_sub_tree = &(p_sub_tree->get_child(sub_key));
			}

			sub_key = path.reduce();
			const auto _count = p_sub_tree->erase(sub_key);
			erased_subkeys += _count;
			if (_count == 0)
				return true;

			if (p_sub_tree->empty() && !parent_path.empty())
				return erase_path(pt, parent_path, erased_subkeys, error);

			return true;
		}
		catch (const std::exception& e) {
			error = e.what();
			return false;
		}
	}
};

ini_settings::ini_settings(const std::string& file_name) : ini_settings(file_name, "", "") {}
ini_settings::ini_settings(const std::string& file_name, const std::string& key,
	const std::string& iv) : _d(*new impl(file_name, key, iv)) {}
ini_settings::~ini_settings() { delete& _d; }

bool ini_settings::get_ini_path(std::string& ini_path, std::string& error) {
	error.clear();
	ini_path.clear();
	if (!_d._ini_path_set && _d._ini_path.empty()) {
		app_version_info ver_info;

		std::string company_name, app_name;
		if (!ver_info.get_company_name(company_name, error))
			return false;

		if (!ver_info.get_app_name(app_name, error))
			return false;

		auto get_app_data = [](std::string& app_data_folder, std::string& error)->bool {
			app_data_folder.clear();
			CHAR szPath[MAX_PATH];
			HRESULT result = SHGetFolderPathA(NULL, CSIDL_LOCAL_APPDATA, NULL, 0, szPath);

			if (FAILED(result)) {
				error = convert_string(_com_error(result).ErrorMessage());
				return false;
			}

			app_data_folder = szPath;
			return true;
		};

		// get appdata folder
		std::string app_data_folder;
		if (!get_app_data(app_data_folder, error))
			return false;

		_d._ini_path = app_data_folder + "\\" +
			company_name + "\\" +
			app_name;
	}
	ini_path = _d._ini_path;
	return true;
}

void ini_settings::set_ini_path(const std::string& ini_path) {
	_d._ini_path = ini_path;
	_d._ini_path_set = true;
}

bool ini_settings::write_value(const std::string& branch,
	const std::string& value_name, const std::string& value, std::string& error) {
	error.clear();
	std::string ini_path;
	if (!get_ini_path(ini_path, error))
		return false;

	const std::string full_ini_file_path = ini_path.empty() ?
		_d._file_name : ini_path + "\\" + _d._file_name;

	std::string _path = value_name;

	if (!branch.empty())
		_path = branch + "." + _path;

	try {
		if (!ini_path.empty()) {
			std::filesystem::path path(ini_path);
			std::filesystem::create_directories(path);
		}

		boost::property_tree::ptree pt;

		std::filesystem::path file(full_ini_file_path);
		if (std::filesystem::is_regular_file(file))
			boost::property_tree::ini_parser::read_ini(full_ini_file_path, pt);

		if (_d._encrypted) {
			std::string data_encrypted;
			if (!_d.encrypt_string(value, data_encrypted, error))
				return false;

			std::string encoded = base32::encode(data_encrypted);
			pt.put(_path, encoded);
			boost::property_tree::write_ini(full_ini_file_path, pt);
			return true;
		}
		else {
			pt.put(_path, value);
			boost::property_tree::write_ini(full_ini_file_path, pt);
			return true;
		}
	}
	catch (const std::exception& e) {
		error = e.what();
		return false;
	}
}

bool ini_settings::read_value(const std::string& branch,
	const std::string& value_name, std::string& value, std::string& error) {
	error.clear();
	value.clear();
	std::string ini_path;
	if (!get_ini_path(ini_path, error))
		return false;

	const std::string full_ini_file_path = ini_path.empty() ?
		_d._file_name : ini_path + "\\" + _d._file_name;

	std::string _path = value_name;

	if (!branch.empty())
		_path = branch + "." + _path;

	try {
		std::filesystem::path path(full_ini_file_path);
		if (!std::filesystem::exists(path))
			return true;

		boost::property_tree::ptree pt;
		boost::property_tree::ini_parser::read_ini(full_ini_file_path, pt);

		if (_d._encrypted) {
			std::string encoded;
			try { encoded = pt.get<std::string>(_path); }
			catch (const std::exception&) {
				return true;
			}

			std::string data_encrypted = base32::decode(encoded);
			return _d.decrypt_string(data_encrypted, value, error);
		}
		else {
			try {
				value = pt.get<std::string>(_path);
				return true;
			}
			catch (const std::exception&) {
				return true;
			}
		}
	}
	catch (const std::exception& e) {
		error = e.what();
		return false;
	}
}

bool liblec::leccore::ini_settings::delete_value(const std::string& branch,
	const std::string& value_name, std::string& error) {
	error.clear();
	if (value_name.empty()) {
		error = "Value name not specified";
		return false;
	}

	std::string ini_path;
	if (!get_ini_path(ini_path, error))
		return false;

	const std::string full_ini_file_path = ini_path.empty() ?
		_d._file_name : ini_path + "\\" + _d._file_name;

	std::string _path = value_name;

	if (!branch.empty())
		_path = branch + "." + _path;

	try {
		boost::property_tree::ptree pt;
		boost::property_tree::ini_parser::read_ini(full_ini_file_path, pt);
		size_t erased_subkeys = 0;
		if (!impl::erase_path(pt, _path, erased_subkeys, error))
			return false;

		if (erased_subkeys > 0)
			boost::property_tree::write_ini(full_ini_file_path, pt);

		return true;
	}
	catch (const std::exception& e) {
		error = e.what();
		return false;
	}
}

bool ini_settings::delete_recursive(const std::string& branch,
	std::string& error) {
	error.clear();
	std::string ini_path;
	if (!get_ini_path(ini_path, error))
		return false;

	const std::string full_ini_file_path = ini_path.empty() ?
		_d._file_name : ini_path + "\\" + _d._file_name;

	std::string _path = branch;

	try {
		boost::property_tree::ptree pt;

		size_t erased_subkeys = 0;
		if (!_path.empty()) {
			boost::property_tree::ini_parser::read_ini(full_ini_file_path, pt);
			if (!impl::erase_path(pt, _path, erased_subkeys, error))
				return false;
		}

		if (erased_subkeys > 0)
			boost::property_tree::write_ini(full_ini_file_path, pt);

		return true;
	}
	catch (const std::exception& e) {
		error = e.what();
		return false;
	}
}


//
// Created by Hamza El-Kebir on 6/17/21.
//

#ifndef LODESTAR_FASTFOURIERTRANSFORM_HPP
#define LODESTAR_FASTFOURIERTRANSFORM_HPP

#include <Eigen/Dense>
#include <unsupported/Eigen/FFT>

namespace ls {
    namespace data {
        class FastFourierTransform {

        };
    }
}

#endif //LODESTAR_FASTFOURIERTRANSFORM_HPP


#ifndef CLASSIFIER_HPP
#define CLASSIFIER_HPP

#include <caffe/caffe.hpp>

#include <opencv/cv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include <dlib/image_processing/frontal_face_detector.h>
#include <dlib/image_processing/render_face_detections.h>
#include <dlib/image_processing.h>
#include <dlib/gui_widgets.h>
#include <dlib/image_io.h>
#include <dlib/opencv/to_open_cv.h>

#include <memory>
#include <string>

using namespace caffe;

class Classifier {
public:
    Classifier(const std::string& model_file, const std::string& trained_file, const std::string& mean_file, int cpu_only);
    Classifier(const std::string& model_file, const std::string& trained_file, int cpu_only);
    ~Classifier();
    std::vector<float> GetFeature(const cv::Mat& img);
    std::vector<float> GetFeature(const std::string& image_name);
    void SetMean(const std::string& mean_file);
    dlib::frontal_face_detector detector;
    dlib::shape_predictor sp;
private:

    void WrapInputLayer(std::vector<cv::Mat>* input_channels);
    void Preprocess(const cv::Mat& img, std::vector<cv::Mat>* input_channels);

private:
    std::shared_ptr<Net<float> > _net;
    cv::Size _input_geometry;
    int _num_channels;
    cv::Mat _mean;
    bool _flag_sub_mean = false;
    int scale_ = 1;
};

Classifier::~Classifier() {

}

Classifier::Classifier(const std::string& model_file,
                       const std::string& trained_file,
                       const std::string& mean_file, int cpu_only) {
    _flag_sub_mean = true;
    if (cpu_only == 1) {
        Caffe::set_mode(Caffe::CPU);
    }
    else {
        Caffe::set_mode(Caffe::GPU);
    }

    /* Load the network. */
    _net.reset(new Net<float>(model_file, TEST));
    _net->CopyTrainedLayersFrom(trained_file);

    CHECK_EQ(_net->num_inputs(), 1) << "Network should have exactly one input.";
    CHECK_EQ(_net->num_outputs(), 1) << "Network should have exactly one output.";

    Blob<float>* input_layer = _net->input_blobs()[0];
    _num_channels = input_layer->channels();
    CHECK(_num_channels == 3 || _num_channels == 1)
        << "Input layer should have 1 or 3 channels.";

    _input_geometry = cv::Size(input_layer->width(), input_layer->height());
    SetMean(mean_file);
    Blob<float>* output_layer = _net->output_blobs()[0];
    detector = dlib::get_frontal_face_detector();
    dlib::deserialize("shape_predictor_68_face_landmarks.dat") >> sp;
}

Classifier::Classifier(const std::string& model_file, const std::string& trained_file, int cpu_only) {
    _flag_sub_mean = false;
    if (cpu_only == 1) {
        Caffe::set_mode(Caffe::CPU);
    }
    else {
        Caffe::set_mode(Caffe::GPU);
    }
    _net.reset(new Net<float>(model_file, TEST));
    _net->CopyTrainedLayersFrom(trained_file);
    CHECK_EQ(_net->num_inputs(), 1) << "Network should have exactly one input.";
    CHECK_EQ(_net->num_outputs(), 1) << "Network should have exactly one output.";

    //get the address of Point
    Blob<float>* input_layer = _net->input_blobs()[0];
    _num_channels = input_layer->channels();
    CHECK(_num_channels == 3 || _num_channels == 1) << "Input layer should have 1 or 3 channels.";

    //get size
    _input_geometry = cv::Size(input_layer->width(), input_layer->height());
    Blob<float>* output_layer = _net->output_blobs()[0];
    detector = dlib::get_frontal_face_detector();
    dlib::deserialize("./resource/shape_predictor_68_face_landmarks.dat") >> sp;

}

void Classifier::SetMean(const string& mean_file) {
    BlobProto blob_proto;
    ReadProtoFromBinaryFileOrDie(mean_file.c_str(), &blob_proto);
    Blob<float> mean_blob;
    mean_blob.FromProto(blob_proto);
    CHECK_EQ(mean_blob.channels(), _num_channels) << "Number of channels of mean file doesn't match input layer.";
    std::vector<cv::Mat> channels;
    float* data = mean_blob.mutable_cpu_data();
    for (int i = 0; i < _num_channels; ++i) {
        cv::Mat channel(mean_blob.height(), mean_blob.width(), CV_32FC1, data);
        channels.push_back(channel);
        data += mean_blob.height() * mean_blob.width();
    }
    cv::Mat mean;
    cv::merge(channels, mean);
    cv::Scalar channel_mean = cv::mean(mean);
    _mean = cv::Mat(_input_geometry, mean.type(), channel_mean);
}

std::vector<float> Classifier::GetFeature(const cv::Mat& img) {
    double time1;
    Blob<float>* input_layer = _net->input_blobs()[0];

    input_layer->Reshape(1, _num_channels,
        _input_geometry.height, _input_geometry.width);
    _net->Reshape();

    std::vector<cv::Mat> input_channels;

    WrapInputLayer(&input_channels);

    Preprocess(img, &input_channels);
    _net->Forward();

    Blob<float>* output_layer = _net->output_blobs()[0];
    const float* begin = output_layer->cpu_data();
    const float* end = begin + output_layer->channels();

    return std::vector<float>(begin, end);
}

std::vector<float> Classifier::GetFeature(const string& image_name) {
    double time1;
    cv::Mat img = cv::imread(image_name, -1);
    Blob<float>* input_layer = _net->input_blobs()[0];
    input_layer->Reshape(1, _num_channels, _input_geometry.height, _input_geometry.width);
    _net->Reshape();

    std::vector<cv::Mat> input_channels;
    WrapInputLayer(&input_channels);
    Preprocess(img, &input_channels);
    _net->Forward();

    Blob<float>* output_layer = _net->output_blobs()[0];
    const float* begin = output_layer->cpu_data();
    const float* end = begin + output_layer->channels();
    return std::vector<float>(begin, end);
}

void Classifier::WrapInputLayer(std::vector<cv::Mat>* input_channels) {
    Blob<float>* input_layer = _net->input_blobs()[0];
    int width = input_layer->width();
    int height = input_layer->height();
    float* input_data = input_layer->mutable_cpu_data();
    for (int i = 0; i < input_layer->channels(); ++i) {
        cv::Mat channel(height, width, CV_32FC1, input_data);
        input_channels->push_back(channel);
        input_data += width * height;
    }
}

void Classifier::Preprocess(const cv::Mat& img, std::vector<cv::Mat>* input_channels) {
    cv::Mat sample;
    if (img.channels() == 3 && _num_channels == 1)
        cv::cvtColor(img, sample, cv::COLOR_BGR2GRAY);
    else if (img.channels() == 4 && _num_channels == 1)
        cv::cvtColor(img, sample, cv::COLOR_BGRA2GRAY);
    else if (img.channels() == 4 && _num_channels == 3)
        cv::cvtColor(img, sample, cv::COLOR_BGRA2BGR);
    else if (img.channels() == 1 && _num_channels == 3)
        cv::cvtColor(img, sample, cv::COLOR_GRAY2BGR);
    else
        sample = img;

    cv::Mat sample_resized;
    if (sample.size() != _input_geometry)
        cv::resize(sample, sample_resized, _input_geometry);
    else
        sample_resized = sample;

    cv::Mat sample_float;
    if (_num_channels == 3)
        sample_resized.convertTo(sample_float, CV_32FC3);
    else
        sample_resized.convertTo(sample_float, CV_32FC1);

    cv::Mat sample_normalized;
    if (_flag_sub_mean == true) {
        cv::subtract(sample_float, _mean, sample_normalized);
        cv::split(sample_normalized, *input_channels);
    }
    else {
        cv::split(sample_float, *input_channels);
    }

    CHECK(reinterpret_cast<float*>(input_channels->at(0).data) == _net->input_blobs()[0]->cpu_data())
        << "Input channels are not wrapping the input layer of the network.";
}

#endif // CLASSIFIER_HPP


/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2021 INRIA.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#include "SimpleMatrix.hpp"

#include <assert.h>                                             // for assert
#include <memory>                                               // for __sha...
#include <iostream>                                             // for ostream
#include <boost/numeric/ublas/io.hpp>                           // for opera...
#include <boost/numeric/ublas/matrix_proxy.hpp>                 // for matri...
#include <boost/numeric/bindings/blas.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include "SimpleMatrixFriends.hpp"                              // for subprod
#include "SiconosAlgebra.hpp"    // for symmetric, triangular ...
#include "BlockMatrix.hpp"                                      // for Block...
#include "BlockMatrixIterators.hpp"                             // for Const...
#include "SiconosException.hpp"                           // for Sicon...
#include "ioMatrix.hpp"                                         // for read
#include "Tools.hpp"                                            // for toString
#include "bindings_utils.hpp"                                   // for fill
#include "NumericsMatrix.h"

#include "NumericsSparseMatrix.h"
#include "CSparseMatrix.h"

//#define DEBUG_MESSAGES
#include "siconos_debug.h"

#ifdef DEBUG_MESSAGES
#include "NumericsVector.h"
#include <cs.h>
#endif

using namespace Siconos;
namespace siconosBindings = boost::numeric::bindings::blas;
using std::cout;
using std::endl;

// =================================================
//                CONSTRUCTORS
// =================================================

SimpleMatrix::SimpleMatrix():
  SiconosMatrix(Siconos::DENSE)
{
  mat.Dense = new DenseMat(ublas::zero_matrix<double>());
}

// parameters: dimensions and type.
SimpleMatrix::SimpleMatrix(unsigned int row,
                           unsigned int col,
                           UBLAS_TYPE typ,
                           unsigned int upper,
                           unsigned int lower):
  SiconosMatrix(Siconos::DENSE)
{
  if(typ == DENSE)
  {
    mat.Dense = new DenseMat(ublas::zero_matrix<double>(row, col));
    // _num = 1; default value
  }
  else if(typ == TRIANGULAR)
  {
    mat.Triang = new TriangMat(ublas::zero_matrix<double>(row, col));
    _num = TRIANGULAR;
  }
  else if(typ == SYMMETRIC)
  {
    mat.Sym = new SymMat(ublas::zero_matrix<double>(row, col));
    _num = SYMMETRIC;
  }
  else if(typ == SPARSE)
  {
    mat.Sparse = new SparseMat(row, col, upper);
    _num = SPARSE;
    zero();
  }
  else if(typ == SPARSE_COORDINATE)
  {
    mat.SparseCoordinate = new SparseCoordinateMat(row, col, upper);
    _num = SPARSE_COORDINATE;
    zero();
  }
  else if(typ == BANDED)
  {
    mat.Banded = new BandedMat(row, col, upper, lower);
    _num = BANDED;
    zero();
  }
  else if(typ == ZERO)
  {
    mat.Zero = new ZeroMat(row, col);
    _num = ZERO;
  }
  else if(typ == IDENTITY)
  {
    mat.Identity = new IdentityMat(row, col);
    _num = IDENTITY;
  }
  else THROW_EXCEPTION("invalid type.");
}

// parameters: dimensions, input value and type
SimpleMatrix::SimpleMatrix(unsigned int row, unsigned int col, double inputValue, UBLAS_TYPE typ, unsigned int upper, unsigned int lower):
  SiconosMatrix(typ)
{
  // This constructor has sense only for dense matrices ...
  if(typ == DENSE)
  {
    mat.Dense = new DenseMat(ublas::scalar_matrix<double>(row, col, inputValue));
    // _num = Siconos::DENSE; default value
  }
  else
     THROW_EXCEPTION("invalid type.");
}

// // parameters: a vector (stl) of double and the type.
// SimpleMatrix::SimpleMatrix(const std::vector<double>& v, unsigned int row, unsigned int col, UBLAS_TYPE typ, unsigned int lower, unsigned int upper):
//   SiconosMatrix(1, row, col), _isPLUFactorized(false), _isQRFactorized(false), _isPLUInversed(false), _isCholeskyFactorized(false), _isCholeskyFactorizedInPlace(false)
// {
//   if( (  (v.size() != row*col) && (typ != SYMMETRIC && typ != BANDED) )
//       || (v.size() != row*row && typ == SYMMETRIC)
//       || (typ == BANDED && ( (v.size()) != (unsigned int)(std::max)(row, col)*(lower+1+upper) ) ))
//      THROW_EXCEPTION("invalid vector size");

//   if(typ == DENSE)
//     {
//       mat.Dense = new DenseMat(row,col);
//       // _num = Siconos::DENSE; default value
//     }
//   else if(typ == TRIANGULAR)
//     {
//       mat.Triang = new TriangMat(row,col);
//       _num = Siconos::TRIANGULAR;
//     }
//   else if(typ == SYMMETRIC)
//     {
//       mat.Sym = new SymMat(row);
//       _num = Siconos::SYMMETRIC;
//     }
//   else if(typ == SPARSE)
//     {
//        THROW_EXCEPTION("warning -- use constructor(const SparseMat &m) or constructor(UBLAS_TYPE, int row, int col) with UBLAS_TYPE = SPARSE");

//     }
//   else if(typ == BANDED)
//     {
//       mat.Banded = new BandedMat(row, col, lower, upper);
//       _num = Siconos::BANDED;
//     }
//   else
//      THROW_EXCEPTION("invalid type of matrix given");

//   std::copy(v.begin(), v.end(), (vect.Dense)->begin());

// }

// Copy constructors
SimpleMatrix::SimpleMatrix(const SimpleMatrix &m):
  SiconosMatrix(m.num())
{

  _isSymmetric = m.isSymmetric();
  _isPositiveDefinite = m.isPositiveDefinite();

  _isPLUFactorized= m.isPLUFactorized();
  _isPLUFactorizedInPlace= m.isPLUFactorizedInPlace();
  _isPLUInversed= m.isPLUInversed();

  if(_num == Siconos::DENSE)
  {
    mat.Dense = new DenseMat(m.size(0), m.size(1));
    noalias(*mat.Dense) = (*m.dense());
  }
  //   mat.Dense = new DenseMat(*m.dense());

  else if(_num == Siconos::TRIANGULAR)
    mat.Triang = new TriangMat(*m.triang());

  else if(_num == Siconos::SYMMETRIC)

    mat.Sym = new SymMat(*m.sym());

  else if(_num == Siconos::SPARSE)
    mat.Sparse = new SparseMat(*m.sparse());

  else if(_num == Siconos::SPARSE_COORDINATE)
    mat.SparseCoordinate = new SparseCoordinateMat(*m.sparseCoordinate());

  else if(_num == Siconos::BANDED)
    mat.Banded = new BandedMat(*m.banded());

  else if(_num == Siconos::ZERO)
    mat.Zero = new ZeroMat(m.size(0), m.size(1));

  else// if(_num == Siconos::IDENTITY)
    mat.Identity = new IdentityMat(m.size(0), m.size(1));
}

/** copy constructor of a block given by the coord = [r0A r1A c0A c1A]
 *  \param A the matrix for extracting the block
 */
SimpleMatrix::SimpleMatrix(const SimpleMatrix& A, const Index& coord):
  SiconosMatrix(A.num())
{
  if(coord[0]>=coord[1])
     THROW_EXCEPTION("Empty row range coord[0]>= coord[1]");
  if(coord[2]>=coord[3])
     THROW_EXCEPTION("Empty column range coord[2]>= coord[3]");
  if(coord[1] > A.size(0))
     THROW_EXCEPTION("row index too large.");
  if(coord[3] > A.size(1))
     THROW_EXCEPTION("column index too large.");

  if(_num == Siconos::DENSE)
  {
    ublas::matrix_range<DenseMat> subA(*A.dense(), ublas::range(coord[0], coord[1]), ublas::range(coord[2], coord[3]));
    mat.Dense=new DenseMat(subA);
  }
  else if(_num == Siconos::TRIANGULAR)
  {
    ublas::matrix_range<TriangMat> subA(*A.triang(), ublas::range(coord[0], coord[1]), ublas::range(coord[2], coord[3]));
    mat.Triang=new TriangMat(subA);
  }
  else if(_num == Siconos::SYMMETRIC)
  {
    ublas::matrix_range<SymMat> subA(*A.sym(), ublas::range(coord[0], coord[1]), ublas::range(coord[2], coord[3]));
    mat.Sym=new SymMat(subA);
  }
  else if(_num == Siconos::SPARSE)
  {
    ublas::matrix_range<SparseMat> subA(*A.sparse(), ublas::range(coord[0], coord[1]), ublas::range(coord[2], coord[3]));
    mat.Sparse=new SparseMat(subA);
  }
  else if(_num == Siconos::SPARSE_COORDINATE)
  {
    ublas::matrix_range<SparseCoordinateMat> subA(*A.sparseCoordinate(), ublas::range(coord[0], coord[1]), ublas::range(coord[2], coord[3]));
    mat.SparseCoordinate=new SparseCoordinateMat(subA);
  }
  else if(_num == Siconos::BANDED)
  {
    ublas::matrix_range<BandedMat> subA(*A.banded(), ublas::range(coord[0], coord[1]), ublas::range(coord[2], coord[3]));
    mat.Banded=new BandedMat(subA);
  }
  else if(_num == Siconos::ZERO)
  {
    mat.Zero = new ZeroMat(coord[1]-coord[0], coord[3]-coord[2]);
  }
  else// if(_num == Siconos::IDENTITY)
    mat.Identity = new IdentityMat(coord[1]-coord[0], coord[3]-coord[2]);
}

SimpleMatrix::SimpleMatrix(const SiconosMatrix &m):
  SiconosMatrix(m.num())
{
  // _num is set in SiconosMatrix constructor with m.num() ... must be changed if m is Block
  Siconos::UBLAS_TYPE numM = m.num();

  _isSymmetric = m.isSymmetric();
  _isPositiveDefinite = m.isPositiveDefinite();

  _isPLUFactorized= m.isPLUFactorized();
  _isPLUFactorizedInPlace= m.isPLUFactorizedInPlace();
  _isPLUInversed= m.isPLUInversed();

  if(m.ipiv())
    _ipiv.reset(new VInt(*(m.ipiv())));

  if(numM == 0)  // ie if m is Block, this matrix is set to a dense.
  {
    const BlockMatrix& mB = static_cast<const BlockMatrix&>(m);
    _num = Siconos::DENSE;
    // get number of blocks in a row/col of m.
    mat.Dense = new DenseMat(m.size(0), m.size(1));
    ConstBlocksIterator1 it;
    ConstBlocksIterator2 it2;
    unsigned int posRow = 0;
    unsigned int posCol = 0;

    for(it = mB._mat->begin1(); it != mB._mat->end1(); ++it)
    {
      for(it2 = it.begin(); it2 != it.end(); ++it2)
      {
        setBlock(posRow, posCol, **it2);
        posCol += (*it2)->size(1);
      }
      posRow += (*it)->size(0);
      posCol = 0;
    }
  }
  else if(_num == Siconos::DENSE)
  {
    mat.Dense = new DenseMat(m.size(0), m.size(1));
    noalias(*mat.Dense) = (*m.dense());
  }

  else if(_num == Siconos::TRIANGULAR)
    mat.Triang = new TriangMat(*m.triang());

  else if(_num == Siconos::SYMMETRIC)
    mat.Sym = new SymMat(*m.sym());

  else if(_num == Siconos::SPARSE)
    mat.Sparse = new SparseMat(*m.sparse());

  else if(_num == Siconos::SPARSE_COORDINATE)
    mat.SparseCoordinate = new SparseCoordinateMat(*m.sparseCoordinate());

  else if(_num == Siconos::BANDED)
    mat.Banded = new BandedMat(*m.banded());

  else if(_num == Siconos::ZERO)
    mat.Zero = new ZeroMat(m.size(0), m.size(1));

  else // if(_num == Siconos::IDENTITY)
    mat.Identity = new IdentityMat(m.size(0), m.size(1));
}

SimpleMatrix::SimpleMatrix(const DenseMat& m):
  SiconosMatrix(Siconos::DENSE)
{
  mat.Dense = new DenseMat(m);
}

SimpleMatrix::SimpleMatrix(const TriangMat& m):
  SiconosMatrix(Siconos::TRIANGULAR)
{
  mat.Triang = new TriangMat(m);
}

SimpleMatrix::SimpleMatrix(const SymMat& m):
  SiconosMatrix(Siconos::SYMMETRIC)
{
  mat.Sym = new SymMat(m);
}

SimpleMatrix::SimpleMatrix(const SparseMat& m):
  SiconosMatrix(Siconos::SPARSE)
{
  mat.Sparse = new SparseMat(m);
}

SimpleMatrix::SimpleMatrix(const SparseCoordinateMat& m):
  SiconosMatrix(SPARSE_COORDINATE)
{
  mat.SparseCoordinate = new SparseCoordinateMat(m);
}

SimpleMatrix::SimpleMatrix(const BandedMat& m):
  SiconosMatrix(Siconos::BANDED)
{
  mat.Banded = new BandedMat(m);
}

SimpleMatrix::SimpleMatrix(const ZeroMat& m):
  SiconosMatrix(Siconos::ZERO)
{
  mat.Zero = new ZeroMat(m);
}

SimpleMatrix::SimpleMatrix(const IdentityMat& m):
  SiconosMatrix(Siconos::IDENTITY)
{
  mat.Identity = new IdentityMat(m);
}

SimpleMatrix::SimpleMatrix(const std::string &file, bool ascii):
  SiconosMatrix(Siconos::DENSE)
{
  mat.Dense = new DenseMat();
  if(ascii)
  {
    ioMatrix::read(file, "ascii", *this);
  }
  else
  {
    ioMatrix::read(file, "binary", *this);
  }
}

SimpleMatrix::~SimpleMatrix()
{
  if(_num == Siconos::DENSE)
  {
    delete(mat.Dense);
    if (_numericsMatrix)
    {
      // _numericsMatrix->matrix0 points to the array contained in the ublas matrix
      // To avoid double free on this pointer, we set it to NULL before deletion
      if (_numericsMatrix->matrix0)
        _numericsMatrix->matrix0 =nullptr;
    }
  }
  else if(_num == Siconos::TRIANGULAR)
    delete(mat.Triang);
  else if(_num == Siconos::SYMMETRIC)
    delete(mat.Sym);
  else if(_num == Siconos::SPARSE_COORDINATE)
    delete(mat.SparseCoordinate);
  else if(_num == Siconos::SPARSE)
    delete(mat.Sparse);
  else if(_num == Siconos::BANDED)
    delete(mat.Banded);
  else if(_num == Siconos::ZERO)
    delete(mat.Zero);
  else if(_num == Siconos::IDENTITY)
    delete(mat.Identity);
}

void SimpleMatrix::updateNumericsMatrix()
{
  /* set the numericsMatrix */
  NumericsMatrix * NM;
  if(_num == DENSE)
  {
    _numericsMatrix.reset(NM_new(),NM_free_not_dense); // When we reset, we do not free the matrix0
                                                        //that is linked to the array of the boost container
    NM = _numericsMatrix.get();
    double * data = (double*)(getArray());
    DEBUG_EXPR(NV_display(data,size(0)*size(1)););
    NM_fill(NM, NM_DENSE, size(0), size(1), data ); // Pointer link
  }
  else
  {
    // For all the other cases, we build a sparse matrix and we call numerics for the factorization of a sparse matrix.
    _numericsMatrix.reset(NM_create(NM_SPARSE, size(0), size(1)),NM_free);
    NM = _numericsMatrix.get();
    _numericsMatrix->matrix2->origin = NSM_CSC;
    NM_csc_alloc(NM, nnz());
    fillCSC(numericsSparseMatrix(NM)->csc, std::numeric_limits<double>::epsilon());
    DEBUG_EXPR(cs_print(numericsSparseMatrix(NM)->csc, 0););
  }
}

bool SimpleMatrix::checkSymmetry(double tol) const
{
  SP::SimpleMatrix  m_trans(new SimpleMatrix(*this));
  m_trans->trans();
  double err = (*this-*m_trans).normInf();
  if((*m_trans).normInf() > 0.0)
  {
    err /= (*m_trans).normInf();
  }
  // std::cout << "err_rel  ="<< err <<std::endl;
  return (err < tol);
}
//======================================
// get Ublas component (dense, sym ...)
//======================================

const DenseMat SimpleMatrix::getDense(unsigned int, unsigned int) const
{
  if(_num != Siconos::DENSE)
     THROW_EXCEPTION(" the current matrix is not a Dense matrix");

  return *mat.Dense;
}

const TriangMat SimpleMatrix::getTriang(unsigned int, unsigned int) const
{
  if(_num != Siconos::TRIANGULAR)
     THROW_EXCEPTION("the current matrix is not a Triangular matrix");

  return *mat.Triang;
}

const SymMat SimpleMatrix::getSym(unsigned int, unsigned int) const
{
  if(_num != Siconos::SYMMETRIC)
     THROW_EXCEPTION("he current matrix is not a Symmetric matrix");

  return *mat.Sym;
}

const SparseMat SimpleMatrix::getSparse(unsigned int, unsigned int) const
{
  if(_num != Siconos::SPARSE)
     THROW_EXCEPTION("the current matrix is not a Sparse matrix");

  return *mat.Sparse;
}

const SparseCoordinateMat SimpleMatrix::getSparseCoordinate(unsigned int, unsigned int) const
{
  if(_num != Siconos::SPARSE_COORDINATE)
     THROW_EXCEPTION("the current matrix is not a Sparse Coordinate matrix");

  return *mat.SparseCoordinate;
}
const BandedMat SimpleMatrix::getBanded(unsigned int, unsigned int) const
{
  if(_num != Siconos::BANDED)
     THROW_EXCEPTION("the current matrix is not a Banded matrix");

  return *mat.Banded;
}

const ZeroMat SimpleMatrix::getZero(unsigned int, unsigned int) const
{
  if(_num != Siconos::ZERO)
     THROW_EXCEPTION("the current matrix is not a Zero matrix");

  return *mat.Zero;
}

const IdentityMat SimpleMatrix::getIdentity(unsigned int, unsigned int) const
{
  if(_num != Siconos::IDENTITY)
     THROW_EXCEPTION("the current matrix is not a Identity matrix");

  return *mat.Identity;
}

DenseMat* SimpleMatrix::dense(unsigned int, unsigned int) const
{
  if(_num != Siconos::DENSE)
     THROW_EXCEPTION("the current matrix is not a Dense matrix");

  return mat.Dense;
}

TriangMat* SimpleMatrix::triang(unsigned int, unsigned int) const
{
  if(_num != Siconos::TRIANGULAR)
     THROW_EXCEPTION("the current matrix is not a Triangular matrix");

  return mat.Triang;
}

SymMat* SimpleMatrix::sym(unsigned int, unsigned int) const
{
  if(_num != Siconos::SYMMETRIC)
     THROW_EXCEPTION("the current matrix is not a Symmetric matrix");

  return mat.Sym;
}

SparseMat* SimpleMatrix::sparse(unsigned int, unsigned int) const
{
  if(_num != Siconos::SPARSE)
     THROW_EXCEPTION("the current matrix is not a Sparse matrix");

  return mat.Sparse;
}

SparseCoordinateMat* SimpleMatrix::sparseCoordinate(unsigned int, unsigned int) const
{
  if(_num != Siconos::SPARSE_COORDINATE)
     THROW_EXCEPTION("the current matrix is not a Sparse matrix");

  return mat.SparseCoordinate;
}

BandedMat* SimpleMatrix::banded(unsigned int, unsigned int) const
{
  if(_num != Siconos::BANDED)
     THROW_EXCEPTION("the current matrix is not a Banded matrix");

  return mat.Banded;
}

ZeroMat* SimpleMatrix::zero_mat(unsigned int, unsigned int) const
{
  if(_num != Siconos::ZERO)
     THROW_EXCEPTION("the current matrix is not a Zero matrix");

  return mat.Zero;
}

IdentityMat* SimpleMatrix::identity(unsigned int, unsigned int) const
{
  if(_num != Siconos::IDENTITY)
     THROW_EXCEPTION("the current matrix is not a Identity matrix");

  return mat.Identity;
}

double* SimpleMatrix::getArray(unsigned int, unsigned int) const
{
  if(_num == Siconos::SPARSE)
     THROW_EXCEPTION("not yet implemented for sparse matrix.");

  if(_num == Siconos::DENSE)
    return (((*mat.Dense).data()).data());
  else if(_num == Siconos::TRIANGULAR)
    return &(((*mat.Triang).data())[0]);
  else if(_num == Siconos::SYMMETRIC)
    return &(((*mat.Sym).data())[0]);
  else if(_num == Siconos::ZERO)
  {
    ZeroMat::iterator1 it = (*mat.Zero).begin1();
    return const_cast<double*>(&(*it));
  }
  else if(_num == Siconos::IDENTITY)
  {
    IdentityMat::iterator1 it = (*mat.Identity).begin1();
    return const_cast<double*>(&(*it));
  }
  else
    return &(((*mat.Banded).data())[0]);
}

// ===========================
//       fill matrix
// ===========================

void SimpleMatrix::zero()
{
  unsigned int size1 = size(0);
  unsigned int size2 = size(1);
  if(_num == Siconos::DENSE)
    *mat.Dense = ublas::zero_matrix<double>(size1, size2);
  else if(_num == Siconos::TRIANGULAR)
    *mat.Triang = ublas::zero_matrix<double>(size1, size2);

  else if(_num == Siconos::SYMMETRIC)
    *mat.Sym = ublas::zero_matrix<double>(size1, size2);

  else if(_num == Siconos::SPARSE)
    *mat.Sparse = ublas::zero_matrix<double>(size1, size2);

  else if(_num == Siconos::SPARSE_COORDINATE)
    *mat.SparseCoordinate = ublas::zero_matrix<double>(size1, size2);

  else if(_num == Siconos::BANDED)
    *mat.Banded = ublas::zero_matrix<double>(size1, size2);

  else if(_num == Siconos::IDENTITY)
     THROW_EXCEPTION("you can not set to zero a matrix of type Identity!.");
  resetFactorizationFlags();
  // if _num == Siconos::ZERO: nothing
}

void SimpleMatrix::randomize()
{
  if(_num == Siconos::DENSE)
    Siconos::algebra::fill(*mat.Dense);
  else
     THROW_EXCEPTION("only implemented for dense matrices.");
  resetFactorizationFlags();
}

void SimpleMatrix::randomize_sym()
{
  if(_num == Siconos::DENSE)
    Siconos::algebra::fill_sym(*mat.Dense);
  else
     THROW_EXCEPTION("only implemented for dense matrices.");
  resetFactorizationFlags();
}

void SimpleMatrix::eye()
{
  unsigned int size1 = size(0);
  unsigned int size2 = size(1);
  if(_num == Siconos::DENSE)
    *mat.Dense = ublas::identity_matrix<double>(size1, size2);

  else if(_num == Siconos::TRIANGULAR)
    *mat.Triang = ublas::identity_matrix<double>(size1, size2);

  else if(_num == Siconos::SYMMETRIC)
    *mat.Sym = ublas::identity_matrix<double>(size1, size2);

  else if(_num == Siconos::SPARSE)
    *mat.Sparse = ublas::identity_matrix<double>(size1, size2);

  else if(_num == Siconos::BANDED)
    *mat.Banded = ublas::identity_matrix<double>(size1, size2);

  else if(_num == Siconos::ZERO)
     THROW_EXCEPTION("you can not set to identity a matrix of type Zero!.");
  resetFactorizationFlags();
}

unsigned int SimpleMatrix::size(unsigned int index) const
{
  if(_num == Siconos::DENSE)
  {
    if(index == 0) return (*mat.Dense).size1();
    else  return (*mat.Dense).size2();
  }
  else if(_num == Siconos::TRIANGULAR)
  {
    if(index == 0) return (*mat.Triang).size1();
    else return (*mat.Triang).size2();
  }
  else if(_num == Siconos::SYMMETRIC)
  {
    if(index == 0) return (*mat.Sym).size1();
    else  return (*mat.Sym).size2();
  }
  else if(_num == Siconos::SPARSE)
  {
    if(index == 0) return (*mat.Sparse).size1();
    else return (*mat.Sparse).size2();
  }
  else if(_num == Siconos::SPARSE_COORDINATE)
  {
    if(index == 0) return (*mat.SparseCoordinate).size1();
    else return (*mat.SparseCoordinate).size2();
  }
  else if(_num == Siconos::BANDED)
  {
    if(index == 0) return (*mat.Banded).size1();
    else  return (*mat.Banded).size2();
  }
  else if(_num == Siconos::ZERO)
  {
    if(index == 0) return (*mat.Zero).size1();
    else  return (*mat.Zero).size2();
  }
  else if(_num == Siconos::IDENTITY)
  {
    if(index == 0) return (*mat.Identity).size1();
    else  return (*mat.Identity).size2();
  }
  else return 0;

};

//=======================
// set matrix dimension
//=======================

void SimpleMatrix::resize(unsigned int row, unsigned int col, unsigned int lower, unsigned int upper, bool preserve)
{

  if(_num == Siconos::DENSE)
  {
    (*mat.Dense).resize(row, col, preserve);
  }
  else if(_num == Siconos::TRIANGULAR)
  {
    (*mat.Triang).resize(row, col, preserve);
  }
  else if(_num == Siconos::SYMMETRIC)
  {
    (*mat.Sym).resize(row, col, preserve);
  }
  else if(_num == Siconos::SPARSE)
  {
    (*mat.Sparse).resize(row, col, preserve);
  }
  else if(_num == Siconos::SPARSE_COORDINATE)
  {
    (*mat.SparseCoordinate).resize(row, col, preserve);
  }
  else if(_num == Siconos::BANDED)
  {
    (*mat.Banded).resize(row, col, lower, upper, preserve);
  }
  else if(_num == Siconos::ZERO)
  {
    (*mat.Zero).resize(row, col, preserve);
  }
  else if(_num == Siconos::IDENTITY)
  {
    (*mat.Identity).resize(row, col, preserve);
  }
  resetFactorizationFlags();
}

//=====================
// screen display
//=====================

void SimpleMatrix::display() const
{
  std::cout.setf(std::ios::scientific);
  std::cout.precision(6);

  if(size(0) == 0 || size(1) ==0)
  {
    std::cout << "SimpleMatrix::display(): empty matrix" << std::endl;
  }
  std::cout << "SimpleMatrix storage type - num = " << _num << "\n";
  if(_num == Siconos::DENSE)
  {
    Siconos::algebra::print_m(*mat.Dense);
    //std::cout << *mat.Dense << std::endl;
  }
  else if(_num == Siconos::TRIANGULAR)
    std::cout << *mat.Triang << std::endl;
  else if(_num == Siconos::SYMMETRIC)
    std::cout << *mat.Sym << std::endl;
  else if(_num == Siconos::SPARSE)
  {
    std::cout << "non zero element (nnz) = " <<  mat.Sparse->nnz() << std::endl;

    std::cout << *mat.Sparse << std::endl;
  }
  else if(_num == Siconos::SPARSE_COORDINATE)
  {
    std::cout << *mat.SparseCoordinate << std::endl;
  }
  else if(_num == Siconos::BANDED)
    std::cout << *mat.Banded << std::endl;
  else if(_num == Siconos::ZERO)
    std::cout << *mat.Zero << std::endl;
  else if(_num == Siconos::IDENTITY)
    std::cout << *mat.Identity << std::endl;
}
void SimpleMatrix::displayExpert(bool brief) const
{
  std::cout.setf(std::ios::scientific);
  std::cout.precision(6);

  if(size(0) == 0 || size(1) ==0)
  {
    std::cout << "SimpleMatrix::display(): empty matrix" << std::endl;
  }
  std::cout << "SimpleMatrix storage type - num = " << _num << "\n";
  if(_num == Siconos::DENSE)
  {
    Siconos::algebra::print_m(*mat.Dense);
    //std::cout << *mat.Dense << std::endl;
  }
  else if(_num == Siconos::TRIANGULAR)
    std::cout << *mat.Triang << std::endl;
  else if(_num == Siconos::SYMMETRIC)
    std::cout << *mat.Sym << std::endl;
  else if(_num == Siconos::SPARSE)
  {
    std::cout << "non zero element (nnz) = " <<  mat.Sparse->nnz() << std::endl;
    std::cout << "non zero element (nnz_capacity) = " <<  mat.Sparse->nnz_capacity() << std::endl;
    std::cout << "filled1 = " <<  mat.Sparse->filled1() << std::endl;
    std::cout << "filled2 = " <<  mat.Sparse->filled2() << std::endl;

    std::cout << "index_data1 = [ " ;
    size_t i=0;
    for(i = 0; i < mat.Sparse->filled1()-1 ; i++)
    {
      std::cout << mat.Sparse->index1_data()[i] << ", " ;
    }
    std::cout << mat.Sparse->index1_data()[i] << "]" <<  std::endl;

    std::cout << "index_data2 = [" ;
    for(i = 0; i < mat.Sparse->filled2()-1 ; i++)
    {
      std::cout << mat.Sparse->index2_data()[i] << ", " ;
    }
    std::cout << mat.Sparse->index2_data()[i] << "]" << std::endl;

    std::cout << "value_data = [" ;
    for(i = 0; i < mat.Sparse->filled2()-1 ; i++)
    {
      std::cout << mat.Sparse->value_data()[i] << ", " ;
    }
    std::cout << mat.Sparse->value_data()[i] << "]" << std::endl;

    std::cout << *mat.Sparse << std::endl;
  }
  else if(_num == Siconos::SPARSE_COORDINATE)
  {
    std::cout << "non zero element (nnz) = " <<  mat.SparseCoordinate->nnz() << std::endl;

    for(size_t i = 0; i < mat.SparseCoordinate->nnz(); ++i)
    {
      //std::cout << i << std::endl;
      std::cout << "M(" << mat.SparseCoordinate->index1_data()[i] << ", " ;
      std::cout << mat.SparseCoordinate->index2_data()[i] << ") =  " ;
      std::cout << mat.SparseCoordinate->value_data()[i] << std::endl;
    }
  }
  else if(_num == Siconos::BANDED)
    std::cout << *mat.Banded << std::endl;
  else if(_num == Siconos::ZERO)
    std::cout << *mat.Zero << std::endl;
  else if(_num == Siconos::IDENTITY)
    std::cout << *mat.Identity << std::endl;
}

//=====================
// convert to a string
//=====================

std::string SimpleMatrix::toString() const
{
  return ::toString(*this);
}

//=====================
// convert to an ostream
//=====================

std::ostream& operator<<(std::ostream& os, const SimpleMatrix& sm)
{
  if(sm._num == Siconos::DENSE)
    os << *sm.mat.Dense;
  else if(sm._num == Siconos::TRIANGULAR)
    os << *sm.mat.Triang;
  else if(sm._num == Siconos::SYMMETRIC)
    os << *sm.mat.Sym;
  else if(sm._num == Siconos::SPARSE)
    os << *sm.mat.Sparse;
  else if(sm._num == Siconos::BANDED)
    os << *sm.mat.Banded;
  else if(sm._num == Siconos::ZERO)
    os << *sm.mat.Zero;
  else if(sm._num == Siconos::IDENTITY)
    os << *sm.mat.Identity;
  return os;
}

void SimpleMatrix::assign(const SimpleMatrix &smat)
{

  switch(_num)
  {
  case Siconos::SPARSE:
  {

    switch(smat.num())
    {
    case Siconos::SPARSE:
    {
      mat.Sparse->assign(smat.getSparse());
      break;
    }
    default:
    {
    }

    }
  }
  default:
  {
     THROW_EXCEPTION("do not know how to assign for the given storage type ");
  }
  }
}

// void prod(const SiconosMatrix& A, const BlockVector& x, SiconosVector& y, bool init)
// {
//   assert(!(A.isPLUFactorizedInPlace()) && "A is PLUFactorizedInPlace in prod !!");
//   if(init)
//     y.zero();
//   unsigned int startRow = 0;
//   unsigned int startCol = 0;
//   // In private_addprod, the sum of all blocks of x, x[i], is computed: y = Sum_i (subA x[i]), with subA a submatrix of A,
//   // starting from position startRow in rows and startCol in columns.
//   // private_prod takes also into account the fact that each block of x can also be a block.
//   VectorOfVectors::const_iterator it;
//   for(it = x.begin(); it != x.end(); ++it)
//   {
//     private_addprod(A, startRow, startCol, **it, y);
//     startCol += (*it)->size();
//   }
// }

// void private_addprod(const SiconosMatrix& A, unsigned int startRow, unsigned int startCol, const BlockVector& x, SiconosVector& y)
// {
//   assert(!(A.isPLUFactorizedInPlace()) && "A is PLUFactorizedInPlace in prod !!");
//   assert(!A.isBlock() && "private_addprod(A,start,x,y) error: not yet implemented for block matrix.");
//   VectorOfVectors::const_iterator it;
//   unsigned int startColBis = startCol;
//   for(it = x.begin(); it != x.end(); ++it)
//   {
//     private_addprod(A, startRow, startColBis, **it, y);
//     startColBis += (*it)->size();
//   }

// }

// // x block, y siconos
// void private_prod(const SiconosMatrix& A, unsigned int startRow, const BlockVector& x, SiconosVector& y, bool init)
// {
//   assert(!(A.isPLUFactorizedInPlace()) && "A is PLUFactorizedInPlace in prod !!");
//   // Computes y = subA *x (or += if init = false), subA being a sub-matrix of A, between el. of index (row) startRow and startRow + sizeY
//   if(init)  // y = subA * x , else y += subA * x
//     y.zero();
//   private_addprod(A, startRow, 0, x, y);
// }

// // x and y blocks
// void private_prod(SPC::SiconosMatrix A, const unsigned int startRow, SPC::BlockVector x, SP::BlockVector y, bool init)
// {
//   assert(!(A->isPLUFactorizedInPlace()) && "A is PLUFactorizedInPlace in prod !!");

//   unsigned int row = startRow;
//   VectorOfVectors::const_iterator it;
//   for(it = y->begin(); it != y->end(); ++it)
//   {
//     private_prod(*A, row, *x, **it, init);
//     row += (*it)->size();
//   }
// }

// // x and y blocks
// void private_prod(SPC::SiconosMatrix A, const unsigned int startRow, SPC::SiconosVector x, SP::BlockVector y, bool init)
// {
//   assert(!(A->isPLUFactorizedInPlace()) && "A is PLUFactorizedInPlace in prod !!");

//   unsigned int row = startRow;
//   VectorOfVectors::const_iterator it;
//   for(it = y->begin(); it != y->end(); ++it)
//   {
//     private_prod(*A, row, *x, **it, init);
//     row += (*it)->size();
//   }
// }

// void private_addprod(SPC::BlockVector x, SPC::SiconosMatrix A, unsigned int startRow, unsigned int startCol, SP::SiconosVector y)
// {
//   assert(!(A->isPLUFactorizedInPlace()) && "A is PLUFactorizedInPlace in prod !!");
//   VectorOfVectors::const_iterator it;
//   unsigned int startColBis = startCol;
//   for(it = x->begin(); it != x->end(); ++it)
//   {
//     private_addprod((*it), A, startRow, startColBis, y);
//     startColBis += (*it)->size();
//   }

// }

// void private_prod(SPC::SiconosVector x, SPC::SiconosMatrix A, unsigned int startCol, SP::BlockVector  y, bool init)
// {
//   assert(!(A->isPLUFactorizedInPlace()) && "A is PLUFactorizedInPlace in prod !!");

//   unsigned int col = startCol;
//   VectorOfVectors::const_iterator it;
//   for(it = y->begin(); it != y->end(); ++it)
//   {
//     private_prod(x, A, col, *it, init);
//     col += (*it)->size();
//   }
// }

// void private_prod(SPC::BlockVector x, SPC::SiconosMatrix A, unsigned int startCol, SP::SiconosVector  y, bool init)
// {
//   assert(!(A->isPLUFactorizedInPlace()) && "A is PLUFactorizedInPlace in prod !!");

//   // Computes y = subA *x (or += if init = false), subA being a sub-matrix of trans(A), between el. of A of index (col) startCol and startCol + sizeY
//   if(init)  // y = subA * x , else y += subA * x
//     y->zero();
//   private_addprod(x, A, startCol, 0, y);

// }

// void private_prod(SPC::BlockVector x, SPC::SiconosMatrix A, unsigned int startCol, SP::BlockVector  y, bool init)
// {
//   assert(!(A->isPLUFactorizedInPlace()) && "A is PLUFactorizedInPlace in prod !!");

//   unsigned int col = startCol;
//   VectorOfVectors::const_iterator it;
//   for(it = y->begin(); it != y->end(); ++it)
//   {
//     private_prod(x, A, col, *it, init);
//     col += (*it)->size();
//   }
// }

// void private_addprod(double a, SPC::SiconosMatrix A, unsigned int startRow, unsigned int startCol, SPC::SiconosVector x, SP::SiconosVector y)
// {
//   assert(!(A->isPLUFactorizedInPlace()) && "A is PLUFactorizedInPlace in prod !!");

//   if(A->isBlock())
//      THROW_EXCEPTION("not yet implemented for block matrix.");

//   // we take a submatrix subA of A, starting from row startRow to row (startRow+sizeY) and between columns startCol and (startCol+sizeX).
//   // Then computation of y = subA*x + y.
//   Siconos::UBLAS_TYPE numA = A->num();
//   Siconos::UBLAS_TYPE numY = y->num();
//   Siconos::UBLAS_TYPE numX = x->num();
//   unsigned int sizeX = x->size();
//   unsigned int sizeY = y->size();

//   if(numX != numY)
//      THROW_EXCEPTION("not yet implemented for x and y of different types.");

//   if(numY == 1 && numX == 1)
//   {

//     assert(y->dense() != x->dense());

//     if(numA == 1)
//       noalias(*y->dense()) += a * prod(ublas::subrange(*A->dense(), startRow, startRow + sizeY, startCol, startCol + sizeX), *x->dense());
//     else if(numA == 2)
//       noalias(*y->dense()) += a * prod(ublas::subrange(*A->triang(), startRow, startRow + sizeY, startCol, startCol + sizeX), *x->dense());
//     else if(numA == 3)
//       noalias(*y->dense()) += a * prod(ublas::subrange(*A->sym(), startRow, startRow + sizeY, startCol, startCol + sizeX), *x->dense());
//     else if(numA == 4)
//       noalias(*y->dense()) += a * prod(ublas::subrange(*A->sparse(), startRow, startRow + sizeY, startCol, startCol + sizeX), *x->dense());
//     else //if(numA==5)
//       noalias(*y->dense()) += a * prod(ublas::subrange(*A->banded(), startRow, startRow + sizeY, startCol, startCol + sizeX), *x->dense());
//   }
//   else // x and y sparse
//   {
//     if(numA == 4)
//       *y->sparse() += a * prod(ublas::subrange(*A->sparse(), startRow, startRow + sizeY, startCol, startCol + sizeX), *x->sparse());
//     else
//        THROW_EXCEPTION("not yet implemented for x, y  sparse and A not sparse.");
//   }

// }

// void private_prod(double a, SPC::SiconosMatrix A, unsigned int startRow, SPC::SiconosVector x, SP::SiconosVector  y, bool init)
// {
//   assert(!(A->isPLUFactorizedInPlace()) && "A is PLUFactorizedInPlace in prod !!");

//   // Computes y = subA *x (or += if init = false), subA being a sub-matrix of A, between el. of index (row) startRow and startRow + sizeY

//   if(init)  // y = subA * x , else y += subA * x
//     y->zero();
//   private_addprod(a, A, startRow, 0, x, y);

// }

unsigned SimpleMatrix::copyData(double* data) const
{
  assert((_num == Siconos::DENSE) && "SiconosMatrix::copyData : forbidden: the current matrix is not dense.");

  unsigned size = mat.Dense->size1() * mat.Dense->size2();
  siconosBindings::detail::copy(size, getArray(), 1, data, 1);
  return size;
}


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

#include <boost/simd/boolean/functions/selsub.hpp>
#include <boost/simd/include/functions/simd/if_else_zero.hpp>
#include <boost/simd/include/functions/simd/minus.hpp>
#include <boost/simd/include/functions/simd/plus.hpp>
#include <boost/simd/include/functions/simd/negate.hpp>
#include <boost/simd/sdk/meta/cardinal_of.hpp>
#include <boost/mpl/equal_to.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION_IF ( boost::simd::tag::selsub_, tag::cpu_, (A0)(A1)(X)
                                , (boost::mpl::equal_to < boost::simd::meta::cardinal_of<A0>
                                                        , boost::simd::meta::cardinal_of<A1>
                                                        >
                                  )
                                , ((simd_<unspecified_<A0>,X>))
                                  ((simd_<unspecified_<A1>,X>))
                                  ((simd_<unspecified_<A1>,X>))
                                )
  {
    typedef A1 result_type;
    inline result_type operator()(A0 const& a0, A1 const& a1, A1 const& a2) const
    {
     return a1 - if_else_zero(a0, a2);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION_IF ( boost::simd::tag::selsub_, tag::cpu_, (A0)(A1)(X)
                                , (boost::mpl::equal_to < boost::simd::meta::cardinal_of<A0>
                                                        , boost::simd::meta::cardinal_of<A1>
                                                        >
                                  )
                                , ((simd_<unspecified_<A0>,X>))
                                  ((simd_<floating_<A1>,X>))
                                  ((simd_<floating_<A1>,X>))
                                )
  {
    typedef A1 result_type;
    inline result_type operator()(A0 const& a0, A1 const& a1, A1 const& a2) const
    {
      // this is a workaround for a gcc (at least 4.6) over-optimization in case or a1 and a2 are
      // equal (constant?) and invalid (inf -inf or nan) in which case the general impl sometimes
      // return 0 in place of nan in float cases.
     return a1 + if_else_zero(a0, -a2);
    }
  };
} } }

#endif