#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <vector>
#include <map>
#include <string>
#include <iostream>
#include <random>
#include <Eigen/Geometry>
#include <arc_utilities/arc_helpers.hpp>

#ifndef BAXTER_ROBOT_CONFIG_HPP
#define BAXTER_ROBOT_CONFIG_HPP

namespace baxter_robot_controllers
{
    class PIDParams
    {
    protected:

        double kp_;
        double ki_;
        double kd_;
        double i_clamp_;

    public:

        PIDParams(const double kp, const double ki, const double kd, const double i_clamp) : kp_(kp), ki_(ki), kd_(kd), i_clamp_(i_clamp) {}

        PIDParams() : kp_(0.0), ki_(0.0), kd_(0.0), i_clamp_(0.0) {}

        inline double Kp() const
        {
            return kp_;
        }

        inline double Ki() const
        {
            return ki_;
        }

        inline double Kd() const
        {
            return kd_;
        }

        inline double Iclamp() const
        {
            return i_clamp_;
        }
    };

    class JointLimits
    {
    protected:

        double min_position_;
        double max_position_;
        double max_velocity_;
        double max_effort_;

    public:

        JointLimits(const double min_position, const double max_position, const double max_velocity, const double max_effort)
        {
            assert(min_position <= max_position);
            min_position_ = min_position;
            max_position_ = max_position;
            max_velocity_ = std::abs(max_velocity);
            max_effort_ = std::abs(max_effort);
        }

        JointLimits() : min_position_(0.0), max_position_(0.0), max_velocity_(0.0), max_effort_(0.0) {}

        inline double MinPosition() const
        {
            return min_position_;
        }

        inline double MaxPosition() const
        {
            return max_position_;
        }

        inline std::pair<double, double> PositionLimits() const
        {
            return std::make_pair(min_position_, max_position_);
        }

        inline double MaxVelocity() const
        {
            return max_velocity_;
        }

        inline double MaxEffort() const
        {
            return max_effort_;
        }
    };

    inline std::map<std::string, JointLimits> GetLeftArmDefaultLimits()
    {
        std::map<std::string, JointLimits> joint_limits;
        joint_limits["left_s0"] = JointLimits(-1.70167993878, 1.70167993878, 0.27, 500.0);
        joint_limits["left_s1"] = JointLimits(-2.147, 1.047, 0.27, 500.0);
        joint_limits["left_e0"] = JointLimits(-3.05417993878, 3.05417993878, 0.27, 500.0);
        joint_limits["left_e1"] = JointLimits(-0.05, 2.618, 0.27, 500.0);
        joint_limits["left_w0"] = JointLimits(-3.059, 3.059, 0.3, 500.0);
        joint_limits["left_w1"] = JointLimits(-1.57079632679, 2.094, 0.3, 500.0);
        joint_limits["left_w2"] = JointLimits(-3.059, 3.059, 0.5, 500.0);
        return joint_limits;
    }

    inline std::map<std::string, JointLimits> GetRightArmDefaultLimits()
    {
        std::map<std::string, JointLimits> joint_limits;
        joint_limits["right_s0"] = JointLimits(-1.70167993878, 1.70167993878, 0.27, 500.0);
        joint_limits["right_s1"] = JointLimits(-2.147, 1.047, 0.27, 500.0);
        joint_limits["right_e0"] = JointLimits(-3.05417993878, 3.05417993878, 0.27, 500.0);
        joint_limits["right_e1"] = JointLimits(-0.05, 2.618, 0.27, 500.0);
        joint_limits["right_w0"] = JointLimits(-3.059, 3.059, 0.3, 500.0);
        joint_limits["right_w1"] = JointLimits(-1.57079632679, 2.094, 0.3, 500.0);
        joint_limits["right_w2"] = JointLimits(-3.059, 3.059, 0.5, 500.0);
        return joint_limits;
    }

    inline std::map<std::string, JointLimits> GetLeftArmLimits(const double velocity_scaling=1.0, const double effort_scaling=1.0)
    {
        assert(velocity_scaling >= 0.0);
        assert(velocity_scaling <= 1.0);
        assert(effort_scaling >= 0.0);
        assert(effort_scaling <= 1.0);
        const std::map<std::string, JointLimits> default_joint_limits = GetLeftArmDefaultLimits();
        std::map<std::string, JointLimits> joint_limits;
        const auto default_limit_pairs = arc_helpers::GetKeysAndValues(default_joint_limits);
        for (size_t idx = 0; idx < default_limit_pairs.size(); idx++)
        {
            const std::string& joint_name = default_limit_pairs[idx].first;
            const JointLimits& default_joint_limit = default_limit_pairs[idx].second;
            const JointLimits scaled_joint_limit(default_joint_limit.MinPosition(), default_joint_limit.MaxPosition(), (default_joint_limit.MaxVelocity() * velocity_scaling), (default_joint_limit.MaxEffort() * effort_scaling));
            joint_limits[joint_name] = scaled_joint_limit;
        }
        return joint_limits;
    }

    inline std::map<std::string, JointLimits> GetRightArmLimits(const double velocity_scaling=1.0, const double effort_scaling=1.0)
    {
        assert(velocity_scaling >= 0.0);
        assert(velocity_scaling <= 1.0);
        assert(effort_scaling >= 0.0);
        assert(effort_scaling <= 1.0);
        const std::map<std::string, JointLimits> default_joint_limits = GetRightArmDefaultLimits();
        std::map<std::string, JointLimits> joint_limits;
        const auto default_limit_pairs = arc_helpers::GetKeysAndValues(default_joint_limits);
        for (size_t idx = 0; idx < default_limit_pairs.size(); idx++)
        {
            const std::string& joint_name = default_limit_pairs[idx].first;
            const JointLimits& default_joint_limit = default_limit_pairs[idx].second;
            const JointLimits scaled_joint_limit(default_joint_limit.MinPosition(), default_joint_limit.MaxPosition(), (default_joint_limit.MaxVelocity() * velocity_scaling), (default_joint_limit.MaxEffort() * effort_scaling));
            joint_limits[joint_name] = scaled_joint_limit;
        }
        return joint_limits;
    }

    inline std::map<std::string, PIDParams> GetLeftArmDefaultPositionControllerParams()
    {
        std::map<std::string, baxter_robot_controllers::PIDParams> joint_controller_params;
        joint_controller_params["left_s0"] = baxter_robot_controllers::PIDParams(1.0, 0.0, 0.1, 1.0);
        joint_controller_params["left_s1"] = baxter_robot_controllers::PIDParams(1.0, 0.0, 0.1, 1.0);
        joint_controller_params["left_e0"] = baxter_robot_controllers::PIDParams(1.0, 0.0, 0.1, 1.0);
        joint_controller_params["left_e1"] = baxter_robot_controllers::PIDParams(1.0, 0.0, 0.1, 1.0);
        joint_controller_params["left_w0"] = baxter_robot_controllers::PIDParams(1.0, 0.0, 0.1, 1.0);
        joint_controller_params["left_w1"] = baxter_robot_controllers::PIDParams(1.0, 0.0, 0.1, 1.0);
        joint_controller_params["left_w2"] = baxter_robot_controllers::PIDParams(2.0, 0.0, 0.1, 1.0);
        return joint_controller_params;
    }

    inline std::map<std::string, PIDParams> GetRightArmDefaultPositionControllerParams()
    {
        std::map<std::string, baxter_robot_controllers::PIDParams> joint_controller_params;
        joint_controller_params["right_s0"] = baxter_robot_controllers::PIDParams(1.0, 0.0, 0.1, 1.0);
        joint_controller_params["right_s1"] = baxter_robot_controllers::PIDParams(1.0, 0.0, 0.1, 1.0);
        joint_controller_params["right_e0"] = baxter_robot_controllers::PIDParams(1.0, 0.0, 0.1, 1.0);
        joint_controller_params["right_e1"] = baxter_robot_controllers::PIDParams(1.0, 0.0, 0.1, 1.0);
        joint_controller_params["right_w0"] = baxter_robot_controllers::PIDParams(1.0, 0.0, 0.1, 1.0);
        joint_controller_params["right_w1"] = baxter_robot_controllers::PIDParams(1.0, 0.0, 0.1, 1.0);
        joint_controller_params["right_w2"] = baxter_robot_controllers::PIDParams(2.0, 0.0, 0.1, 1.0);
        return joint_controller_params;
    }

    inline std::map<std::string, PIDParams> GetLeftArmDefaultVelocityControllerParams()
    {
        std::map<std::string, baxter_robot_controllers::PIDParams> joint_controller_params;
        joint_controller_params["left_s0"] = baxter_robot_controllers::PIDParams(2.0, 0.0, 0.0, 0.0);
        joint_controller_params["left_s1"] = baxter_robot_controllers::PIDParams(8.0, 0.0, 0.0, 0.0);
        joint_controller_params["left_e0"] = baxter_robot_controllers::PIDParams(3.0, 0.0, 0.0, 0.0);
        joint_controller_params["left_e1"] = baxter_robot_controllers::PIDParams(5.0, 0.0, 0.0, 0.0);
        joint_controller_params["left_w0"] = baxter_robot_controllers::PIDParams(2.0, 0.0, 0.0, 0.0);
        joint_controller_params["left_w1"] = baxter_robot_controllers::PIDParams(1.0, 0.0, 0.0, 0.0);
        joint_controller_params["left_w2"] = baxter_robot_controllers::PIDParams(1.0, 0.0, 0.0, 0.0);
        return joint_controller_params;
    }

    inline std::map<std::string, PIDParams> GetRightArmDefaultVelocityControllerParams()
    {
        std::map<std::string, baxter_robot_controllers::PIDParams> joint_controller_params;
        // Slow, bad
//        joint_controller_params["right_s0"] = baxter_robot_controllers::PIDParams(1.0, 0.0, 0.000001, 0.0);
//        joint_controller_params["right_s1"] = baxter_robot_controllers::PIDParams(4.0, 0.0, 0.000001, 0.0);
//        joint_controller_params["right_e0"] = baxter_robot_controllers::PIDParams(1.5, 0.0, 0.000001, 0.0);
//        joint_controller_params["right_e1"] = baxter_robot_controllers::PIDParams(2.5, 0.0, 0.000001, 0.0);
//        joint_controller_params["right_w0"] = baxter_robot_controllers::PIDParams(1.0, 0.0, 0.000001, 0.0);
//        joint_controller_params["right_w1"] = baxter_robot_controllers::PIDParams(0.5, 0.0, 0.000001, 0.0);
//        joint_controller_params["right_w2"] = baxter_robot_controllers::PIDParams(0.5, 0.0, 0.000001, 0.0);
        // Stable, okish
//        joint_controller_params["right_s0"] = baxter_robot_controllers::PIDParams(2.0, 0.0, 0.001, 0.0);
//        joint_controller_params["right_s1"] = baxter_robot_controllers::PIDParams(8.0, 0.0, 0.001, 0.0);
//        joint_controller_params["right_e0"] = baxter_robot_controllers::PIDParams(3.0, 0.0, 0.001, 0.0);
//        joint_controller_params["right_e1"] = baxter_robot_controllers::PIDParams(5.0, 0.0, 0.001, 0.0);
//        joint_controller_params["right_w0"] = baxter_robot_controllers::PIDParams(2.0, 0.0, 0.001, 0.0);
//        joint_controller_params["right_w1"] = baxter_robot_controllers::PIDParams(1.0, 0.0, 0.001, 0.0);
//        joint_controller_params["right_w2"] = baxter_robot_controllers::PIDParams(1.0, 0.0, 0.001, 0.0);
        joint_controller_params["right_s0"] = baxter_robot_controllers::PIDParams(2.0, 0.0, 0.001, 0.0);
        joint_controller_params["right_s1"] = baxter_robot_controllers::PIDParams(8.0, 0.0, 0.001, 0.0);
        joint_controller_params["right_e0"] = baxter_robot_controllers::PIDParams(3.0, 0.0, 0.001, 0.0);
        joint_controller_params["right_e1"] = baxter_robot_controllers::PIDParams(5.0, 0.0, 0.001, 0.0);
        joint_controller_params["right_w0"] = baxter_robot_controllers::PIDParams(2.0, 0.0, 0.001, 0.0);
        joint_controller_params["right_w1"] = baxter_robot_controllers::PIDParams(1.0, 0.0, 0.001, 0.0);
        joint_controller_params["right_w2"] = baxter_robot_controllers::PIDParams(1.0, 0.0, 0.001, 0.0);
        return joint_controller_params;
    }

}
#endif // BAXTER_ROBOT_CONFIG_HPP


// Copyright (c) 2012-2018, The CryptoNote developers, The Bytecoin developers, [ ] developers.
// Licensed under the GNU Lesser General Public License. See LICENSE for details.

#pragma once

#include <boost/variant.hpp>
#include <functional>
#include <vector>
#include "common/BinaryArray.hpp"
#include "crypto/types.hpp"

// We define here, as CryptoNoteConfig.h is never included anywhere anymore
#define cryptonote_ALLOW_DEBUG_COMMANDS 1

namespace cryptonote {

using crypto::Hash;
using crypto::PublicKey;
using crypto::SecretKey;
using crypto::KeyPair;
using crypto::KeyDerivation;
using crypto::KeyImage;
using crypto::Signature;

using common::BinaryArray;

using namespace std::placeholders;  // We enjoy standard bindings

typedef uint32_t Height;
typedef uint64_t Difficulty;
typedef uint64_t Amount;
typedef uint32_t Timestamp;
typedef uint64_t UnlockMoment;
// Height or Timestamp, 32-bit is enough, but historically we already have several very large values in blockchain
typedef int64_t SignedAmount;

struct CoinbaseInput {
	Height block_index = 0;
};

struct KeyInput {
	Amount amount = 0;
	std::vector<uint32_t> output_indexes;
	KeyImage key_image;
};

struct KeyOutput {
	PublicKey key;
};

typedef boost::variant<CoinbaseInput, KeyInput> TransactionInput;

typedef boost::variant<KeyOutput> TransactionOutputTarget;

struct TransactionOutput {
	Amount amount = 0;
	TransactionOutputTarget target;
};

struct TransactionPrefix {
	uint8_t version          = 0;
	UnlockMoment unlock_time = 0;
	std::vector<TransactionInput> inputs;
	std::vector<TransactionOutput> outputs;
	BinaryArray extra;
};

struct Transaction : public TransactionPrefix {
	std::vector<std::vector<Signature>> signatures;
};

struct BaseTransaction : public TransactionPrefix {};  // has 'ignored' field during seria

struct ParentBlock {
	uint8_t major_version = 0;
	uint8_t minor_version = 0;
	Hash previous_block_hash;
	uint16_t transaction_count = 0;
	std::vector<Hash> base_transaction_branch;
	BaseTransaction base_transaction;
	std::vector<Hash> blockchain_branch;
};

struct BlockHeader {
	uint8_t major_version = 0;
	uint8_t minor_version = 0;
	uint32_t nonce        = 0;
	Timestamp timestamp   = 0;
	Hash previous_block_hash;
};

struct BlockTemplate : public BlockHeader {
	ParentBlock parent_block;
	Transaction base_transaction;
	std::vector<Hash> transaction_hashes;
};

struct AccountPublicAddress {
	PublicKey spend_public_key;
	PublicKey view_public_key;
};

struct SendProof {  // proofing that some tx actually sent amount to particular address
	Hash transaction_hash;
	AccountPublicAddress address;
	Amount amount = 0;
	std::string message;
	KeyDerivation derivation;
	Signature signature;
	// pair of derivation and signature form a proof of only fact that creator knows transaction private key and
	// he or she wished to include public view key of address into proof. To further check, look up tx_hash in
	// main chain and sum amounts of outputs which have spend keys corresponding to address public spend key
};

struct AccountKeys {
	AccountPublicAddress address;
	SecretKey spend_secret_key;
	SecretKey view_secret_key;
};

struct RawBlock {
	BinaryArray block;  // BlockTemplate
	std::vector<BinaryArray> transactions;
};

class Block {
public:
	BlockTemplate header;
	std::vector<Transaction> transactions;

	bool from_raw_block(const RawBlock &);
	bool to_raw_block(RawBlock &) const;
};

// Predicates for using in maps, sets, etc
inline bool operator==(const AccountPublicAddress &a, const AccountPublicAddress &b) {
	return std::tie(a.view_public_key, a.spend_public_key) == std::tie(b.view_public_key, b.spend_public_key);
}
inline bool operator!=(const AccountPublicAddress &a, const AccountPublicAddress &b) { return !operator==(a, b); }
inline bool operator<(const AccountPublicAddress &a, const AccountPublicAddress &b) {
	return std::tie(a.view_public_key, a.spend_public_key) < std::tie(b.view_public_key, b.spend_public_key);
}

}  // namespace cryptonote

namespace seria {
class ISeria;

void ser(cryptonote::Hash &v, ISeria &s);
void ser(cryptonote::KeyImage &v, ISeria &s);
void ser(cryptonote::PublicKey &v, ISeria &s);
void ser(cryptonote::SecretKey &v, ISeria &s);
void ser(cryptonote::KeyDerivation &v, ISeria &s);
void ser(cryptonote::Signature &v, ISeria &s);

void ser_members(cryptonote::AccountPublicAddress &v, ISeria &s);
void ser_members(cryptonote::SendProof &v, ISeria &s);
void ser_members(cryptonote::TransactionInput &v, ISeria &s);
void ser_members(cryptonote::TransactionOutput &v, ISeria &s);
void ser_members(cryptonote::TransactionOutputTarget &v, ISeria &s);

void ser_members(cryptonote::CoinbaseInput &v, ISeria &s);
void ser_members(cryptonote::KeyInput &v, ISeria &s);

void ser_members(cryptonote::KeyOutput &v, ISeria &s);

void ser_members(cryptonote::TransactionPrefix &v, ISeria &s);
void ser_members(cryptonote::BaseTransaction &v, ISeria &s);
void ser_members(cryptonote::Transaction &v, ISeria &s);

void ser_members(cryptonote::BlockTemplate &v, ISeria &s);
void ser_members(cryptonote::BlockHeader &v, ISeria &s);
void ser_members(cryptonote::ParentBlock &v, ISeria &s);

void ser_members(cryptonote::RawBlock &v, ISeria &s);
void ser_members(cryptonote::Block &v, ISeria &s);
}


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

#include "zdd.hpp"

#include <vector>

#include <boost/assign/std/vector.hpp>
#include <boost/format.hpp>
#include <boost/range/algorithm.hpp>

#include <core/utils/bitset_utils.hpp>
#include <core/utils/range_utils.hpp>
#include <classical/dd/zdd_to_sets.hpp>

using namespace boost::assign;

namespace cirkit
{

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

enum class zdd_operation { diff, _union, intersection, symmetric_difference, join, meet, delta, nonsub, nonsup, minhit };

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

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

zdd_manager::zdd_manager( unsigned nvars, unsigned log_max_objs, bool verbose )
  : dd_manager( nvars, log_max_objs, verbose ) {}

zdd_manager::~zdd_manager() {}

unsigned zdd_manager::zdd_diff( unsigned z1, unsigned z2 )
{
  /* terminating cases */
  if ( z1 == 0u ) { return 0u; }
  if ( z2 == 0u ) { return z1; }
  if ( z1 == z2 ) { return 0u; }

  const auto r = cache.lookup( z1, z2, (unsigned)zdd_operation::diff );
  if ( r >= 0 ) { return r; }

  const auto& node1 = nodes.at( z1 );
  const auto& node2 = nodes.at( z2 );
  unsigned rlow, rhigh, idx;
  if ( node1.var < node2.var )
  {
    rlow = zdd_diff( node1.low, z2 );
    idx = unique_create( node1.var, node1.high, rlow );
  }
  else if ( node1.var > node2.var )
  {
    idx = zdd_diff( z1, node2.low );
  }
  else
  {
    rlow = zdd_diff( node1.low, node2.low );
    rhigh = zdd_diff( node1.high, node2.high );
    idx = unique_create( node1.var, rhigh, rlow );
  }
  return cache.insert( z1, z2, (unsigned)zdd_operation::diff, idx );
}

unsigned zdd_manager::zdd_union( unsigned z1, unsigned z2 )
{
  /* terminating cases */
  if ( z1 == 0u ) { return z2; }
  if ( z2 == 0u ) { return z1; }
  if ( z1 == z2 ) { return z1; }

  /* commutativity */
  if ( z1 > z2 ) { return zdd_union( z2, z1 ); }

  const auto r = cache.lookup( z1, z2, (unsigned)zdd_operation::_union );
  if ( r >= 0 ) { return r; }

  const auto& node1 = nodes.at( z1 );
  const auto& node2 = nodes.at( z2 );
  unsigned rlow, rhigh;
  if ( node1.var < node2.var )
  {
    rlow = zdd_union( node1.low, z2 );
    rhigh = node1.high;
  }
  else if ( node1.var > node2.var )
  {
    rlow = zdd_union( z1, node2.low );
    rhigh = node2.high;
  }
  else
  {
    rlow = zdd_union( node1.low, node2.low );
    rhigh = zdd_union( node1.high, node2.high );
  }
  const auto idx = unique_create( std::min( node1.var, node2.var ), rhigh, rlow );
  return cache.insert( z1, z2, (unsigned)zdd_operation::_union, idx );
}

unsigned zdd_manager::zdd_intersection( unsigned z1, unsigned z2 )
{
  /* terminating cases */
  if ( z1 == 0u ) { return 0u; }
  if ( z2 == 0u ) { return 0u; }
  if ( z1 == z2 ) { return z1; }

  /* commutativity */
  if ( z1 > z2 ) { return zdd_intersection( z2, z1 ); }

  const auto& node1 = nodes.at( z1 );
  const auto& node2 = nodes.at( z2 );
  if ( node1.var < node2.var )
  {
    return zdd_intersection( node1.low, z2 );
  }
  if ( node1.var > node2.var )
  {
    return zdd_intersection( z1, node2.low );
  }

  const auto r = cache.lookup( z1, z2, (unsigned)zdd_operation::intersection );
  if ( r >= 0 ) { return r; }

  auto rlow = zdd_intersection( node1.low, node2.low );
  auto rhigh = zdd_intersection( node1.high, node2.high );

  const auto idx = unique_create( node1.var, rhigh, rlow );
  return cache.insert( z1, z2, (unsigned)zdd_operation::intersection, idx );
}

unsigned zdd_manager::zdd_symmetric_difference( unsigned z1, unsigned z2 )
{
  /* terminating cases */
  if ( z1 == 0u ) { return z2; }
  if ( z2 == 0u ) { return z1; }
  if ( z1 == z2 ) { return 0u; }

  /* commutativity */
  if ( z1 > z2 ) { return zdd_symmetric_difference( z2, z1 ); }

  const auto r = cache.lookup( z1, z2, (unsigned)zdd_operation::symmetric_difference );
  if ( r >= 0 ) { return r; }

  const auto& node1 = nodes.at( z1 );
  const auto& node2 = nodes.at( z2 );
  unsigned rlow, rhigh;
  if ( node1.var < node2.var )
  {
    rlow = zdd_symmetric_difference( node1.low, z2 );
    rhigh = node1.high;
  }
  else if ( node1.var > node2.var )
  {
    rlow = zdd_symmetric_difference( z1, node2.low );
    rhigh = node2.high;
  }
  else
  {
    rlow = zdd_symmetric_difference( node1.low, node2.low );
    rhigh = zdd_symmetric_difference( node1.high, node2.high );
  }
  const auto idx = unique_create( std::min( node1.var, node2.var ), rhigh, rlow );
  return cache.insert( z1, z2, (unsigned)zdd_operation::symmetric_difference, idx );
}

unsigned zdd_manager::zdd_join( unsigned z1, unsigned z2 )
{
  /* swapping */
  const auto& node1 = nodes.at( z1 );
  const auto& node2 = nodes.at( z2 );

  /* commutativity */
  if ( node1.var < node2.var || ( ( node1.var == node2.var ) && ( z1 > z2 ) ) ) { return zdd_join( z2, z1 ); }

  /* terminating cases */
  if ( z1 == 0u ) { return 0u; }
  if ( z1 == 1u ) { return z2; }

  const auto r = cache.lookup( z1, z2, (unsigned)zdd_operation::join );
  if ( r >= 0 ) { return r; }

  unsigned rlow, rhigh;
  if ( node1.var > node2.var )
  {
    rlow = zdd_join( z1, node2.low );
    rhigh = zdd_join( z1, node2.high );
  }
  else
  {
    rlow = zdd_join( node1.low, node2.low );
    auto r1 = zdd_join( node1.low, node2.high );
    auto r2 = zdd_join( node1.high, node2.low );
    auto r3 = zdd_join( node1.high, node2.high );
    rhigh = zdd_union( zdd_union( r1, r2 ), r3 );
  }

  const auto idx = unique_create( node2.var, rhigh, rlow );
  return cache.insert( z1, z2, (unsigned)zdd_operation::join, idx );
}

unsigned zdd_manager::zdd_meet( unsigned z1, unsigned z2 )
{
  /* swapping */
  const auto& node1 = nodes.at( z1 );
  const auto& node2 = nodes.at( z2 );

  /* commutativity */
  if ( node1.var < node2.var || ( ( node1.var == node2.var ) && ( z1 > z2 ) ) ) { return zdd_join( z2, z1 ); }

  /* terminating cases */
  if ( z1 <= 1u ) { return z1; }

  const auto r = cache.lookup( z1, z2, (unsigned)zdd_operation::meet );
  if ( r >= 0 ) { return r; }

  if ( node1.var > node2.var )
  {
    auto idx = zdd_meet( z1, zdd_union( node2.low, node2.high ) );
    return cache.insert( z1, z2, (unsigned)zdd_operation::join, idx );
  }
  else
  {
    auto rhigh = zdd_join( node1.high, node2.high );
    auto r1 = zdd_join( node1.low, node2.high );
    auto r2 = zdd_join( node1.high, node2.low );
    auto r3 = zdd_join( node1.low, node2.low );
    auto rlow = zdd_union( zdd_union( r1, r2 ), r3 );

    const auto idx = unique_create( node2.var, rhigh, rlow );
    return cache.insert( z1, z2, (unsigned)zdd_operation::join, idx );
  }
}

unsigned zdd_manager::zdd_delta( unsigned z1, unsigned z2 )
{
  /* swapping */
  const auto& node1 = nodes.at( z1 );
  const auto& node2 = nodes.at( z2 );

  /* commutativity */
  if ( node1.var < node2.var || ( ( node1.var == node2.var ) && ( z1 > z2 ) ) ) { return zdd_delta( z2, z1 ); }

  /* terminating cases */
  if ( z1 == 0u ) { return 0u; }
  if ( z1 == 1u ) { return z2; }

  const auto r = cache.lookup( z1, z2, (unsigned)zdd_operation::delta );
  if ( r >= 0 ) { return r; }

  unsigned rlow, rhigh;
  if ( node1.var > node2.var )
  {
    rlow = zdd_delta( z1, node2.low );
    rhigh = zdd_delta( z1, node2.high );
  }
  else
  {
    rlow  = zdd_union( zdd_delta( node1.low, node2.low ), zdd_delta( node1.high, node2.high ) );
    rhigh = zdd_union( zdd_delta( node1.low, node2.high ), zdd_delta( node1.high, node2.low ) );
  }

  const auto idx = unique_create( node2.var, rhigh, rlow );
  return cache.insert( z1, z2, (unsigned)zdd_operation::delta, idx );
}

unsigned zdd_manager::zdd_nonsub( unsigned z1, unsigned z2 )
{
  /* terminating cases */
  if ( z1 == 0u ) { return 0u; }
  if ( z1 == 1u ) { return 0u; }
  if ( z2 == 0u ) { return z1; }
  if ( z1 == z2 ) { return 0u; }

  const auto r = cache.lookup( z1, z2, (unsigned)zdd_operation::nonsub );
  if ( r >= 0 ) { return r; }

  const auto& node1 = nodes.at( z1 );
  const auto& node2 = nodes.at( z2 );

  unsigned rlow, rhigh;

  if ( node1.var > node2.var )
  {
    rlow = zdd_nonsub( node1.low, z2 );
    rhigh = node1.high;
  }
  else
  {
    auto r1 = zdd_nonsub( node1.low, node2.low );
    auto r2 = zdd_nonsub( node1.low, node2.high );
    rlow = zdd_intersection( r1, r2 );
    rhigh = zdd_nonsub( node1.high, node2.high );
  }

  const auto idx = unique_create( node1.var, rhigh, rlow );
  return cache.insert( z1, z2, (unsigned)zdd_operation::nonsub, idx );
}

unsigned zdd_manager::zdd_nonsup( unsigned z1, unsigned z2 )
{
  /* terminating cases */
  if ( z1 == 0u ) { return 0u; }
  if ( z2 == 1u ) { return 0u; }
  if ( z2 == 0u ) { return z1; }
  if ( z1 == z2 ) { return 0u; }

  const auto& node1 = nodes.at( z1 );
  const auto& node2 = nodes.at( z2 );

  if ( node1.var > node2.var )
  {
    return zdd_nonsup( z1, node2.low );
  }

  const auto r = cache.lookup( z1, z2, (unsigned)zdd_operation::nonsup );
  if ( r >= 0 ) { return r; }

  unsigned rlow, rhigh;

  if ( node1.var < node2.var )
  {
    rlow = zdd_nonsup( node1.low, z2 );
    rhigh = zdd_nonsup( node1.high, z2 );
  }
  else
  {
    rlow = zdd_nonsup( node1.high, node2.high );
    auto rtmp = zdd_nonsup( node1.high, node2.low );
    rhigh = zdd_intersection( rtmp, rlow );
    rlow = zdd_nonsup( node1.low, node2.low );
  }
  const auto idx = unique_create( node1.var, rhigh, rlow );
  return cache.insert( z1, z2, (unsigned)zdd_operation::nonsup, idx );
}

unsigned zdd_manager::zdd_minhit( unsigned z )
{
  /* terminating cases */
  if ( z == 0u ) { return 1u; }
  if ( z == 1u ) { return 0u; }

  const auto r = cache.lookup( z, z, (unsigned)zdd_operation::minhit );
  if ( r >= 0 ) { return r; }

  const auto& node = nodes.at( z );
  auto rtmp = zdd_union( node.low, node.high );
  auto rlow = zdd_minhit( rtmp );
  rtmp = zdd_minhit( node.low );
  auto rhigh = zdd_nonsup( rtmp, rlow );

  const auto idx = unique_create( node.var, rhigh, rlow );
  return cache.insert( z, z, (unsigned)zdd_operation::minhit, idx );
}

unsigned zdd_manager::unique_create( unsigned var, unsigned high, unsigned low )
{
  if ( verbose )
  {
    std::cout << boost::format( "[i] attempt to create (%d, %d, %d)" ) % var % high % low << std::endl;
  }
  assert( var < nvars );
  assert( var < nodes[high].var );
  assert( var < nodes[low].var );

  if ( high == 0u ) { return low; }

  return unique_lookup( var, high, low );
}

std::ostream& operator<<( std::ostream& os, const zdd_manager& mgr )
{
  for ( auto node : index( mgr.nodes ) )
  {
    os << node.index << ": " << node.value << std::endl;
  }
  return os;
}

zdd& zdd::operator=( const zdd& other )
{
  if ( this == &other ) { return *this; }
  assert( !manager || manager == other.manager );
  manager = other.manager;
  index   = other.index;
  return *this;
}

unsigned zdd::var() const
{
  return manager->get_var( index );
}

zdd zdd::high() const
{
  return zdd( manager, manager->get_high( index ) );
}

zdd zdd::low() const
{
  return zdd( manager, manager->get_low( index ) );
}

zdd zdd::operator-( const zdd& other ) const
{
  assert( manager == other.manager );
  return zdd( manager, manager->zdd_diff( index, other.index ) );
}

zdd zdd::operator||( const zdd& other ) const
{
  assert( manager == other.manager );
  return zdd( manager, manager->zdd_union( index, other.index ) );
}

zdd zdd::operator&&( const zdd& other ) const
{
  assert( manager == other.manager );
  return zdd( manager, manager->zdd_intersection( index, other.index ) );
}

zdd zdd::operator^( const zdd& other ) const
{
  assert( manager == other.manager );
  return zdd( manager, manager->zdd_symmetric_difference( index, other.index ) );
}

zdd zdd::operator+( const zdd& other ) const
{
  assert( manager == other.manager );
  return zdd( manager, manager->zdd_join( index, other.index ) );
}

zdd zdd::operator*( const zdd& other ) const
{
  assert( manager == other.manager );
  return zdd( manager, manager->zdd_meet( index, other.index ) );
}

zdd zdd::delta( const zdd& other ) const
{
  assert( manager == other.manager );
  return zdd( manager, manager->zdd_delta( index, other.index ) );
}

zdd zdd::nonsub( const zdd& other ) const
{
  assert( manager == other.manager );
  return zdd( manager, manager->zdd_nonsub( index, other.index ) );
}

zdd zdd::nonsup( const zdd& other ) const
{
  assert( manager == other.manager );
  return zdd( manager, manager->zdd_nonsup( index, other.index ) );
}

zdd zdd::minhit() const
{
  return zdd( manager, manager->zdd_minhit( index ) );
}

bool zdd::equals( const zdd& other ) const
{
  assert( manager == other.manager );
  return index == other.index;
}

std::ostream& operator <<( std::ostream& os, const cirkit::zdd& z )
{
  auto s = cirkit::zdd_to_sets( z );
  for ( const auto& e : s )
  {
    cirkit::print_as_set( os, e ) << std::endl;
  }
  return os;
}

}

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


/*===================================================================

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center,
Division of Medical and Biological Informatics.
All rights reserved.

This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.

See LICENSE.txt or http://www.mitk.org for details.

===================================================================*/

#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/xml_parser.hpp>
#include <boost/foreach.hpp>

#include <mitkStickModel.h>
#include <mitkTensorModel.h>
#include <mitkAstroStickModel.h>
#include <mitkBallModel.h>
#include <mitkDotModel.h>

template< class ScalarType >
mitk::FiberfoxParameters< ScalarType >::FiberfoxParameters()
    : m_Repetitions(1)
    , m_SignalScale(100)
    , m_tEcho(100)
    , m_tLine(1)
    , m_tInhom(50)
    , m_Bvalue(1000)
    , m_AxonRadius(0)
    , m_Spikes(0)
    , m_SpikeAmplitude(1)
    , m_KspaceLineOffset(0)
    , m_EddyStrength(0)
    , m_Tau(70)
    , m_CroppingFactor(1)
    , m_DoAddGibbsRinging(false)
    , m_DoSimulateRelaxation(true)
    , m_DoDisablePartialVolume(false)
    , m_DoAddMotion(false)
    , m_DoRandomizeMotion(true)
    , m_NoiseModel(NULL)
    , m_FrequencyMap(NULL)
    , m_MaskImage(NULL)
    , m_ResultNode(mitk::DataNode::New())
    , m_ParentNode(NULL)
    , m_SignalModelString("")
    , m_ArtifactModelString("")
    , m_OutputPath("")
    , m_NumBaseline(1)
    , m_NumGradients(6)
{
    m_ImageDirection.SetIdentity();
    m_ImageOrigin.Fill(0.0);
    m_ImageRegion.SetSize(0, 11);
    m_ImageRegion.SetSize(1, 11);
    m_ImageRegion.SetSize(2, 3);
    m_ImageSpacing.Fill(2.0);

    m_Translation.Fill(0.0);
    m_Rotation.Fill(0.0);

    GenerateGradientHalfShell();
}

template< class ScalarType >
mitk::FiberfoxParameters< ScalarType >::~FiberfoxParameters()
{
    //    if (m_NoiseModel!=NULL)
    //        delete m_NoiseModel;
}

template< class ScalarType >
void mitk::FiberfoxParameters< ScalarType >::GenerateGradientHalfShell()
{
    int NPoints = 2*m_NumGradients;
    m_GradientDirections.clear();

    m_NumBaseline = NPoints/20;
    if (m_NumBaseline==0)
        m_NumBaseline=1;

    GradientType g;
    g.Fill(0.0);
    for (unsigned int i=0; i<m_NumBaseline; i++)
        m_GradientDirections.push_back(g);

    if (NPoints==0)
        return;

    vnl_vector<double> theta; theta.set_size(NPoints);
    vnl_vector<double> phi; phi.set_size(NPoints);
    double C = sqrt(4*M_PI);
    phi(0) = 0.0;
    phi(NPoints-1) = 0.0;
    for(int i=0; i<NPoints; i++)
    {
        theta(i) = acos(-1.0+2.0*i/(NPoints-1.0)) - M_PI / 2.0;
        if( i>0 && i<NPoints-1)
        {
            phi(i) = (phi(i-1) + C /
                      sqrt(NPoints*(1-(-1.0+2.0*i/(NPoints-1.0))*(-1.0+2.0*i/(NPoints-1.0)))));
            // % (2*DIST_m_GradientDirections_PI);
        }
    }

    for(int i=0; i<NPoints; i++)
    {
        g[2] = sin(theta(i));
        if (g[2]<0)
            continue;
        g[0] = cos(theta(i)) * cos(phi(i));
        g[1] = cos(theta(i)) * sin(phi(i));
        m_GradientDirections.push_back(g);
    }
}

template< class ScalarType >
std::vector< int > mitk::FiberfoxParameters< ScalarType >::GetBaselineIndices()
{
    std::vector< int > result;
    for( unsigned int i=0; i<this->m_GradientDirections.size(); i++)
        if (m_GradientDirections.at(i).GetNorm()<0.0001)
            result.push_back(i);
    return result;
}

template< class ScalarType >
unsigned int mitk::FiberfoxParameters< ScalarType >::GetFirstBaselineIndex()
{
    for( unsigned int i=0; i<this->m_GradientDirections.size(); i++)
        if (m_GradientDirections.at(i).GetNorm()<0.0001)
            return i;
    return -1;
}

template< class ScalarType >
bool mitk::FiberfoxParameters< ScalarType >::IsBaselineIndex(unsigned int idx)
{
    if (m_GradientDirections.size()>idx && m_GradientDirections.at(idx).GetNorm()<0.0001)
        return true;
    return false;
}

template< class ScalarType >
unsigned int mitk::FiberfoxParameters< ScalarType >::GetNumWeightedVolumes()
{
    return m_NumGradients;
}

template< class ScalarType >
unsigned int mitk::FiberfoxParameters< ScalarType >::GetNumBaselineVolumes()
{
    return m_NumBaseline;
}

template< class ScalarType >
unsigned int mitk::FiberfoxParameters< ScalarType >::GetNumVolumes()
{
    return m_GradientDirections.size();
}

template< class ScalarType >
typename mitk::FiberfoxParameters< ScalarType >::GradientListType mitk::FiberfoxParameters< ScalarType >::GetGradientDirections()
{
    return m_GradientDirections;
}

template< class ScalarType >
typename mitk::FiberfoxParameters< ScalarType >::GradientType mitk::FiberfoxParameters< ScalarType >::GetGradientDirection(unsigned int i)
{
    return m_GradientDirections.at(i);
}

template< class ScalarType >
void mitk::FiberfoxParameters< ScalarType >::SetNumWeightedGradients(int numGradients)
{
    m_NumGradients = numGradients;
    GenerateGradientHalfShell();
}

template< class ScalarType >
void mitk::FiberfoxParameters< ScalarType >::SetGradienDirections(GradientListType gradientList)
{
    m_GradientDirections = gradientList;
    m_NumGradients = 0;
    m_NumBaseline = 0;
    for( unsigned int i=0; i<this->m_GradientDirections.size(); i++)
    {
        if (m_GradientDirections.at(i).GetNorm()>0.0001)
            m_NumGradients++;
        else
            m_NumBaseline++;
    }
}

template< class ScalarType >
void mitk::FiberfoxParameters< ScalarType >::SetGradienDirections(mitk::DiffusionImage<short>::GradientDirectionContainerType::Pointer gradientList)
{
    m_NumGradients = 0;
    m_NumBaseline = 0;
    m_GradientDirections.clear();

    for( unsigned int i=0; i<gradientList->Size(); i++)
    {
        GradientType g;
        g[0] = gradientList->at(i)[0];
        g[1] = gradientList->at(i)[1];
        g[2] = gradientList->at(i)[2];
        m_GradientDirections.push_back(g);

        if (m_GradientDirections.at(i).GetNorm()>0.0001)
            m_NumGradients++;
        else
            m_NumBaseline++;
    }
}

template< class ScalarType >
void mitk::FiberfoxParameters< ScalarType >::LoadParameters(string filename)
{
    boost::property_tree::ptree parameters;
    boost::property_tree::xml_parser::read_xml(filename, parameters);

    m_FiberModelList.clear();
    m_NonFiberModelList.clear();
    if (m_NoiseModel!=NULL)
        delete m_NoiseModel;

    BOOST_FOREACH( boost::property_tree::ptree::value_type const& v1, parameters.get_child("fiberfox") )
    {
        if( v1.first == "image" )
        {
            m_ImageRegion.SetSize(0, v1.second.get<int>("basic.size.x"));
            m_ImageRegion.SetSize(1, v1.second.get<int>("basic.size.y"));
            m_ImageRegion.SetSize(2, v1.second.get<int>("basic.size.z"));
            m_ImageSpacing[0] = v1.second.get<double>("basic.spacing.x");
            m_ImageSpacing[1] = v1.second.get<double>("basic.spacing.y");
            m_ImageSpacing[2] = v1.second.get<double>("basic.spacing.z");
            m_NumGradients = v1.second.get<int>("basic.numgradients");
            GenerateGradientHalfShell();
            m_Bvalue = v1.second.get<int>("basic.bvalue");
            m_Repetitions = v1.second.get<int>("repetitions");
            m_SignalScale = v1.second.get<int>("signalScale");
            m_tEcho = v1.second.get<double>("tEcho");
            m_tLine = v1.second.get<double>("tLine");
            m_tInhom = v1.second.get<double>("tInhom");
            m_AxonRadius = v1.second.get<double>("axonRadius");
            m_DoSimulateRelaxation = v1.second.get<bool>("doSimulateRelaxation");
            m_DoDisablePartialVolume = v1.second.get<bool>("doDisablePartialVolume");
            if (v1.second.get<bool>("artifacts.addnoise"))
            {
                switch (v1.second.get<int>("artifacts.noisedistribution"))
                {
                case 0:
                {
                    m_NoiseModel = new mitk::RicianNoiseModel< ScalarType >();
                    break;
                }
                case 1:
                {
                    m_NoiseModel = new mitk::ChiSquareNoiseModel< ScalarType >();
                    break;
                }
                default:
                {
                    m_NoiseModel = new mitk::RicianNoiseModel< ScalarType >();
                }
                }
                m_NoiseModel->SetNoiseVariance(v1.second.get<double>("artifacts.noisevariance"));
            }

            m_KspaceLineOffset = v1.second.get<double>("artifacts.kspaceLineOffset");
            m_CroppingFactor = (100-v1.second.get<double>("artifacts.aliasingfactor"))/100;
            m_Spikes = v1.second.get<int>("artifacts.spikesnum");
            m_SpikeAmplitude = v1.second.get<double>("artifacts.spikesscale");
            m_EddyStrength = v1.second.get<double>("artifacts.eddyStrength");
            m_DoAddGibbsRinging = v1.second.get<bool>("artifacts.addringing");
            m_DoAddMotion = v1.second.get<bool>("artifacts.doAddMotion");
            m_DoRandomizeMotion = v1.second.get<bool>("artifacts.randomMotion");
            m_Translation[0] = v1.second.get<double>("artifacts.translation0");
            m_Translation[1] = v1.second.get<double>("artifacts.translation1");
            m_Translation[2] = v1.second.get<double>("artifacts.translation2");
            m_Rotation[0] = v1.second.get<double>("artifacts.rotation0");
            m_Rotation[1] = v1.second.get<double>("artifacts.rotation1");
            m_Rotation[2] = v1.second.get<double>("artifacts.rotation2");

            // compartment 1
            switch (v1.second.get<int>("compartment1.index"))
            {
            case 0:
            {
                mitk::StickModel<double>* stickModel = new mitk::StickModel<double>();
                stickModel->SetGradientList(m_GradientDirections);
                stickModel->SetBvalue(m_Bvalue);
                stickModel->SetDiffusivity(v1.second.get<double>("compartment1.stick.d"));
                stickModel->SetT2(v1.second.get<double>("compartment1.stick.t2"));
                m_FiberModelList.push_back(stickModel);
                break;
            }
            case 1:
            {
                mitk::TensorModel<double>* zeppelinModel = new mitk::TensorModel<double>();
                zeppelinModel->SetGradientList(m_GradientDirections);
                zeppelinModel->SetBvalue(m_Bvalue);
                zeppelinModel->SetDiffusivity1(v1.second.get<double>("compartment1.zeppelin.d1"));
                zeppelinModel->SetDiffusivity2(v1.second.get<double>("compartment1.zeppelin.d2"));
                zeppelinModel->SetDiffusivity3(v1.second.get<double>("compartment1.zeppelin.d2"));
                zeppelinModel->SetT2(v1.second.get<double>("compartment1.zeppelin.t2"));
                m_FiberModelList.push_back(zeppelinModel);
                break;
            }
            case 2:
            {
                mitk::TensorModel<double>* tensorModel = new mitk::TensorModel<double>();
                tensorModel->SetGradientList(m_GradientDirections);
                tensorModel->SetBvalue(m_Bvalue);
                tensorModel->SetDiffusivity1(v1.second.get<double>("compartment1.tensor.d1"));
                tensorModel->SetDiffusivity2(v1.second.get<double>("compartment1.tensor.d2"));
                tensorModel->SetDiffusivity3(v1.second.get<double>("compartment1.tensor.d3"));
                tensorModel->SetT2(v1.second.get<double>("compartment1.tensor.t2"));
                m_FiberModelList.push_back(tensorModel);
                break;
            }
            }

            // compartment 2
            switch (v1.second.get<int>("compartment2.index"))
            {
            case 0:
            {
                mitk::StickModel<double>* stickModel = new mitk::StickModel<double>();
                stickModel->SetGradientList(m_GradientDirections);
                stickModel->SetBvalue(m_Bvalue);
                stickModel->SetDiffusivity(v1.second.get<double>("compartment2.stick.d"));
                stickModel->SetT2(v1.second.get<double>("compartment2.stick.t2"));
                m_FiberModelList.push_back(stickModel);
                break;
            }
            case 1:
            {
                mitk::TensorModel<double>* zeppelinModel = new mitk::TensorModel<double>();
                zeppelinModel->SetGradientList(m_GradientDirections);
                zeppelinModel->SetBvalue(m_Bvalue);
                zeppelinModel->SetDiffusivity1(v1.second.get<double>("compartment2.zeppelin.d1"));
                zeppelinModel->SetDiffusivity2(v1.second.get<double>("compartment2.zeppelin.d2"));
                zeppelinModel->SetDiffusivity3(v1.second.get<double>("compartment2.zeppelin.d2"));
                zeppelinModel->SetT2(v1.second.get<double>("compartment2.zeppelin.t2"));
                m_FiberModelList.push_back(zeppelinModel);
                break;
            }
            case 2:
            {
                mitk::TensorModel<double>* tensorModel = new mitk::TensorModel<double>();
                tensorModel->SetGradientList(m_GradientDirections);
                tensorModel->SetBvalue(m_Bvalue);
                tensorModel->SetDiffusivity1(v1.second.get<double>("compartment2.tensor.d1"));
                tensorModel->SetDiffusivity2(v1.second.get<double>("compartment2.tensor.d2"));
                tensorModel->SetDiffusivity3(v1.second.get<double>("compartment2.tensor.d3"));
                tensorModel->SetT2(v1.second.get<double>("compartment2.tensor.t2"));
                m_FiberModelList.push_back(tensorModel);
                break;
            }
            }

            // compartment 3
            switch (v1.second.get<int>("compartment3.index"))
            {
            case 0:
            {
                mitk::BallModel<double>* ballModel = new mitk::BallModel<double>();
                ballModel->SetGradientList(m_GradientDirections);
                ballModel->SetBvalue(m_Bvalue);
                ballModel->SetDiffusivity(v1.second.get<double>("compartment3.ball.d"));
                ballModel->SetT2(v1.second.get<double>("compartment3.ball.t2"));
                m_NonFiberModelList.push_back(ballModel);
                break;
            }
            case 1:
            {
                mitk::AstroStickModel<double>* astrosticksModel = new mitk::AstroStickModel<double>();
                astrosticksModel->SetGradientList(m_GradientDirections);
                astrosticksModel->SetBvalue(m_Bvalue);
                astrosticksModel->SetDiffusivity(v1.second.get<double>("compartment3.astrosticks.d"));
                astrosticksModel->SetT2(v1.second.get<double>("compartment3.astrosticks.t2"));
                astrosticksModel->SetRandomizeSticks(v1.second.get<bool>("compartment3.astrosticks.randomize"));
                m_NonFiberModelList.push_back(astrosticksModel);
                break;
            }
            case 2:
            {
                mitk::DotModel<double>* dotModel = new mitk::DotModel<double>();
                dotModel->SetGradientList(m_GradientDirections);
                dotModel->SetT2(v1.second.get<double>("compartment3.dot.t2"));
                m_NonFiberModelList.push_back(dotModel);
                break;
            }
            }

            // compartment 4
            switch (v1.second.get<int>("compartment4.index"))
            {
            case 0:
            {
                mitk::BallModel<double>* ballModel = new mitk::BallModel<double>();
                ballModel->SetGradientList(m_GradientDirections);
                ballModel->SetBvalue(m_Bvalue);
                ballModel->SetDiffusivity(v1.second.get<double>("compartment4.ball.d"));
                ballModel->SetT2(v1.second.get<double>("compartment4.ball.t2"));
                m_NonFiberModelList.push_back(ballModel);
                break;
            }
            case 1:
            {
                mitk::AstroStickModel<double>* astrosticksModel = new mitk::AstroStickModel<double>();
                astrosticksModel->SetGradientList(m_GradientDirections);
                astrosticksModel->SetBvalue(m_Bvalue);
                astrosticksModel->SetDiffusivity(v1.second.get<double>("compartment4.astrosticks.d"));
                astrosticksModel->SetT2(v1.second.get<double>("compartment4.astrosticks.t2"));
                astrosticksModel->SetRandomizeSticks(v1.second.get<bool>("compartment4.astrosticks.randomize"));
                m_NonFiberModelList.push_back(astrosticksModel);
                break;
            }
            case 2:
            {
                mitk::DotModel<double>* dotModel = new mitk::DotModel<double>();
                dotModel->SetGradientList(m_GradientDirections);
                dotModel->SetT2(v1.second.get<double>("compartment4.dot.t2"));
                m_NonFiberModelList.push_back(dotModel);
                break;
            }
            }
        }
    }
}

template< class ScalarType >
void mitk::FiberfoxParameters< ScalarType >::PrintSelf()
{
    MITK_INFO << "m_ImageRegion: " << m_ImageRegion;
    MITK_INFO << "m_ImageSpacing: " << m_ImageSpacing;
    MITK_INFO << "m_ImageOrigin: " << m_ImageOrigin;
    MITK_INFO << "m_ImageDirection: " << m_ImageDirection;
    MITK_INFO << "m_NumGradients: " << m_NumGradients;
    MITK_INFO << "m_Bvalue: " << m_Bvalue;
    MITK_INFO << "m_Repetitions: " << m_Repetitions;
    MITK_INFO << "m_SignalScale: " << m_SignalScale;
    MITK_INFO << "m_tEcho: " << m_tEcho;
    MITK_INFO << "m_tLine: " << m_tLine;
    MITK_INFO << "m_tInhom: " << m_tInhom;
    MITK_INFO << "m_AxonRadius: " << m_AxonRadius;
    MITK_INFO << "m_KspaceLineOffset: " << m_KspaceLineOffset;
    MITK_INFO << "m_AddGibbsRinging: " << m_DoAddGibbsRinging;
    MITK_INFO << "m_EddyStrength: " << m_EddyStrength;
    MITK_INFO << "m_Spikes: " << m_Spikes;
    MITK_INFO << "m_SpikeAmplitude: " << m_SpikeAmplitude;
    MITK_INFO << "m_CroppingFactor: " << m_CroppingFactor;
    MITK_INFO << "m_DoSimulateRelaxation: " << m_DoSimulateRelaxation;
    MITK_INFO << "m_DoDisablePartialVolume: " << m_DoDisablePartialVolume;
    MITK_INFO << "m_DoAddMotion: " << m_DoAddMotion;
    MITK_INFO << "m_RandomMotion: " << m_DoRandomizeMotion;
    MITK_INFO << "m_Translation: " << m_Translation;
    MITK_INFO << "m_Rotation: " << m_Rotation;
    MITK_INFO << "m_SignalModelString: " << m_SignalModelString;
    MITK_INFO << "m_ArtifactModelString: " << m_ArtifactModelString;
    MITK_INFO << "m_OutputPath: " << m_OutputPath;
}


#include <boost/foreach.hpp>

#include <base/cgal_typedefs.h>
#include <ceres/ceres.h>
#include <ceres/rotation.h>
#include <IO/fileIO.h>
#include <IO/colmapIO.h>

#include <util/helper.h>
#include <util/endian.h>
#include <util/vectorArithmetic.h>
#include <processing/pointSetProcessing.h>
#include <processing/normalAndSensorProcessing.h>

#include <rPLY/ply.h>

//// colmap util
#ifdef COLMAP
#include <util/misc.h>
#include <util/endian.h>
#include <util/ply.h>
#include <mvs/workspace.h>
#include <mvs/fusion.h>
#endif

//#include <Open3D/Open3D.h>
//#include <Open3D/Utility/IJsonConvertible.h>
//#include <CGAL/Bbox_3.h>

/////////// read colmap reconstructions ///////////
//void getSensorMap(const std::string path, std::map<int, Point>& sensorMap){

//    colmap::mvs::StereoFusionOptions options;
//    colmap::mvs::Workspace::Options workspace_options;

//    workspace_options.max_image_size = options.max_image_size;
//    workspace_options.image_as_rgb = true;
//    workspace_options.cache_size = options.cache_size;
//    workspace_options.workspace_path = path;
//    workspace_options.workspace_format = "COLMAP";
//    workspace_options.input_type = "geometric";

//    auto ws = new colmap::mvs::Workspace(workspace_options);

//    const auto& model = ws->GetModel();

//    const auto image_names = colmap::ReadTextFileLines(colmap::JoinPaths(
//        path, workspace_options.stereo_folder, "fusion.cfg"));

//    for (const auto& image_name : image_names) {
//        const int image_idx = model.GetImageIdx(image_name);
//        const auto& image = model.images.at(image_idx);

//        // get the sensor position
//        auto R = Eigen::Map<const Eigen::Matrix<float, 3, 3, Eigen::RowMajor>>(image.GetR());
//        auto T = Eigen::Map<const Eigen::Vector3f>(image.GetT());
//        Point sensor_position = eigen2Cgal(-R.transpose()*T);
////        std::cout << image_idx << " " << image_name << " " << sensor_position << std::endl;
////        std::cout << sensor_position << std::endl;
//        sensorMap[image_idx] = sensor_position;
//    }
//}

//void readColmapRecon(dirHolder& dir, dataHolder& data){
//    auto start = std::chrono::high_resolution_clock::now();
//    cout << "\nRead colmap reconstruction: " << endl;

////    const std::string vis_path = colmap::JoinPaths(dir.path, "fused.ply.vis");
//    string vis_path = dir.path + "fused.ply.vis"
//    getSensorMap(vis_path, data.sensor_map);

//    // apply transformation to register tanks and temples colmap reconstruction with tanks and temples ground truth
//    bool applyTrafo = true;
//    cout << "\t-read transformation matrix " << dir.read_file+"_trans.txt" << endl;
//    Eigen::Matrix4d transformation_matrix = importTranslationMatrix(dir.path+dir.read_file+"_trans.txt");
////    cout << "\t\t" << transformation_matrix << endl;

//    open3d::geometry::PointCloud gt_pc;
//    cout << "\t-read ground truth from " << dir.read_file + ".ply" << endl;
//    open3d::io::ReadPointCloudFromPLY(dir.path+dir.read_file+".ply",gt_pc);
////    const auto bbox = gt_pc.GetOrientedBoundingBox();
//    const auto bbox = gt_pc.GetAxisAlignedBoundingBox().Scale(1.5);

//    // read in the points from the fused and fused.vis files
//    const auto& ply_points = colmap::ReadPly(colmap::JoinPaths(dir.path, "fused.ply"));
//    fstream vis_file(vis_path, std::ios::in | std::ios::binary);
//    CHECK(vis_file.is_open()) << vis_path;
//    const size_t vis_num_points = colmap::ReadBinaryLittleEndian<uint64_t>(&vis_file);
//    CHECK_EQ(vis_num_points, ply_points.size());

//    data.points.reserve(ply_points.size());
//    vector<Eigen::Vector3d> points_eigen;
//    vector<Eigen::Vector3d> sensors_eigen;
////    vector<Eigen::Vector3d> colors_eigen;
//    for (const auto& ply_point : ply_points){

//        points_eigen.push_back(Eigen::Vector3d(ply_point.x, ply_point.y, ply_point.z));
//        Point input_point(ply_point.x, ply_point.y, ply_point.z);

//        vertex_info vinf;
//        vinf.normal = Vector(ply_point.nx, ply_point.ny, ply_point.nz);
//        // color
//        unsigned char r,g,b;
//        r = ply_point.r;
//        g = ply_point.g;
//        b = ply_point.b;
//        std::array<unsigned char, 3> col = {r,g,b};
//        vinf.color = col;

//        // make a list of visible images, sorted by the angle between the sensor_vec and the normal of the point, smallest angles first
//        int num_visible_images = colmap::ReadBinaryLittleEndian<uint32_t>(&vis_file);
//        std::vector<std::pair<double,Point>> sensor_points;
//        for (uint32_t i = 0; i < num_visible_images; ++i) {
//            const int image_idx = colmap::ReadBinaryLittleEndian<uint32_t>(&vis_file);
//            Point sensor_point = data.sensor_map.find(image_idx)->second;
//            Vector v1 = sensor_point - input_point;
//            Vector v2 = vinf.normal;
//            double angle = std::acos( v1 * v2 / std::sqrt(v1.squared_length()*v2.squared_length()));
//            if(CGAL::cross_product(v1,v2).z()<0)
//                angle = 2*M_PI-angle;
//            sensor_points.push_back(std::make_pair(angle, sensor_point));
//        }
//        sort(sensor_points.begin(), sensor_points.end(), sortSensors);

//        for(auto& sensor_position : sensor_points){
//            if(applyTrafo)
//                applyTransformation(sensor_position.second, transformation_matrix);
//            vinf.sensor_positions.push_back(sensor_position.second);
//        }
//        sensors_eigen.push_back(Eigen::Vector3d(vinf.sensor_positions[0].x(), vinf.sensor_positions[0].y(), vinf.sensor_positions[0].z()));
////        colors_eigen.push_back(Eigen::Vector3d(ply_point.x, ply_point.y, ply_point.z));
//        if(applyTrafo)
//            applyTransformation(input_point, transformation_matrix);
//        if(pointInsideBbox(input_point,bbox)){
//            data.points.push_back(input_point);
//            vinf.sensor_vec = sensor_points[0].second-input_point;
//            data.infos.push_back(vinf);
//        }

//    }

//////    // load ground truth, make a bounding box and clip the reconstruction with this bounding box
//////    const string cropfile = dir.path + dir.read_file + "_cropfile.json";
//////    cout << "\t-read cropfile " << dir.read_file + "_cropfile.json" << endl;
//////    open3d::visualization::SelectionPolygonVolume bounding_poly;
//////    open3d::io::ReadIJsonConvertible(cropfile, bounding_poly);
//////    open3d::geometry::PointCloud pc;
//////    pc.normals_ = sensors_eigen;
//////    pc.points_ = points_eigen;

//////    auto pc_transformed = pc.Transform(transformation_matrix);

//////    cout << "size before cropping: " << pc_transformed.points_.size() << endl;
//////    auto pc_cropped = bounding_poly.CropPointCloud(pc_transformed);
//////    cout << "size after cropping: " << pc_cropped->points_.size() << endl;
//////    data.points.clear(); data.infos.clear();
//////    for(int i = 0; i < pc_cropped->points_.size(); i++){
//////        data.points.push_back(Point(pc_cropped->points_[i].x(), pc_cropped->points_[i].y(), pc_cropped->points_[i].z()));
//////        vertex_info vinf;
//////        vinf.sensor_positions.push_back(Point(pc_cropped->normals_[i].x(), pc_cropped->normals_[i].y(), pc_cropped->normals_[i].z()));
//////        data.infos.push_back(vinf);
//////    }

//    auto stop = std::chrono::high_resolution_clock::now();
//    auto duration = std::chrono::duration_cast<std::chrono::seconds>(stop - start);
//    cout << "\t-" << data.points.size() << " points read" << endl;
//    cout << "\t-" << data.sensor_map.size() << " cameras read" << endl;
////    cout << "\t-camera positions of sensor_vec which is closest to normal will be exported to scalar_x0, scalar_y0, scalar_z0" << endl;
//    cout << "\t-in " << duration.count() << "s" << endl;

//}

void readColmapFiles(dirHolder dir, dataHolder& data)
{
    auto start = std::chrono::high_resolution_clock::now();
//    cout << "\nRead colmap points from " << dir.read_file << endl;
    cout << "\nRead colmap points from " << "fused.ply" << endl;

    const auto point_file = dir.path + "fused.ply";
    const auto vis_path = dir.path + "fused.ply.vis";
    const auto cam_file = dir.path + "sparse/images.bin";
    vector<vector<int>> vis_info;

    // read Binary PLY with normal and color
    Mesh_ply aMesh;
    Import_PLY(point_file.c_str(), &aMesh);

    // read the infos
//    loadVisibilityFile(vis_file, vis_info);
    fstream vis_file(vis_path, std::ios::in | std::ios::binary);
    const size_t vis_num_points = colmap::ReadBinaryLittleEndian<uint64_t>(&vis_file);
    CHECK_EQ(vis_num_points, aMesh.mVertices.size());
    cout << "\t-read visibility information" << endl;

    getSensorMap(cam_file, data.sensor_map);
    cout << "\t-read " << data.sensor_map.size() << " images from sparse/images.bin" << endl;

    for(int k=0; k < data.sensor_map.size(); k++){
        auto current = data.sensor_map.find(k+1);
        cout << current->first << " ";
        cout << current->second << endl;
    }

    // parse the read points
    for(int i = 0; i < aMesh.mVertices.size(); i++){
        // save points
        Point pt(aMesh.mVertices[i].x, aMesh.mVertices[i].y, aMesh.mVertices[i].z);
        data.points.push_back(pt);
        // sensor
        vertex_info vinf;
        vinf.normal = Vector(aMesh.mNormals[i].x, aMesh.mNormals[i].y, aMesh.mNormals[i].z);
        // color
        if(aMesh.mvColors.size() > 0){
            unsigned char r,g,b;
            r = aMesh.mvColors[i].x * 255;
            g = aMesh.mvColors[i].y * 255;
            b = aMesh.mvColors[i].z * 255;
            std::array<unsigned char, 3> col = {r,g,b};
            vinf.color = col;
        }

        // index, e.g. used in the concatenate data function
        vinf.global_idx = i;

        // attach info to point
        int num_visible_images = colmap::ReadBinaryLittleEndian<uint32_t>(&vis_file);
        std::vector<std::pair<double,Point>> sensor_points;
        for (uint32_t i = 0; i < num_visible_images; ++i) {
            const int image_idx = colmap::ReadBinaryLittleEndian<uint32_t>(&vis_file);
            // image_idx from the fused.ply.vis and image/camera idx from the sensorMap simply are not the same
                // careful here, the colmap camera index starts at 1 not at 0!!!!
            Point sensor_point = data.sensor_map.find(image_idx)->second;
            Vector v1 = sensor_point - pt;
            Vector v2 = vinf.normal;
            double angle = std::acos( v1 * v2 / std::sqrt(v1.squared_length()*v2.squared_length()));
            if(CGAL::cross_product(v1,v2).z()<0)
                angle = 2*M_PI-angle;
            sensor_points.push_back(std::make_pair(angle, sensor_point));
        }
        sort(sensor_points.begin(), sensor_points.end(), sortSensors);
        vinf.sensor_vec = sensor_points[0].second-pt;
        for(auto& sensor_position : sensor_points){
//            if(applyTrafo)
//                applyTransformation(sensor_position.second, transformation_matrix);
            vinf.sensor_positions.push_back(sensor_position.second);
        }
        data.infos.push_back(vinf);
    }
    auto stop = std::chrono::high_resolution_clock::now();
    auto duration = std::chrono::duration_cast<std::chrono::seconds>(stop - start);
    cout << "\t-parsed " << data.points.size() << " points" << endl;
    cout << "\t-in " << duration.count() << "s" << endl;
}

////////////////////////////////////////////////
//////////////// WU TENGS CODE /////////////////
////////////////////////////////////////////////
//// for colmap fused.ply.vis(binary format)
//bool loadVisibilityFile(const string pszVisFile, vector<vector<int>>& vis_info)
//{
//    FILE * fp = fopen(pszVisFile.c_str(), "rb");
//    if (fp == NULL){
//        printf("Can not open the file %s\n", pszVisFile.c_str());
//        return false;
//    }

//    uint64_t vis_num_points = 0;
//    fread(& vis_num_points, sizeof(uint64_t), 1, fp);

//    if (vis_num_points < 1){
//        printf("no visibilty information\n");
//        fclose(fp);

//        return false;
//    }

//    vis_info.reserve(vis_num_points);
//    //for (int i = 0; i < vis_num_points; ++i){
//    for (int i = 0; i < vis_num_points; i++){
//        int num_visible_images = 0;
//        fread(& num_visible_images, sizeof(uint32_t), 1, fp);

//        std::vector<int> image_view;
//        image_view.reserve(num_visible_images);
////        for (int j = 0; j < num_visible_images; ++j){
//        for (int j = 0; j < num_visible_images; j++){
//            int image_idx = 0;
//            fread(& image_idx, sizeof(uint32_t), 1, fp);
//            image_view.push_back(image_idx);
//        }
//        vis_info.push_back(image_view);
//    }

//    fclose(fp);
//    return true;
//}

// because colmap save qvec and tvec
// we need cvec
//Eigen::Vector4d NormalizeQuaternion(const Eigen::Vector4d& qvec){
//  const double norm = qvec.norm();
//  if (norm == 0) {
//    // We do not just use (1, 0, 0, 0) because that is a constant and when used
//    // for automatic differentiation that would lead to a zero derivative.
//    return Eigen::Vector4d(1.0, qvec(1), qvec(2), qvec(3));
//  } else {
//    return qvec / norm;
//  }
//}
//Eigen::Vector3d ProjectionCenterFromPose(const double* qvec,
//                                         const Eigen::Vector3d& tvec){
//  // Inverse rotation as conjugate quaternion.
////  const Eigen::Vector4d normalized_qvec = NormalizeQuaternion(qvec);
//  Eigen::Matrix<double, 3, 3, Eigen::RowMajor> R;
//  ceres::QuaternionToRotation(qvec, R.data());

////  const Eigen::Quaterniond quat(normalized_qvec(0), -normalized_qvec(1),
////                                -normalized_qvec(2), -normalized_qvec(3));
//  return -R.transpose() * tvec;
//}
// because colmap save qvec and tvec
// we need cvec
Eigen::Vector4d NormalizeQuaternion(const Eigen::Vector4d& qvec) {
  const double norm = qvec.norm();
  if (norm == 0) {
    // We do not just use (1, 0, 0, 0) because that is a constant and when used
    // for automatic differentiation that would lead to a zero derivative.
    return Eigen::Vector4d(1.0, qvec(1), qvec(2), qvec(3));
  } else {
    return qvec / norm;
  }
}

Eigen::Vector3d ProjectionCenterFromPose(const Eigen::Vector4d& qvec,
                                         const Eigen::Vector3d& tvec) {
  // Inverse rotation as conjugate quaternion.
  const Eigen::Vector4d normalized_qvec = NormalizeQuaternion(qvec);
  const Eigen::Quaterniond quat(normalized_qvec(0), -normalized_qvec(1),
                                -normalized_qvec(2), -normalized_qvec(3));
  return quat * -tvec;
}

bool getSensorMap(const string pszImageFile, map<int, Point>& image_proj_center)
{
    FILE* fp = fopen(pszImageFile.c_str(), "rb");
    if (fp == NULL){
        printf("Can not open the file %s\n", pszImageFile.c_str());
        return false;
    }

    uint64_t num_reg_images = 0;

    fread(& num_reg_images, sizeof(uint64_t), 1, fp);

    if (num_reg_images < 1){
        printf("no image information\n");
        fclose(fp);

        return true;
    }

    for (int i = 0; i < num_reg_images; ++i){
        uint32_t num_image_index = 0;
        fread(& num_image_index, sizeof(uint32_t), 1, fp);

        double qvec_array[4] = { 0 };
        fread(qvec_array, sizeof(double), 4, fp);

        double tvec_array[3] = { 0 };
        fread(tvec_array, sizeof(double), 3, fp);

        Eigen::Vector4d qvec;
        for (int j = 0; j < 4; ++j){
            qvec(j) = qvec_array[j];
        }

        Eigen::Vector3d tvec;
        for (int j = 0; j < 3; ++j){
            tvec(j) = tvec_array[j];
        }

        Eigen::Vector3d cvec = ProjectionCenterFromPose(qvec, tvec);
//        Eigen::Vector3d cvec = ProjectionCenterFromPose(qvec_array, tvec);

        Point proj_center(cvec(0), cvec(1), cvec(2));

        uint32_t num_camera_index = 0;
        fread(& num_camera_index, sizeof(uint32_t), 1, fp);

        // read char in binary
        std::string img_name;

        char name_char;
        do {
            fread(& name_char, sizeof(char), 1, fp);
            if (name_char != '\0') {
                img_name += name_char;
            }
        } while (name_char != '\0');

        uint64_t num_points2D = 0;
        fread(& num_points2D, sizeof(uint64_t), 1, fp);

        for (int j = 0; j < num_points2D; ++j){
            double xy[2] = { 0 };
            fread(xy, sizeof(double), 2, fp);

            uint64_t num_point3d_index = 0;
            fread(& num_point3d_index, sizeof(uint64_t), 1, fp);
        }

//        std::cout << num_camera_index << " " << img_name << " " << proj_center << std::endl;
        std::cout << num_image_index << " " << img_name << " " << proj_center << std::endl;

//        image_proj_center[num_camera_index] = proj_center;
        image_proj_center[num_image_index] = proj_center;

//        image_proj_center.insert(map<int, Point>::value_type(num_camera_index, proj_center));
    }
    fclose(fp);

    return true;
}


#include <Eigen/Core>

namespace CortidQCT {

} // namespace CortidQCT


// -*- C++ -*-
//
// Package:    HLTVertexFilter
// Class:      HLTVertexFilter
//
/**\class HLTVertexFilter HLTVertexFilter.cc

 Description: HLTFilter to accept events with at least a given number of vertices

 Implementation:
     This class implements an HLTFilter to select events with at least
     a certain number of vertices matching some selection criteria.
*/
// Original Author:  Andrea Bocci
//         Created:  Tue Apr 20 12:34:27 CEST 2010

#include <cmath>

#include <boost/foreach.hpp>

// user include files
#include "FWCore/Framework/interface/Frameworkfwd.h"
#include "FWCore/Framework/interface/Event.h"
#include "FWCore/ParameterSet/interface/ParameterSet.h"
#include "FWCore/ParameterSet/interface/ConfigurationDescriptions.h"
#include "FWCore/ParameterSet/interface/ParameterSetDescription.h"
#include "DataFormats/Common/interface/Handle.h"
#include "DataFormats/VertexReco/interface/Vertex.h"
#include "DataFormats/VertexReco/interface/VertexFwd.h"
#include "DataFormats/HLTReco/interface/TriggerFilterObjectWithRefs.h"
#include "DataFormats/HLTReco/interface/TriggerTypeDefs.h"
#include "HLTrigger/HLTcore/interface/HLTFilter.h"

//
// class declaration
//
class HLTVertexFilter : public HLTFilter {
public:
  explicit HLTVertexFilter(const edm::ParameterSet & config);
  ~HLTVertexFilter() override;
  static void fillDescriptions(edm::ConfigurationDescriptions & descriptions);

private:

  bool hltFilter(edm::Event & event, const edm::EventSetup & setup, trigger::TriggerFilterObjectWithRefs & filterproduct) const override;

  edm::EDGetTokenT<reco::VertexCollection> m_inputToken;
  edm::InputTag m_inputTag;     // input vertex collection
  double        m_minNDoF;      // minimum vertex NDoF
  double        m_maxChi2;      // maximum vertex chi2
  double        m_maxD0;        // maximum transverse distance from the beam
  double        m_maxZ;         // maximum longitudinal distance nominal center of the detector
  unsigned int  m_minVertices;

};

//
// constructors and destructor
//
HLTVertexFilter::HLTVertexFilter(const edm::ParameterSet& config) : HLTFilter(config),
  m_inputTag(config.getParameter<edm::InputTag>("inputTag")),
  m_minNDoF(config.getParameter<double>("minNDoF")),
  m_maxChi2(config.getParameter<double>("maxChi2")),
  m_maxD0(config.getParameter<double>("maxD0")),
  m_maxZ(config.getParameter<double>("maxZ")),
  m_minVertices(config.getParameter<unsigned int>("minVertices"))
{
  m_inputToken = consumes<reco::VertexCollection>(m_inputTag);
}

HLTVertexFilter::~HLTVertexFilter() = default;

void
HLTVertexFilter::fillDescriptions(edm::ConfigurationDescriptions& descriptions) {
  edm::ParameterSetDescription desc;
  makeHLTFilterDescription(desc);
  desc.add<edm::InputTag>("inputTag",edm::InputTag("hltPixelVertices"));
  desc.add<double>("minNDoF",0.);
  desc.add<double>("maxChi2",99999.);
  desc.add<double>("maxD0",1.);
  desc.add<double>("maxZ",15.);
  desc.add<unsigned int>("minVertices",1);
  descriptions.add("hltVertexFilter",desc);
}

//
// member functions
//

// ------------ method called on each new Event  ------------
bool
HLTVertexFilter::hltFilter(edm::Event &  event, edm::EventSetup const & setup, trigger::TriggerFilterObjectWithRefs & filterproduct) const {

  // get hold of collection of objects
  edm::Handle<reco::VertexCollection> vertices;
  event.getByToken(m_inputToken, vertices);

  unsigned int n = 0;
  if (vertices.isValid()) {
    BOOST_FOREACH(const reco::Vertex & vertex, * vertices) {
      if (vertex.isValid()
          and not vertex.isFake()
          and (vertex.chi2() <= m_maxChi2)
          and (vertex.ndof() >= m_minNDoF)
          and (std::abs(vertex.z()) <= m_maxZ)
	  and (std::abs(vertex.y()) <= m_maxD0)
	  and (std::abs(vertex.x()) <= m_maxD0)
          and (std::sqrt(std::pow(vertex.x(),2)+std::pow(vertex.y(),2)) <= m_maxD0)
          )
        ++n;
    }
  }

  // filter decision
  return (n >= m_minVertices);
}

//define this as a plug-in
#include "FWCore/Framework/interface/MakerMacros.h"
DEFINE_FWK_MODULE(HLTVertexFilter);


/**
 * Copyright Soramitsu Co., Ltd. 2017 All Rights Reserved.
 * http://soramitsu.co.jp
 *
 * 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.
 */
#ifndef IROHA_AMOUNT_H
#define IROHA_AMOUNT_H

#include <boost/multiprecision/cpp_dec_float.hpp>
#include <boost/multiprecision/cpp_int.hpp>
#include <boost/optional.hpp>
#include <ciso646>
#include <cstdint>
#include <string>

namespace iroha {

  /**
   * Keeps integer and scale values allowing performing math
   * operations on them
   */
  class Amount {
   public:
    /**
     * Creates Amount with integer = 0 and scale = 0
     */
    Amount();

    /**
     * Amount with integer = amount and scale = 0
     * @param amount integer part
     */
    Amount(boost::multiprecision::uint256_t amount);

    /**
     * Amount with provided integer and scale part
     * @param amount integer part
     * @param precision scale part
     */
    Amount(boost::multiprecision::uint256_t amount, uint8_t precision);

    Amount(uint64_t first, uint64_t second, uint64_t third, uint64_t fourth);

    Amount(uint64_t first,
           uint64_t second,
           uint64_t third,
           uint64_t fourth,
           uint8_t precision);

    std::vector<uint64_t> to_uint64s();

    /**
     * Copy constructor
     */
    Amount(const Amount &);
    Amount &operator=(const Amount &);

    /**
     * Move constructor
     */
    Amount(Amount &&);
    Amount &operator=(Amount &&);

    boost::multiprecision::uint256_t getIntValue();
    uint8_t getPrecision();

    static boost::optional<Amount> createFromString(std::string str_amount);

    /**
     * Takes percentage from current amount
     * @param percents
     * @return
     */
    Amount percentage(boost::multiprecision::uint256_t percents) const;

    /**
     * Takes percentage represented as amount value
     * The current scale and scale of percents may differ
     * @param percents
     * @return
     */
    Amount percentage(const Amount &percents) const;

    /**
     * Sums up two optionals of the amounts.
     * Requires to have the same scale.
     * Otherwise nullopt is returned
     * @param a left term
     * @param b right term
     * @param optional result
     */
    friend boost::optional<Amount> operator+(boost::optional<Amount> a,
                                             boost::optional<Amount> b) {
      // check precisions
      if (a->precision_ != b->precision_) {
        return boost::none;
      }
      auto res = a->add(*b);
      // check overflow
      if (res.value_ < a->value_ or res.value_ < b->value_) {
        return boost::none;
      }
      return res;
    }

    /**
     * Subtracts right term from the left term
     * Requires to have the same scale.
     * Otherwise nullopt is returned
     * @param a left term
     * @param b right term
     * @param optional result
     */
    friend boost::optional<Amount> operator-(boost::optional<Amount> a,
                                             boost::optional<Amount> b) {
      // check precisions
      if (a->precision_ != b->precision_) {
        return boost::none;
      }
      // check if a greater than b
      if (a->value_ < b->value_) {
        return boost::none;
      }
      return a->subtract(*b);
    }

    /**
     * Comparisons are possible between amounts with different precisions.
     *
     * @return
     */
    bool operator==(const Amount &) const;
    bool operator!=(const Amount &) const;
    bool operator<(const Amount &) const;
    bool operator>(const Amount &) const;
    bool operator<=(const Amount &) const;
    bool operator>=(const Amount &) const;

    std::string to_string() const;
    ~Amount() = default;

   private:
    /**
     * Support function for comparison operators.
     * Returns 0 when equal, -1 when current Amount smaller, and 1 when it is
     * greater
     * @param other
     * @return
     */
    int compareTo(const Amount &other) const;

    /**
     * Sums two amounts.
     * @return
     */
    Amount add(const Amount &) const;
    /**
     * Subtracts one amount from another.
     * Requires to have the same scale between both amounts.
     * Otherwise nullopt is returned
     * @return
     */
    Amount subtract(const Amount &) const;

    boost::multiprecision::uint256_t value_{0};
    uint8_t precision_{0};
  };
}  // namespace iroha
#endif  // IROHA_AMOUNT_H


// __BEGIN_LICENSE__
//  Copyright (c) 2006-2013, United States Government as represented by the
//  Administrator of the National Aeronautics and Space Administration. All
//  rights reserved.
//
//  The NASA Vision Workbench is 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.
// __END_LICENSE__

#include <test/Helpers.h>
#include <vw/Core/CompoundTypes.h>      // for CompoundNumChannels, etc
#include <vw/Core/FundamentalTypes.h>   // for uint8
#include <vw/Math/Vector.h>             // for operator==, Vector2f, etc
#include <vw/Image/ImageMath.h>         // for operator*, operator+, etc
#include <vw/Image/ImageView.h>         // for ImageView
#include <vw/Image/Manipulation.h>      // for copy
#include <vw/Image/PixelMask.h>         // for is_valid, IsMasked, etc
#include <vw/Image/PixelMath.h>         // for operator*, operator+, etc
#include <vw/Image/PixelTypeInfo.h>     // for PixelChannelType
#include <vw/Image/PixelTypes.h>        // for operator<<, etc

#include <boost/utility/enable_if.hpp>  // for disable_if, enable_if

using namespace vw;
using boost::enable_if;
using boost::disable_if;

template <template<class> class TraitT, class T>
static bool bool_trait( T const& /*arg*/ ) {
  return TraitT<T>::value;
}

// Assignment, this in ensures a constant is applied to all elements
template <class T, class V>
void assignment( PixelMask<T> & a, V value ) {
  for ( size_t i = 0; i < CompoundNumChannels<PixelMask<T> >::value - 1; i++ )
    a[i] = value;
}

// Construction, this in ensures a constant is applied to all elements
template <class T, class V>
typename enable_if<IsCompound<T>, T>::type construct( V value ) {
  T a;
  for ( size_t i = 0; i < CompoundNumChannels<T>::value; i++ )
    a[i] = value;
  return a;
}
template <class T, class V>
typename disable_if<IsCompound<T>, T>::type construct( V value ) {
  return T(value);
}

typedef ::testing::Types<PixelGray<uint8>, PixelGray<float>, PixelRGB<uint8>, PixelRGB<float>, uint8, float, Vector2f, Vector2i> MyTypes;

template <class PixelT>
struct MaskedImageMath : public ::testing::Test {
  typedef PixelT Px;
  typedef typename PixelChannelType<Px>::type ChT;
  typedef PixelMask<Px> MPx;
  ImageView<MPx> Av, Ai, Bv, Bi;

  void SetUp() {
    Av.set_size(1,1);
    Ai.set_size(1,1);
    Bv.set_size(1,1);
    Bi.set_size(1,1);
  }
};

TYPED_TEST_CASE( MaskedImageMath, MyTypes );
TYPED_TEST( MaskedImageMath, Sum ) {

  assignment( this->Av(0,0), 2 );
  assignment( this->Ai(0,0), 2 );
  assignment( this->Bv(0,0), 100 );
  assignment( this->Bi(0,0), 100 );
  this->Av(0,0).validate();
  this->Bv(0,0).validate();

  // Test traits
  ASSERT_TRUE(  bool_trait<IsMasked>( this->Av(0,0) ) );
  ASSERT_TRUE(  bool_trait<IsMasked>( this->Ai(0,0) ) );

  // Test Operations
  typedef typename TestFixture::MPx MPx;
  typedef typename TestFixture::Px Px;
  ImageView<MPx> F = this->Av + this->Bv;
  EXPECT_TRUE( is_valid(F(0,0)) );
  EXPECT_VW_EQ( construct<Px>(102), F(0,0).child() );
  F = this->Ai + this->Bv;
  EXPECT_FALSE( is_valid(F(0,0)) );
  EXPECT_VW_EQ( construct<Px>(102), F(0,0).child() );
  F = this->Av + this->Bi;
  EXPECT_FALSE( is_valid(F(0,0)) );
  EXPECT_VW_EQ( construct<Px>(102), F(0,0).child() );
  F = this->Ai + this->Bi;
  EXPECT_FALSE( is_valid(F(0,0)) );
  EXPECT_VW_EQ( construct<Px>(102), F(0,0).child() );
  F = this->Av + 2;
  EXPECT_TRUE( is_valid(F(0,0)) );
  EXPECT_VW_EQ( construct<Px>(4), F(0,0).child() );
  F = this->Ai + 2;
  EXPECT_FALSE( is_valid(F(0,0)) );
  EXPECT_VW_EQ( construct<Px>(4), F(0,0).child() );
  F = 2 + this->Bv;
  EXPECT_TRUE( is_valid(F(0,0)) );
  EXPECT_VW_EQ( construct<Px>(102), F(0,0).child()  );
  F = 2 + this->Bi;
  EXPECT_FALSE( is_valid(F(0,0)) );
  EXPECT_VW_EQ( construct<Px>(102), F(0,0).child() );
  F = copy(this->Av);
  F += this->Bv;
  EXPECT_TRUE( is_valid(F(0,0)) );
  EXPECT_VW_EQ( construct<Px>(102), F(0,0).child() );
  F = copy(this->Av);
  F += this->Bi;
  EXPECT_FALSE( is_valid(F(0,0)) );
  EXPECT_VW_EQ( construct<Px>(102), F(0,0).child() );
  F = copy(this->Ai);
  F += this->Bv;
  EXPECT_FALSE( is_valid(F(0,0)) );
  EXPECT_VW_EQ( construct<Px>(102), F(0,0).child() );
  F = copy(this->Ai);
  F += this->Bi;
  EXPECT_FALSE( is_valid(F(0,0)) );
  EXPECT_VW_EQ( construct<Px>(102), F(0,0).child() );
  F = copy(this->Av);
  F += 2;
  EXPECT_TRUE( is_valid(F(0,0)) );
  EXPECT_VW_EQ( construct<Px>(4), F(0,0).child() );
  F = copy(this->Ai);
  F += 2;
  EXPECT_FALSE( is_valid(F(0,0)) );
  EXPECT_VW_EQ( construct<Px>(4), F(0,0).child() );
}

TYPED_TEST( MaskedImageMath, Difference ) {

  this->Ai.set_size(1,1);
  assignment( this->Ai(0,0), 100 );
  this->Av.set_size(1,1);
  assignment( this->Av(0,0), 100 );
  this->Bi.set_size(1,1);
  assignment( this->Bi(0,0), 50 );
  this->Bv.set_size(1,1);
  assignment( this->Bv(0,0), 50 );
  this->Av(0,0).validate();
  this->Bv(0,0).validate();

  // Test traits
  ASSERT_TRUE(  bool_trait<IsMasked>( this->Av(0,0) ) );
  ASSERT_TRUE(  bool_trait<IsMasked>( this->Ai(0,0) ) );

  // Test Operations
  typedef typename TestFixture::MPx MPx;
  typedef typename TestFixture::Px Px;
  ImageView<MPx> F = this->Av - this->Bv;
  EXPECT_TRUE( is_valid(F(0,0)) );
  EXPECT_VW_EQ( construct<Px>(50), F(0,0).child() );
  F = this->Ai - this->Bv;
  EXPECT_FALSE( is_valid(F(0,0)) );
  EXPECT_VW_EQ( construct<Px>(50), F(0,0).child() );
  F = this->Av - this->Bi;
  EXPECT_FALSE( is_valid(F(0,0)) );
  EXPECT_VW_EQ( construct<Px>(50), F(0,0).child() );
  F = this->Ai - this->Bi;
  EXPECT_FALSE( is_valid(F(0,0)) );
  EXPECT_VW_EQ( construct<Px>(50), F(0,0).child() );
  F = this->Av - 2;
  EXPECT_TRUE( is_valid(F(0,0)) );
  EXPECT_VW_EQ( construct<Px>(98), F(0,0).child() );
  F = this->Ai - 2;
  EXPECT_FALSE( is_valid(F(0,0)) );
  EXPECT_VW_EQ( construct<Px>(98), F(0,0).child() );
  F = 100 - this->Bv;
  EXPECT_TRUE( is_valid(F(0,0)) );
  EXPECT_VW_EQ( construct<Px>(50), F(0,0).child() );
  F = 100 - this->Bi;
  EXPECT_FALSE( is_valid(F(0,0)) );
  EXPECT_VW_EQ( construct<Px>(50), F(0,0).child() );
  F = copy(this->Av);
  F -= this->Bv;
  EXPECT_TRUE( is_valid(F(0,0)) );
  EXPECT_VW_EQ( construct<Px>(50), F(0,0).child() );
  F = copy(this->Av);
  F -= this->Bi;
  EXPECT_FALSE( is_valid(F(0,0)) );
  EXPECT_VW_EQ( construct<Px>(50), F(0,0).child() );
  F = copy(this->Ai);
  F -= this->Bv;
  EXPECT_FALSE( is_valid(F(0,0)) );
  EXPECT_VW_EQ( construct<Px>(50), F(0,0).child() );
  F = copy(this->Ai);
  F -= this->Bi;
  EXPECT_FALSE( is_valid(F(0,0)) );
  EXPECT_VW_EQ( construct<Px>(50), F(0,0).child() );
  F = copy(this->Av);
  F -= 2;
  EXPECT_TRUE( is_valid(F(0,0)) );
  EXPECT_VW_EQ( construct<Px>(98), F(0,0).child() );
  F = copy(this->Ai);
  F -= 2;
  EXPECT_FALSE( is_valid(F(0,0)) );
  EXPECT_VW_EQ( construct<Px>(98), F(0,0).child() );
}

TYPED_TEST( MaskedImageMath, Product ) {
  assignment( this->Av(0,0), 100 );
  assignment( this->Ai(0,0), 100 );
  assignment( this->Bv(0,0), 2 );
  assignment( this->Bi(0,0), 2 );
  this->Av(0,0).validate();
  this->Bv(0,0).validate();

  // Test traits
  ASSERT_TRUE(  bool_trait<IsMasked>( this->Av(0,0) ) );
  ASSERT_TRUE(  bool_trait<IsMasked>( this->Ai(0,0) ) );

  // Test Operations
  typedef typename TestFixture::MPx MPx;
  typedef typename TestFixture::Px Px;
  ImageView<MPx> F = this->Av * this->Bv;
  EXPECT_TRUE( is_valid(F(0,0)) );
  EXPECT_VW_EQ( construct<Px>(200), F(0,0).child() );
  F = this->Ai * this->Bv;
  EXPECT_FALSE( is_valid(F(0,0)) );
  EXPECT_VW_EQ( construct<Px>(200), F(0,0).child() );
  F = this->Av * this->Bi;
  EXPECT_FALSE( is_valid(F(0,0)) );
  EXPECT_VW_EQ( construct<Px>(200), F(0,0).child() );
  F = this->Ai * this->Bi;
  EXPECT_FALSE( is_valid(F(0,0)) );
  EXPECT_VW_EQ( construct<Px>(200), F(0,0).child() );
  F = this->Av * 2;
  EXPECT_TRUE( is_valid(F(0,0)) );
  EXPECT_VW_EQ( construct<Px>(200), F(0,0).child() );
  F = this->Ai * 2;
  EXPECT_FALSE( is_valid(F(0,0)) );
  EXPECT_VW_EQ( construct<Px>(200), F(0,0).child() );
  F = 100 * this->Bv;
  EXPECT_TRUE( is_valid(F) );
  EXPECT_VW_EQ( construct<Px>(200), F(0,0).child() );
  F = 100 * this->Bi;
  EXPECT_FALSE( is_valid(F(0,0)) );
  EXPECT_VW_EQ( construct<Px>(200), F(0,0).child() );
  F = copy(this->Av);
  F *= this->Bv;
  EXPECT_TRUE( is_valid(F(0,0)) );
  EXPECT_VW_EQ( construct<Px>(200), F(0,0).child() );
  F = copy(this->Av);
  F *= this->Bi;
  EXPECT_FALSE( is_valid(F(0,0)) );
  EXPECT_VW_EQ( construct<Px>(200), F(0,0).child() );
  F = copy(this->Ai);
  F *= this->Bv;
  EXPECT_FALSE( is_valid(F(0,0)) );
  EXPECT_VW_EQ( construct<Px>(200), F(0,0).child() );
  F = copy(this->Ai);
  F *= this->Bi;
  EXPECT_FALSE( is_valid(F(0,0)) );
  EXPECT_VW_EQ( construct<Px>(200), F(0,0).child() );
  F = copy(this->Av);
  F *= 2;
  EXPECT_TRUE( is_valid(F(0,0)) );
  EXPECT_VW_EQ( construct<Px>(200), F(0,0).child() );
  F = copy(this->Ai);
  F *= 2;
  EXPECT_FALSE( is_valid(F(0,0)) );
  EXPECT_VW_EQ( construct<Px>(200), F(0,0).child() );
}

TYPED_TEST( MaskedImageMath, Quotient ) {
  assignment( this->Av(0,0), 100 );
  assignment( this->Ai(0,0), 100 );
  assignment( this->Bv(0,0), 2 );
  assignment( this->Bi(0,0), 2 );
  this->Av(0,0).validate();
  this->Bv(0,0).validate();

  // Test Operations
  typedef typename TestFixture::MPx MPx;
  typedef typename TestFixture::Px Px;
  ImageView<MPx> F = this->Av / this->Bv;
  EXPECT_TRUE( is_valid(F(0,0)) );
  EXPECT_VW_EQ( construct<Px>(50), F(0,0).child() );
  F = this->Ai / this->Bv;
  EXPECT_FALSE( is_valid(F(0,0)) );
  EXPECT_VW_EQ( construct<Px>(50), F(0,0).child() );
  F = this->Av / this->Bi;
  EXPECT_FALSE( is_valid(F(0,0)) );
  EXPECT_VW_EQ( construct<Px>(50), F(0,0).child() );
  F = this->Ai / this->Bi;
  EXPECT_FALSE( is_valid(F(0,0)) );
  EXPECT_VW_EQ( construct<Px>(50), F(0,0).child() );
  F = this->Av / 2;
  EXPECT_TRUE( is_valid(F(0,0)) );
  EXPECT_VW_EQ( construct<Px>(50), F(0,0).child() );
  F = this->Ai / 2;
  EXPECT_FALSE( is_valid(F(0,0)) );
  EXPECT_VW_EQ( construct<Px>(50), F(0,0).child() );
  F = 100 / this->Bv;
  EXPECT_TRUE( is_valid(F(0,0)) );
  EXPECT_VW_EQ( construct<Px>(50), F(0,0).child() );
  F = 100 / this->Bi;
  EXPECT_FALSE( is_valid(F(0,0)) );
  EXPECT_VW_EQ( construct<Px>(50), F(0,0).child() );
  F = copy(this->Av);
  F /= this->Bv;
  EXPECT_TRUE( is_valid(F(0,0)) );
  EXPECT_VW_EQ( construct<Px>(50), F(0,0).child() );
  F = copy(this->Av);
  F /= this->Bi;
  EXPECT_FALSE( is_valid(F(0,0)) );
  EXPECT_VW_EQ( construct<Px>(50), F(0,0).child() );
  F = copy(this->Ai);
  F /= this->Bv;
  EXPECT_FALSE( is_valid(F(0,0)) );
  EXPECT_VW_EQ( construct<Px>(50), F(0,0).child() );
  F = copy(this->Ai);
  F /= this->Bi;
  EXPECT_FALSE( is_valid(F(0,0)) );
  EXPECT_VW_EQ( construct<Px>(50), F(0,0).child() );
  F = copy(this->Av);
  F /= 2;
  EXPECT_TRUE( is_valid(F(0,0)) );
  EXPECT_VW_EQ( construct<Px>(50), F(0,0).child() );
  F = copy(this->Ai);
  F /= 2;
  EXPECT_FALSE( is_valid(F(0,0)) );
  EXPECT_VW_EQ( construct<Px>(50), F(0,0).child() );
}


/*****************************************************************************
 * Licensed to Qualys, Inc. (QUALYS) under one or more
 * contributor license agreements.  See the NOTICE file distributed with
 * this work for additional information regarding copyright ownership.
 * QUALYS licenses this file to You 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.
 ****************************************************************************/

/**
 * @file
 * @brief IronBee --- CLIPP Aggregate Modifier Implementation
 *
 * @author Christopher Alfeld <calfeld@qualys.com>
 */

#include "aggregate_modifier.hpp"

#include <clipp/random_support.hpp>

#include <boost/bind.hpp>
#include <boost/function.hpp>
#include <boost/make_shared.hpp>
#include <boost/random.hpp>

#include <ctime>

using boost::bind;
using namespace std;

namespace IronBee {
namespace CLIPP {

namespace  {

struct data_t
{
    list<boost::any> sources;
};

} // Anonymous

struct AggregateModifier::State
{
    //! Distribution of targets.
    distribution_t distribution;

    //! Current target.
    size_t n;
    //! Current aggregate.
    Input::input_p aggregate;
    //! Current data.
    boost::shared_ptr<data_t> data;
};

AggregateModifier::AggregateModifier(size_t n) :
    m_state(new State())
{
    m_state->distribution = bind(constant_distribution, n);
}

bool AggregateModifier::operator()(Input::input_p& input)
{
    if (! input) {
        if (m_state->aggregate) {
            input.swap(m_state->aggregate);
        }
        return true;
    }

    if (! m_state->aggregate) {
        m_state->aggregate = input;
        m_state->n = m_state->distribution();
        m_state->data = boost::make_shared<data_t>();
        m_state->data->sources.push_back(input->source);
        m_state->aggregate->source = m_state->data;
    }
    else {
        copy(
            input->connection.transactions.begin(),
            input->connection.transactions.end(),
            back_inserter(m_state->aggregate->connection.transactions)
        );
        m_state->data->sources.push_back(input->source);
    }

    if (
        m_state->n != 0 &&
        m_state->aggregate->connection.transactions.size() >= m_state->n
    )
    {
        input.swap(m_state->aggregate);
        m_state->aggregate.reset();
        m_state->data.reset();
        return true;
    }

    return false;
}

AggregateModifier AggregateModifier::uniform(
    unsigned int min,
    unsigned int max
)
{
    if (min > max) {
        throw runtime_error("Min must be less than or equal to max.");
    }
    if (min == 0 || max == 0) {
        throw runtime_error("Min and max must be positive.");
    }
    AggregateModifier mod;
    mod.m_state->distribution = make_random_distribution(
        boost::random::uniform_int_distribution<>(min, max)
    );
    return mod;
}

AggregateModifier AggregateModifier::binomial(unsigned int t, double p)
{
    if (t == 0 || p <= 0) {
        throw runtime_error("t and p must be positive.");
    }
    if (p > 1) {
        throw runtime_error("p must be less than or equal to 1.");
    }
    AggregateModifier mod;
    mod.m_state->distribution = make_random_distribution(
        boost::random::binomial_distribution<>(t, p)
    );
    return mod;
}

AggregateModifier AggregateModifier::geometric(double p)
{
    if (p < 0 || p >= 1) {
        throw runtime_error("p must be in [0,1)");
    }
    AggregateModifier mod;
    mod.m_state->distribution = make_random_distribution(
        boost::random::geometric_distribution<>(p)
    );
    return mod;
}

AggregateModifier AggregateModifier::poisson(double mean)
{
    AggregateModifier mod;
    mod.m_state->distribution = make_random_distribution(
        boost::random::poisson_distribution<>(mean)
    );
    return mod;
}

} // CLIPP
} // IronBee


#define BOOST_TEST_MODULE PowerStateGraph PowerStateGraphTest
#define BOOST_TEST_DYN_LINK
#define GOOGLE_STRIP_LOG 4
#include "../src/PowerStateGraph.h"
#include "../src/Statistic.h"
#include "../src/Array.h"
#include <boost/test/unit_test.hpp>
#include <iostream>

BOOST_AUTO_TEST_CASE( constructorTest )
{
    std::cout << "constructorTest..." << std::endl;
    PowerStateGraph psg;
}

BOOST_AUTO_TEST_CASE( updateTest )
{
    std::cout << "updateTest..." << std::endl;

    PowerStateGraph psg;

    //    Signature sig( "data/input/watts_up/kettle.csv", 1, "kettle" );
    //    Signature sig( "data/input/watts_up/toaster.csv", 1, "toaster" );
    //    Signature sig( "data/input/watts_up/tumble.csv", 1, "tumble", 1, 1, 6600 );
        Signature sig( "data/input/watts_up/washer.csv", 1, "washer", 1, 1, 2530 );
        Signature sig2( "data/input/watts_up/washer2.csv", 1, "washer2", 1,1, 2000 );

        psg.update( sig );
        psg.update( sig2 );

        std::cout << psg << std::endl;
        psg.writeGraphViz( std::cout );
}