// Copyright 2019, Collabora, Ltd.
// SPDX-License-Identifier: BSL-1.0
/*!
 * @file
 * @brief  C++ sensor fusion/filtering code that uses flexkalman
 * @author Ryan Pavlik <ryan.pavlik@collabora.com>
 * @ingroup aux_tracking
 */

#pragma once

#ifndef __cplusplus
#error "This header is C++-only."
#endif

#include "tracking/t_lowpass.hpp"
#include "tracking/t_lowpass_vector.hpp"
#include "math/m_api.h"
#include "util/u_time.h"
#include "util/u_debug.h"
#include "util/u_logging.h"

#include <Eigen/Core>
#include <Eigen/Geometry>

#include "flexkalman/EigenQuatExponentialMap.h"

DEBUG_GET_ONCE_LOG_OPTION(simple_imu_log, "SIMPLE_IMU_LOG", U_LOGGING_WARN)

#define SIMPLE_IMU_TRACE(...) U_LOG_IFL_T(ll, __VA_ARGS__)
#define SIMPLE_IMU_DEBUG(...) U_LOG_IFL_D(ll, __VA_ARGS__)
#define SIMPLE_IMU_INFO(...) U_LOG_IFL_I(ll, __VA_ARGS__)
#define SIMPLE_IMU_WARN(...) U_LOG_IFL_W(ll, __VA_ARGS__)
#define SIMPLE_IMU_ERROR(...) U_LOG_IFL_E(ll, __VA_ARGS__)

namespace xrt_fusion {
class SimpleIMUFusion
{
public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
	/*!
	 * @param gravity_rate Value in [0, 1] indicating how much the
	 * accelerometer should affect the orientation each second.
	 */
	explicit SimpleIMUFusion(double gravity_rate = 0.9)
	    : gravity_scale_(gravity_rate), ll(debug_get_log_option_simple_imu_log())
	{
		SIMPLE_IMU_DEBUG("Creating instance");
	}

	/*!
	 * @return true if the filter has started up
	 */
	bool
	valid() const noexcept
	{
		return started_;
	}

	/*!
	 * Get the current state orientation.
	 */
	Eigen::Quaterniond
	getQuat() const
	{
		return quat_;
	}

	/*!
	 * Get the current state orientation as a rotation vector.
	 */
	Eigen::Vector3d
	getRotationVec() const
	{
		return flexkalman::util::quat_ln(quat_);
	}

	/*!
	 * Get the current state orientation as a rotation matrix.
	 */
	Eigen::Matrix3d
	getRotationMatrix() const
	{
		return quat_.toRotationMatrix();
	}

	/*!
	 * @brief Get the predicted orientation at some point in the future.
	 *
	 * Here, we do **not** clamp the delta-t, so only ask for reasonable
	 * points in the future. (The gyro handler math does clamp delta-t for
	 * the purposes of integration in case of long times without signals,
	 * etc, which is OK since the accelerometer serves as a correction.)
	 */
	Eigen::Quaterniond
	getPredictedQuat(timepoint_ns timestamp) const;

	/*!
	 * Get the angular velocity in body space.
	 */
	Eigen::Vector3d const &
	getAngVel() const
	{
		return angVel_;
	}

	/*!
	 * Process a gyroscope report.
	 *
	 * @note At startup, no gyro reports will be considered until at least
	 * one accelerometer report has been processed, to provide us with an
	 * initial estimate of the direction of gravity.
	 *
	 * @param gyro Angular rate in radians per second, in body space.
	 * @param timestamp Nanosecond timestamp of this measurement.
	 *
	 * @return true if the report was used to update the state.
	 */
	bool
	handleGyro(Eigen::Vector3d const &gyro, timepoint_ns timestamp);

	/*!
	 * Process an accelerometer report.
	 *
	 * @param accel Body-relative acceleration measurement in m/s/s.
	 * @param timestamp Nanosecond timestamp of this measurement.
	 *
	 * @return true if the report was used to update the state.
	 */
	bool
	handleAccel(Eigen::Vector3d const &accel, timepoint_ns timestamp);

	/*!
	 * Use this to obtain the residual, world-space acceleration in m/s/s
	 * **not** associated with gravity, after incorporating a measurement.
	 *
	 * @param accel Body-relative acceleration measurement in m/s/s.
	 */
	Eigen::Vector3d
	getCorrectedWorldAccel(Eigen::Vector3d const &accel) const
	{
		Eigen::Vector3d adjusted_accel = accel * getAccelScaleFactor_();
		return (quat_ * adjusted_accel) - (Eigen::Vector3d::UnitY() * MATH_GRAVITY_M_S2);
	}

	/*!
	 * @brief Normalize internal state.
	 *
	 * Call periodically, like after you finish handling both the accel and
	 * gyro from one packet.
	 */
	void
	postCorrect()
	{
		quat_.normalize();
	}

private:
	/*!
	 * Returns a coefficient to correct the scale of the accelerometer
	 * reading.
	 */
	double
	getAccelScaleFactor_() const
	{
		// For a "perfect" accelerometer, gravity_filter_.getState()
		// should return MATH_GRAVITY_M_S2, making this method return 1.
		return MATH_GRAVITY_M_S2 / gravity_filter_.getState();
	}

	//! Body-space angular velocity in radian/s
	Eigen::Vector3d angVel_{Eigen::Vector3d::Zero()};
	//! Current orientation
	Eigen::Quaterniond quat_{Eigen::Quaterniond::Identity()};
	double gravity_scale_;

	/*!
	 * @brief Low-pass filter for extracting the gravity direction from the
	 * full accel signal.
	 *
	 * High-frequency components of the accel are either noise or
	 * user-caused acceleration, and do not reflect the direction of
	 * gravity.
	 */
	LowPassIIRVectorFilter<3, double> accel_filter_{200 /* hz cutoff frequency */};

	/*!
	 * @brief Even-lower low pass filter on the length of the acceleration
	 * vector, used to estimate a corrective scale for the accelerometer
	 * data.
	 *
	 * Over time, the length of the accelerometer data will average out to
	 * be the acceleration due to gravity.
	 */
	LowPassIIRFilter<double> gravity_filter_{1 /* hz cutoff frequency */};
	uint64_t last_accel_timestamp_{0};
	uint64_t last_gyro_timestamp_{0};
	double gyro_min_squared_length_{1.e-8};
	bool started_{false};
	enum u_logging_level ll;
};

inline Eigen::Quaterniond
SimpleIMUFusion::getPredictedQuat(timepoint_ns timestamp) const
{
	timepoint_ns state_time = std::max(last_accel_timestamp_, last_gyro_timestamp_);
	if (state_time == 0) {
		// no data yet.
		return Eigen::Quaterniond::Identity();
	}
	time_duration_ns delta_ns = timestamp - state_time;
	float dt = time_ns_to_s(delta_ns);
	return quat_ * flexkalman::util::quat_exp(angVel_ * dt * 0.5);
}
inline bool
SimpleIMUFusion::handleGyro(Eigen::Vector3d const &gyro, timepoint_ns timestamp)
{
	if (!started_) {

		SIMPLE_IMU_DEBUG(
		    "Discarding gyro report before first usable accel "
		    "report");
		return false;
	}
	time_duration_ns delta_ns = (last_gyro_timestamp_ == 0) ? 1e6 : timestamp - last_gyro_timestamp_;
	if (delta_ns > 1e10) {

		SIMPLE_IMU_DEBUG("Clamping integration period");
		// Limit integration to 1/10th of a second
		// Does not affect updating the last gyro timestamp.
		delta_ns = 1e10;
	}
	float dt = time_ns_to_s(delta_ns);
	last_gyro_timestamp_ = timestamp;
	Eigen::Vector3d incRot = gyro * dt;
	// Crude handling of "approximately zero"
	if (incRot.squaredNorm() < gyro_min_squared_length_) {
		SIMPLE_IMU_TRACE("Discarding gyro data that is approximately zero");
		return false;
	}

	angVel_ = gyro;

	// Update orientation
	quat_ = quat_ * flexkalman::util::quat_exp(incRot * 0.5);

	return true;
}
inline bool
SimpleIMUFusion::handleAccel(Eigen::Vector3d const &accel, timepoint_ns timestamp)
{
	uint64_t delta_ns = (last_accel_timestamp_ == 0) ? 1e6 : timestamp - last_accel_timestamp_;
	float dt = time_ns_to_s(delta_ns);
	if (!started_) {
		auto diff = std::abs(accel.norm() - MATH_GRAVITY_M_S2);
		if (diff > 1.) {
			// We're moving, don't start it now.

			SIMPLE_IMU_DEBUG(
			    "Can't start tracker with this accel "
			    "sample: we're moving too much.");
			return false;
		}

		// Initially, just set it to totally trust gravity.
		started_ = true;
		quat_ = Eigen::Quaterniond::FromTwoVectors(accel.normalized(), Eigen::Vector3d::UnitY());
		accel_filter_.addSample(accel, timestamp);
		gravity_filter_.addSample(accel.norm(), timestamp);
		last_accel_timestamp_ = timestamp;

		SIMPLE_IMU_DEBUG("Got a usable startup accel report");
		return true;
	}
	last_accel_timestamp_ = timestamp;
	accel_filter_.addSample(accel, timestamp);
	gravity_filter_.addSample(accel.norm(), timestamp);

	// Adjust scale of accelerometer
	Eigen::Vector3d adjusted_accel = accel_filter_.getState() * getAccelScaleFactor_();

	// How different is the acceleration length from gravity?
	auto diff = std::abs(adjusted_accel.norm() - MATH_GRAVITY_M_S2);
	auto scale = 1. - diff;
	if (scale <= 0) {
		// Too far from gravity to be useful/trusted for orientation
		// purposes.
		SIMPLE_IMU_TRACE("Too far from gravity to be useful/trusted.");
		return false;
	}

	// This should match the global gravity vector if the rotation
	// is right.
	Eigen::Vector3d measuredGravityDirection = (quat_ * adjusted_accel).normalized();
	auto incremental = Eigen::Quaterniond::FromTwoVectors(measuredGravityDirection, Eigen::Vector3d::UnitY());

	double alpha = scale * gravity_scale_ * dt;
	Eigen::Quaterniond scaledIncrementalQuat = Eigen::Quaterniond::Identity().slerp(alpha, incremental);

	// Update orientation
	quat_ = scaledIncrementalQuat * quat_;

	return true;
}

} // namespace xrt_fusion


//==================================================================================================
/*!
  @file

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_COSD_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_COSD_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-trigonometric
    This function object returns the cosine of the input in degree:
    \f$\cos(\pi x/180)\f$.

    @par Header <boost/simd/function/cosd.hpp>

    @par Note

     - The semantics of the function are similar to @ref cos ones.

    @see sincosd, cos, cospi

    @par Example:

      @snippet cosd.cpp cosd

    @par Possible output:

      @snippet cosd.txt cosd

  **/
  IEEEValue cosd(IEEEValue const& x);
} }
#endif

#include <boost/simd/function/scalar/cosd.hpp>
#include <boost/simd/function/simd/cosd.hpp>

#endif


//Copyright (c) 2014 - 2020, The Trustees of Indiana University.
//
//Licensed under the Apache License, Version 2.0 (the "License");
//you may not use this file except in compliance with the License.
//You may obtain a copy of the License at
//
//    http://www.apache.org/licenses/LICENSE-2.0
//
//Unless required by applicable law or agreed to in writing, software
//distributed under the License is distributed on an "AS IS" BASIS,
//WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//See the License for the specific language governing permissions and
//limitations under the License.

#include <cmath>

#include <boost/math/distributions/poisson.hpp>
#include <boost/math/distributions/normal.hpp>

#include "common/util/logger.hpp"
#include "ms/spec/peak.hpp"
#include "ms/spec/env_peak.hpp"
#include "ms/spec/raw_ms_util.hpp"
#include "ms/feature/peak_cluster.hpp"

namespace toppic {

int PeakCluster::even_charge_idx_ = 0;
int PeakCluster::odd_charge_idx_ = 1;
double PeakCluster::win_size_ = 6.0;

double getBcDistance(std::vector<double> &v1, std::vector<double> &v2) {
  if (v1.size() != v2.size() || v1.size() == 0) {
    LOG_ERROR("Two vectors have different sizes!");
    exit(EXIT_FAILURE);
  }

  double s1 = 0.0;
  double s2 = 0.0;
  for (size_t i = 0; i < v1.size(); i++) {
    s1 += v1[i];
    s2 += v2[i];
  }

  if (!(s1 > 0) || !(s2 > 0)) return 10.0;

  double bc = 0.0;
  for (size_t i = 0; i < v1.size(); i++) {
    double p = v1[i]/ s1;
    double q = v2[i]/ s2;
    bc += std::sqrt(p * q);
  }
  if (bc > std::exp(-10)) {
    return -std::log(bc);
  }
  else {
    return 10;
  }
}

double getPearsonCorr(std::vector<double> &v1, std::vector<double> &v2) {
  if (v1.size() != v2.size() || v1.size() == 0) {
    LOG_ERROR("Two vectors have different sizes or empty vector!");
    exit(EXIT_FAILURE);
  }
  if (v1.size() == 1) {
    return 1.0;
  }
  // Compute means
  double m1 = 0.0;
  double m2 = 0.0;
  for (size_t i = 0; i < v1.size(); i++) {
    m1 += v1[i];
    m2 += v2[i];
  }
  m1 /= v1.size();
  m2 /= v2.size();

  // compute Pearson correlation
  double cov = 0.0;
  double s1 = 0.0;
  double s2 = 0.0;

  for (size_t i = 0; i < v1.size(); i++) {
    double d1 = v1[i] - m1;
    double d2 = v2[i] - m2;
    cov += d1 * d2;
    s1 += d1 * d1;
    s2 += d2 * d2;
  }

  if (s1 <= 0 || s2 <= 0) return 0;

  return cov < 0 ? 0 : cov / std::sqrt(s1 * s2);
}

double getPoissonPValue(PeakPtrVec &win_peaks, double win_size,
                        std::vector<double> &env_peak_intensities) {
  double win_high_inte = raw_ms_util::getHighestPeakInte(win_peaks);
  double inte_thresh = win_high_inte  * 0.1;
  int match_peak_num = 0;
  for (size_t i = 0; i < env_peak_intensities.size(); i++) {
    if (env_peak_intensities[i] > inte_thresh) {
      match_peak_num++;
    }
  }
  int inte_peak_num = 0;
  for (size_t i = 0; i < win_peaks.size(); i++) {
    if (win_peaks[i]->getIntensity() > inte_thresh) {
      inte_peak_num++;
    }
  }
  int possible_peak_num = std::ceil(win_size * 100);

  double n = possible_peak_num;
  double k = env_peak_intensities.size();
  double n1 = inte_peak_num;
  double k1 = match_peak_num;
  double lambda = n1 / n * k;

  boost::math::poisson_distribution<> pd(lambda);

  double pvalue = 1.0 - boost::math::cdf(pd, k1);
  return pvalue;
}

double compRankSumPValue(double n1, double n2, double r1) {
  double u1 = n1 * n2 + n1 * (n1 + 1) * 0.5 - r1;

  double mean_u = 0.5 * (n1 * n2);
  double log_sig_u = 0.5 * (std::log(n1) + std::log(n2) + std::log(n1 + n2 + 1) - std::log(12));
  double sig_u = std::exp(log_sig_u);
  LOG_DEBUG("log sig u " << log_sig_u << " sig u " << sig_u << " n1 " << n1 << " n2 " << n2 << " r1 " << r1);
  // all peaks are matched
  if (sig_u == 0.0) {
    return 0.0;
  }
  boost::math::normal_distribution<> nd(mean_u, sig_u);
  double p_value = boost::math::cdf(nd, u1);
  p_value = std::min(p_value, 1-p_value);
  return std::abs(p_value);
}

double getRankSumPValue(PeakPtrVec win_peaks, std::vector<double> &env_peak_intensities) {
  int rank_sum = 0;
  int match_peak_num = 0;

  std::sort(win_peaks.begin(), win_peaks.end(), Peak::cmpInteDec);

  for (size_t i = 0; i < env_peak_intensities.size(); i++) {
    double peak_inte = env_peak_intensities[i];
    if (peak_inte > 0.0) {
      int rank = win_peaks.size();
      for (size_t j = 0; j < win_peaks.size(); j++) {
        if (peak_inte >= win_peaks[j]->getIntensity()) {
          rank = j+1;
          break;
        }
      }
      rank_sum += rank;
      match_peak_num++;
    }
  }
  int peak_num = win_peaks.size();
  double pvalue = compRankSumPValue(peak_num, match_peak_num, rank_sum);
  return pvalue;
}

PeakCluster::PeakCluster(EnvelopePtr theo_env) {
  theo_env_ = theo_env;
  rep_mass_ = theo_env_->getMonoNeutralMass();
  rep_charge_ = theo_env_->getCharge();

  int peak_num = theo_env_->getPeakNum();
  rep_summed_intensities_.resize(peak_num, 0.0);

  clearScores();

  flag_ = 0;
  init_score_ = false;
  smoother_ = std::make_shared<SavitzkyGolay>(9, 2);
}

void PeakCluster::addEnvelopes(FracFeaturePtr feature_ptr,
                               RealEnvPtrVec envs) {

  int row_num = feature_ptr->getMaxCharge() - feature_ptr->getMinCharge() + 1;
  int col_num = feature_ptr->getMaxMs1Id() - feature_ptr->getMinMs1Id() + 1;

  min_charge_ = feature_ptr->getMinCharge();
  max_charge_ = feature_ptr->getMaxCharge();

  min_ms1_id_ = feature_ptr->getMinMs1Id();
  max_ms1_id_ = feature_ptr->getMaxMs1Id();

  scan_begin_ = feature_ptr->getScanBegin();
  scan_end_ = feature_ptr->getScanEnd();
  LOG_DEBUG("add env row " << row_num << " col " << col_num);

  real_envs_.resize(row_num);

  for (int i = 0; i < row_num; i++) {
    real_envs_[i].resize(col_num);
  }

  for (size_t i = 0; i < envs.size(); i++) {
    int row = envs[i]->getCharge() - min_charge_;
    int col = envs[i]->getSpId() - min_ms1_id_;
    if (row >= 0 && row < row_num && col >= 0 && col < col_num) {
      real_envs_[row][col] = envs[i];
    }
  }
}

void PeakCluster::clearScores() {
  inte_distr_.resize(2, 0.0);
  best_corr_scores_.resize(2, 0.0);
  best_inte_scores_.resize(2, 0.0);
  best_dist_scores_.resize(2, 1.0);

  best_charges_.resize(2, 0);
  sum_dist_scores_.resize(2, 1.0);
  sum_corr_scores_.resize(2, 0.0);
  sum_inte_scores_.resize(2, 0.0);
  xic_corr_between_best_charges_.resize(2, 0.0);
}

void PeakCluster::updateScore(PeakPtrVec2D &raw_peaks, bool check_pvalue) {
  int row_num = max_charge_ - min_charge_ + 1;
  int col_num = max_ms1_id_ - min_ms1_id_ + 1;
  int ref_idx = theo_env_->getReferIdx();

  clearScores();

  std::vector<double> best_charge_dists{10.0, 10.0};

  // sum up peak intensities
  int peak_num = theo_env_->getPeakNum();
  std::vector<double> theo_intensities = theo_env_->getIntensities();
  std::vector<double> summed_intensities(peak_num, 0);

  int xic_len = col_num + 18;
  int xic_start_idx = 9;

  std::vector<std::vector<double>> xic2(2);
  xic2[0].resize(xic_len, 0.0);
  xic2[1].resize(xic_len, 0.0);

  std::vector<std::vector<double>> charge_xic(row_num);

  double tmp_best_bc_dist = 10.0;
  double rep_env_bc_dist = 10.0;
  RealEnvPtr rep_env(nullptr);

  double rep_env_bc_dist_2 = 10.0;
  RealEnvPtr rep_env_2(nullptr);

  std::vector<double> tmp_best_dist_scores{10.0, 10.0};
  std::vector<double> tmp_best_inte_scores(2, 0.0);
  std::vector<double> tmp_best_corr_scores(2, 0.0);

  for (int i = 0; i < row_num; i++) {
    int charge = i + min_charge_;
    double ref_neutral_mass = theo_env_->getRefNeutralMass();
    double ref_mz = Peak::compMz(ref_neutral_mass, charge);
    std::fill(summed_intensities.begin(), summed_intensities.end(), 0.0);

    charge_xic[i].resize(xic_len, 0.0);

    int charge_idx = (charge % 2 == 0) ? even_charge_idx_:odd_charge_idx_;
    // summed_most_abu_isotope_intensity
    double summed_iso_high_inte = 0.0;
    //summed_referenc_intensity
    double summed_win_high_inte = 0.0;

    for (int j = 0; j < col_num; j++) {
      RealEnvPtr env = real_envs_[i][j];
      if (env == nullptr) continue;

      // sum peak inte
      for (int k = 0; k < peak_num; k++) {
        summed_intensities[k] += env->getIntensity(k);
      }

      int ms1_id = min_ms1_id_ + j;
      PeakPtrVec all_peaks = raw_peaks[ms1_id];
      PeakPtrVec win_peaks = raw_ms_util::getPeaksInWindow(all_peaks, ref_mz, win_size_);
      double win_high_inte = raw_ms_util::getHighestPeakInte(win_peaks);
      double win_median_inte = raw_ms_util::getMedianPeakInte(win_peaks);

      if (env->isExist(ref_idx)) {
        summed_iso_high_inte += env->getIntensity(ref_idx);
        summed_win_high_inte += win_high_inte;
      }
      double env_inte_sum = env->getIntensitySum();
      inte_distr_[charge_idx] += env_inte_sum;

      std::vector<double> real_intensities = env->getIntensities();

      double new_bc_dist = getBcDistance(theo_intensities, real_intensities);
      double new_corr = getPearsonCorr(theo_intensities, real_intensities);

      bool good_env = (new_bc_dist < 0.07 || new_corr > 0.7);
      if (good_env) {
        xic2[charge_idx][xic_start_idx + j] += env_inte_sum;
        charge_xic[i][xic_start_idx+j] = env_inte_sum;
      }

      bool level_one_env = true;
      bool level_two_env = true;
      if (check_pvalue) {
        double poisson_pvalue = getPoissonPValue(win_peaks, win_size_, real_intensities);
        double rank_sum_pvalue = getRankSumPValue(win_peaks, real_intensities);
        level_one_env = (rank_sum_pvalue < 0.01 && poisson_pvalue < 0.01);
        //levelTwoEnvelope = (rankSumPValue < 0.05 || poissonPValue < 0.05);
      }
      if (level_one_env ) {
        if (new_bc_dist < best_dist_scores_[charge_idx]) {
          best_dist_scores_[charge_idx] = new_bc_dist;
          double new_inte_score = 1.0;
          if (win_median_inte > 0.0) {
            new_inte_score = env->getReferIntensity()/win_high_inte;
          }
          best_inte_scores_[charge_idx] = std::max(best_inte_scores_[charge_idx], new_inte_score);
        }
        best_corr_scores_[charge_idx] = std::max(best_corr_scores_[charge_idx], new_corr);

        if (new_bc_dist < rep_env_bc_dist) {
          rep_env_bc_dist = new_bc_dist;
          rep_env = env;
        }
      }
      if (level_two_env) {
        if (new_bc_dist < tmp_best_dist_scores[charge_idx]) {
          tmp_best_dist_scores[charge_idx] = new_bc_dist;
          double new_inte_score = 1.0;
          if (win_median_inte > 0.0) {
            new_inte_score = env->getReferIntensity()/win_high_inte;
          }
          tmp_best_inte_scores[charge_idx] = std::max(tmp_best_inte_scores[charge_idx], new_inte_score);
        }
        tmp_best_corr_scores[charge_idx] = std::max(tmp_best_corr_scores[charge_idx], new_corr);

        if (new_bc_dist < rep_env_bc_dist_2) {
          rep_env_bc_dist_2 = new_bc_dist;
          rep_env_2 = env;
        }
      }

      double bc_dist = getBcDistance(theo_intensities, summed_intensities);
      sum_dist_scores_[charge_idx] = std::min(sum_dist_scores_[charge_idx], bc_dist);
      double pc = getPearsonCorr(theo_intensities, summed_intensities);
      sum_corr_scores_[charge_idx] = std::max(sum_corr_scores_[charge_idx], pc);

      if (best_charges_[charge_idx] < 1 || bc_dist < best_charge_dists[charge_idx]) {
        best_charges_[charge_idx] = charge;
        best_charge_dists[charge_idx] = bc_dist;
        if (summed_win_high_inte > 0.0) {
          sum_inte_scores_[charge_idx] = summed_iso_high_inte/summed_win_high_inte;
        }
      }

      if (bc_dist < tmp_best_bc_dist) {
        tmp_best_bc_dist = bc_dist;
        rep_summed_intensities_ = summed_intensities;
      }
    }
  }

  // when good envelope is observed at only even charge...
  if (best_corr_scores_[0] > 0.7 && best_corr_scores_[1] < 0.5) {
    int idx = 1;
    best_corr_scores_[idx] = tmp_best_corr_scores[idx];
    best_inte_scores_[idx] = tmp_best_inte_scores[idx];
    best_dist_scores_[idx] = tmp_best_dist_scores[idx];
  }

  // when good envelope is observed at only odd charge...
  if (best_corr_scores_[1] > 0.7 && best_corr_scores_[0] < 0.5) {
    int idx = 0;
    best_corr_scores_[idx] = tmp_best_corr_scores[idx];
    best_inte_scores_[idx] = tmp_best_inte_scores[idx];
    best_dist_scores_[idx] = tmp_best_dist_scores[idx];
  }

  // normalize intensities
  double s = inte_distr_[0] + inte_distr_[1];
  if (s > 0) {
    inte_distr_[0] = inte_distr_[0] / s;
    inte_distr_[1] = inte_distr_[1] / s;
  }

  if (col_num > 1) {
    int even_best_charge = best_charges_[even_charge_idx_] - min_charge_;
    int odd_best_charge = best_charges_[odd_charge_idx_] - min_charge_;
    if (even_best_charge >= 0 && odd_best_charge >= 0) {
      std::vector<double> v1 = smoother_->smooth(charge_xic[even_best_charge]);
      std::vector<double> v2 = smoother_->smooth(charge_xic[odd_best_charge]);
      xic_corr_between_best_charges_[0] = getPearsonCorr(v1, v2);
      v1 = smoother_->smooth(xic2[even_charge_idx_]);
      v2 = smoother_->smooth(xic2[odd_charge_idx_]);
      xic_corr_between_best_charges_[1] = getPearsonCorr(v1, v2);
    }
  }

  if (rep_env == nullptr && rep_env_2 != nullptr) {
    rep_env = rep_env_2;
  }
  if (rep_env != nullptr) {
    rep_charge_ = rep_env->getCharge();
    rep_mz_ = rep_env->getMonoMz();
    rep_ms1_id_ = rep_env->getSpId();
  }

  init_score_ = true;
}

}


//==================================================================================================
/*!
  @file

  @copyright 2015 NumScale SAS
  @copyright 2015 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_DIVFLOOR_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_DIVFLOOR_HPP_INCLUDED

#include <boost/simd/constant/valmax.hpp>
#include <boost/simd/constant/valmin.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/function/scalar/divs.hpp>
#include <boost/simd/function/scalar/floor.hpp>
#include <boost/simd/function/scalar/iceil.hpp>
#include <boost/simd/function/scalar/minusone.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  BOOST_DISPATCH_OVERLOAD ( divfloor_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::int64_<A0> >
                          , bd::scalar_< bd::int64_<A0> >
                                    )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
      if (!a0) return  Zero<A0>();
      if(a1)
      {
        A0 q = divs(a0, a1);
        A0 r = a0-q*a1;
        if ((r != Zero<A0>())&&((a0^a1) <= 0)) return minusone(q);
        return q;
      }
      else
        return ((a0>0) ? Valmax<A0>() : Valmin<A0>());
    }
  };

  BOOST_DISPATCH_OVERLOAD ( divfloor_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::signed_<A0> >
                          , bd::scalar_< bd::signed_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
      if(a1)
        return static_cast<A0 >(-iceil(-static_cast<double>(a0)/static_cast<double>(a1)));
      else
      {
        return (a0) ? ((a0>0) ? Valmax<A0>() : Valmin<A0>()) : Zero<A0>();
      }
    }
  };

  BOOST_DISPATCH_OVERLOAD ( divfloor_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::unsigned_<A0> >
                          , bd::scalar_< bd::unsigned_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
      if(a1)
        return divs(a0, a1);
      else
        return (a0) ? Valmax<A0>() : Zero<A0>();
    }
  };

#ifdef BOOST_MSVC
#pragma warning(push)
#pragma warning(disable: 4723) // potential divide by 0
#endif

  BOOST_DISPATCH_OVERLOAD ( divfloor_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
      return boost::simd::floor(a0/a1);
    }
  };
} } }

#endif


#include <boost/config.hpp>
#include <boost/version.hpp>

#if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
#include <iostream>
#include <fstream>

#include <CGAL/IO/WKT.h>
#include <CGAL/Simple_cartesian.h>
#include <CGAL/Exact_predicates_exact_constructions_kernel.h>
#include <vector>

//typedef CGAL::Simple_cartesian<CGAL::Gmpq> Kernel;

typedef CGAL::Exact_predicates_exact_constructions_kernel Kernel;

int main(int argc, char* argv[])
{
  typedef CGAL::Point_2<Kernel> Point;
  typedef std::vector<Point> LineString;
  typedef std::vector<LineString> MultiLineString;
  LineString ls;
  {
    std::ifstream is((argc>1)?argv[1]:"data/linestring.wkt");
    CGAL::read_linestring_WKT(is, ls);
    is.close();
  }
  for(Point p : ls)
      std::cout<<p<<std::endl;
  ls.clear();
  MultiLineString mls;
  {
    std::ifstream is((argc>2)?argv[2]:"data/multilinestring.wkt");
    CGAL::read_multi_linestring_WKT(is, mls);
    is.close();
  }
  for(LineString l : mls)
  {
    for(const Point& p : l)
        std::cout<<p<<std::endl;
  }
  return 0;
}
#else
int main()
{
  return 0;
}
#endif


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>

using namespace std;

void test(int n, int m, int order)
{
    assert(n%3 == 0);
    mtl::dense2D<double>           block(3, m);
    mtl::dense_vector<std::size_t> rows(3), columns(m);

    for (int i= 0; i < 3; i++) rows[i]= i;

    switch(order) {
    case 0: for (int i= 0; i < m; i++) columns[i]= i; break;
    case 1: for (int i= 0; i < m; i++) columns[i]= m-i-1; break;
    case 2: for (int i= 0; i < m; i++) columns[i]= (m/2+i)%m; break;
    }
    columns[3]= columns[1]; columns[2]= columns[0];

    for (int i= 0; i < m; i++) {
	block[0][i]= 10+i;
	block[1][i]= 20+i;
	block[2][i]= 30+i;
    }

    mtl::compressed2D<double> A(n, n);
    {
        mtl::mat::inserter<mtl::compressed2D<double>, mtl::update_plus<double> > ins(A, 5);
	for (unsigned r= 0; r < num_rows(A); r+= 3) {
	    ins << element_matrix(block, rows, columns);
	    ins << element_matrix(block, rows, columns);
	    for (int i= 0; i < 3; i++) rows[i]+= 3;
	    for (int i= 0; i < m; i++) ++columns[i];
	}
    }
    if (n < 11) cout << "A is \n" << with_format(A, 4, 3);
    switch(order) {
    case 0:
	MTL_THROW_IF(A[1][2] != 0, mtl::runtime_error("Wrong value"));
	MTL_THROW_IF(A[4][2] != 88, mtl::runtime_error("Wrong value"));break;
    case 1:
	MTL_THROW_IF(A[1][2] != 0, mtl::runtime_error("Wrong value"));
	MTL_THROW_IF(A[4][2] != 0, mtl::runtime_error("Wrong value"));	break;
    case 2:
	MTL_THROW_IF(A[1][2] != 84, mtl::runtime_error("Wrong value"));
	MTL_THROW_IF(A[4][2] != 48, mtl::runtime_error("Wrong value"));	break;
    }

    double array[4]= {1.0, -.4, -0.5, 2.0};
    int v0[]= {6, 7}, v1[]= {7, 8};
    {
        mtl::mat::inserter<mtl::compressed2D<double>, mtl::update_plus<double> > inserter(A, 5);
	inserter << mtl::element_array(mtl::dense2D<double>(mtl::size(v0), mtl::size(v1), array), v0, v1);
    }
    if (n < 11) cout << "A is \n" << with_format(A, 4, 3);
    MTL_THROW_IF(A[7][7] != -0.5, mtl::runtime_error("Wrong value"));

}

int main(int, char**)
{
    test(9, 5, 0);
    test(9, 5, 1);
    test(9, 5, 2);

    return 0;
}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_NEGATE_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_NEGATE_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-ieee
    Function object implementing negate capabilities

    Retuns the first element multiplied by the @ref sign of the second.

    @par Semantic:

    @code
    T r = negate(x,y);
    @endcode

    is similar to:

    @code
    T r =  x*sign(y);
    @endcode

    @par Note
    If y is @ref Zero the result is zero. If you want to avoid that use
    @ref negatenz or @ref copysign.

    @see sign, negatenz, copysign,  Mzero,  is_positive, is_negative

  **/
  const boost::dispatch::functor<tag::negate_> negate = {};
} }
#endif

#include <boost/simd/function/scalar/negate.hpp>
#include <boost/simd/function/simd/negate.hpp>

#endif


//=======================================================================
// Copyright 2015 Tsinghua University.
// Authors: Fuan Pu (Pu.Fuan@gmail.com)
//
// Dung's abstract argumentation framework
//=======================================================================

#ifndef DUNG_REASONER_HPP
#define DUNG_REASONER_HPP

#include <iostream>
#include <sstream>
#include <string>
#include <utility>
#include <algorithm>
#include <iosfwd>

//boost
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>

#include "config/config.hpp"
#include "ArgumentProperty.hpp"
#include "AttackProperty.hpp"
#include "bitmatrix/bitvector.hpp"
#include "bitmatrix/bitmatrix.hpp"
#include "DungAF.hpp"

namespace argumatrix{

using namespace std;

class Reasoner {
public:
	Reasoner(const DungAF& daf, streambuf* osbuff = std::cout.rdbuf());
	virtual ~Reasoner() {}
	/**
	 * @brief Get the attacked arguments by arguments in _bv, \f$R^+(S)\f$
	 * \f$R^+(S) = {x|x is attacked by S}\f$.
	 * \f$R^+(S_bv) = D*S_bv\f$
	 * @param _bv the bitvector with respect to the set \f$S\f$.
	 * @return a set of arguments in bitvector form.
	 */
	bitvector getAttacked(const bitvector& _bv);
	//bitvector R_plus(const bitvector& _bv);

	/**
	 * @brief The characteristic function of an abstract argumentation framework:
	 * \f$F_{AF}(S) = {A|A is acceptable wrt. S}\f$.
	 * F_AF(S_bv) = not{ R^+[ not( R^+(S_bv) ) ] }
	 * @param extension an extension (a set of arguments), the default is empty set.
	 * @return a set of arguments in bitvector form.
	 */
	bitvector characteristic(const bitvector& _bv);

	/**
	 * @brief The characteristic function of an abstract argumentation framework with
	 * the initialization of empty set
	 * @return a set of arguments in bitvector form.
	 */
	bitvector characteristic();

	/**
	 * @brief The neutrality function of an abstract argumentation framework:
	 * N_AF(S) = {A| All arguments that not attacked by S}.
	 * N_AF(S_bv) = not( R^+(S_bv) )
	 * @param extension an extension (a set of arguments).
	 * @return a set of arguments in bitvector form.
	 */
	bitvector neutrality(const bitvector& _bv);

	/**
	 * @brief Argument set S is conflict-free?
	 * S is said to be conflict-free iff for any two arguments a,b in S such
	 * that a does not attack b
	 * @param extension an extension (a set of arguments).
	 * @return true if S is conflict-free.
	 */
	bool is_conflict_free(const bitvector& _bv);
	bool is_conflict_free(const set<string>& argset);

	/**
	 * @brief Argument set A is acceptable w.r.t S? Alternately, S defends A?
	 * A can be an argument or an argument set.
	 * S defends argument (set) A iff for any argument x in X, if attacks A
	 * (or a in A) then there is an argument y in S such that y attacks x.
	 * @param S: a set of arguments.
	 * @param A: an argument or an argument set
	 * @return true if S defends A.
	 */
	bool is_acceptable(const bitvector& S, const bitvector& A);

	/**
	 * @brief Argument a is self-attacking iff it attacks itself.
	 * @param idx: the index of the argument w.r.t the attack matrix
	 * @return true if S defends A.
	 */
	bool is_self_attacking(size_type idx);

	/**
	 * @brief To decide whether a set of arguments (with the form of bitvector) is
	 * an admissible extension.
	 * $S$ is an admissible extension iff $S \subseteq F(S) \cap N(S)$.
	 * @return true if _bv is admissible; otherwise returns false.
	 */
	bool is_admissible(const bitvector& _bv);

	/**
	 * @brief To decide whether a set of arguments (with the form of bitvector) is
	 * a complete extension.
	 * $S$ is a complete extension iff $S == F(S) \cap N(S)$.
	 * @return true if _bv is complete; otherwise returns false.
	 */
	bool is_complete(const bitvector& _bv);

	/**
	 * @brief To decide whether a set of arguments (with the form of bitvector) is
	 * a stable extension.
	 * $S$ is a stable extension iff $S == N(S)$.
	 * @return true if _bv is stable; otherwise returns false.
	 */
	bool is_stable(const bitvector& _bv);

	/**
	 * @brief To decide whether a set of arguments (with the form of bitvector) is
	 * a grounded extension. $S$ is a grounded extension iff it is the least
	 * fixed point of the characteristic function F.
	 * @return true if _bv is grounded; otherwise returns false.
	 */
	bool is_grounded(const bitvector& _bv);

	/**
	 * @brief Get all arguments which are self-attacking. Obviously, if an argument
	 * i attacks itself, the entry[i][i] of the attack matrix must be 1 (true).
	 * Therefore to get the set of self-attacking arguments is to get the diagonal
	 * elements of the attack matrix.
	 * @return a set of arguments in bitvector form, if bitvector[i] = true
	 * representing the argument (with index) i is a self-attacking argument,
	 * otherwise is not.
	 */
	bitvector getSelfAttackingArguments();

	/**
	 * @brief Get all extensions given a specific semantics in format of bitvector.
	 * Each extension is a set of arguments which can "survive the conflict
	 * together". Here, we use a bitvector to represent an extension.
	 * Therefore, all extensions are formed an set of bit vectors, i.e.,
	 * set<bitvector>. Each bitvector in set is an extension w.r.t.
	 * a given semantics. The computed extensions are stored in m_extensions,
	 * therefore, to get all extensions, it must first invoke the function
	 * computeExtension()
	 * @return a set of bitvector, i.e., set<bitvector>.
	 */
	const set<bitvector>& getBvExtensions();

	/**
	 * @brief Get a vector of integers {0, 1, 2}: 2 -- Unknown, 1 -- in grounded
	 * extension, and 0 -- attacked by the grounded extension.
	 * @return a vector of integers {0, 1, 2}.
	 */
	vector<int> getGroundedIntVector();

	/**
	 * @brief Redirect the output stream to streambuf* strbuf or ostream& os. If strbuf = cout.rdbuf(),
	 * then the output is standard output. It can also redirect the output to
	 * a file by the following codes:
	 * ~~~~~{.cpp}
	 *   ofstream ofile("output.txt");
	 *   streambuf* oldsb = xxx.setOutput(ofile.rdbuf());
	 * ~~~~~
	 * or
	 * ~~~~~{.cpp}
	 *   streambuf* oldsb = xxx.setOutput(ofile);
	 * ~~~~~
	 * @param strbuf is a streambuf pointer.
	 * @return return the old streambuf, which can be used to redirect the old
	 * streambuf.
	 */
	streambuf* setOutput(streambuf* strbuf);

	/**
	 * Method:    setOutput
	 * FullName:  public  argumatrix::Reasoner::setOutput
	 * @see       streambuf* setOutput(streambuf* strbuf = std::cout.rdbuf());
	 * @param     ostream & os
	 * @return    streambuf*
	 */
	streambuf* setOutput(ostream& os = std::cout);

	/**
	 * @brief Print an extension with the form of bitvector. Assume the bitvector
	 * is [0, 1, 0, 1], the arguments corresponding to entry 1 will
	 * be print. The member *m_output* will determine where to print.
	 * @param bitvector& bv_ext : The bv_ext is a bitvector, which represents
	 * an extension.
	 * @return no return.
	 * @see [setOutput]
	 */
	void printLabSet(const bitvector& bv_ext);

	/**
	 * @brief Print a set of arguments. The member *m_output* will determine
	 * where to print.
	 * @param const std::set<string>& labset
	 * @return no return.
	 * @see setOutput(streambuf* strbuf = std::cout.rdbuf());
	 */
	void printLabSet(const std::set<string>& labset);

	/**
	 * @brief Output all extensions given a specific semantics with an ostream.
	 * Each extension is a set of arguments which can "survive the conflict
	 * together". Here, we return a string to represent all
	 * extensions. For example, if there are two extensions,
	 * [a,b] and [b, d], for some problem w.r.t. a given semantics, then we
	 * return string "[[a,b],[d,c]]". If extensions is not existing, the string
	 * "[]" will return.
	 * @param ostream& os = std::cout, output the resluts into the ostream os.
	 * @return a set of bitvector, i.e., set<bitvector>.
	 */
	void printBvExts();

	/**
	 * @brief Get the grounded extension
	 * @return a set of arguments in bitvector form.
	 */
	bitvector getGroundedExtension();

	/**
	 * @brief Print the grounded extension
	 * @return no return.
	 */
	void printGroundedExt();

	/**
	 * @brief Convert a set of argument (string) labels to an integer vector of {0,1,2},
	 * All arguments in these set, the indices is 1; otherwise 2 (representing unknown)
	 * @return no return.
	 */
	vector<int> labelSet2IntVector(const std::set<string>& label_set);

public:
	/**
	 * @brief Problem [EE-$\sigma$]
	 * Print all extensions
	 * @param no argument
	 * @return no return.
	 */
	virtual void task_EE() { cerr << "Unimplemented!" << endl; }
	virtual void task_EX() { cerr << "Unimplemented!" << endl; }

	/**
	 * @brief Problem [EC-$\sigma$]
	 * Given an $\textit{AF}=\left< \mathcal{X}, \mathcal{R}\right>$ and
	 * an argument $s \in \mathcal{X}$ (respectively, a set of arguments
	 * $S\subseteq \mathcal{X}$), enumerate all sets $E\subseteq \mathcal{X}$
	 * such that $E \in \mathcal{E}_\sigma(AF)$ and $s\in E$ (respectively,
	 * $S \subseteq E$).
	 * @param set<string>, or a Boolean vector: a set of argument
	 * @return no return.
	 */
	virtual void task_EC(const std::set<string>& argset)
	{ cerr << "Unimplemented!" << endl; }

	/**
	 * @brief Problem [SC-$\sigma$]
	 * Given an $\textit{AF}=\left< \mathcal{X}, \mathcal{R}\right>$ and an
	 * argument $s \in \mathcal{X}$ (respectively, a set of arguments
	 * $S\subseteq \mathcal{X}$), enumerate some set $E\subseteq \mathcal{X}$
	 * such that $E \in \mathcal{E}_\sigma(AF)$ and $s\in E$ (respectively,
	 * $S \subseteq E$).
	 * @param a set of arguments.
	 * @return no return.
	 */
	virtual void task_SC(const std::set<string>& argset)
	{ cerr << "Unimplemented!" << endl; }

	/**
	 * @brief Problem [SE-$\sigma$]
	 * Given an $\textit{AF}=\left< \mathcal{X}, \mathcal{R}\right>$, enumerate
	 * some set $E\subseteq \mathcal{X}$ that are in $\mathcal{E}_\sigma(AF)$.
	 * @param a set of arguments.
	 * @return no return.
	 */
	virtual void task_SE() { cerr << "Unimplemented!" << endl; }

	/**
	 * @brief Problem [DE-$\sigma$]
	 * Given an $AF = \left<X,R\right>$ and a set of arguments $S \subseteq X$. Decide whether S is
	 * a \sigma-extension of AF, i.e., S \in E_\sigma(AF).
	 * @param a set of arguments.
	 * @return true if argset is a \sigma-extension; otherwise return false.
	 * @note For this task, the *set<string>& argset* can be empty, which means
	 * to decide whether the empty set is a $\sigma$-extension of AF. This indicates
	 * that the option -a is not necessary, then the default *set<string>& argset*
	 * is empty.
	 */
	virtual void task_DE(const std::set<string>& argset)
	{ cerr << "Unimplemented!" << endl; }

	/**
	 * @brief Problem [DN-$\sigma$]
	 * Given an $AF = \left<X,R\right>$ and a set of arguments $S \subseteq X$. Decide whether
	 * there exist a non-empty \sigma-extension for AF
	 * @return true if there exist a non-empty \sigma-extension; otherwise return false.
	 */

	virtual void task_DN() { cerr << "Unimplemented!" << endl; }

	/**
	 * @brief Problem [DC-$\sigma$]
	 * Given an $AF = \left<X,R\right>$ and an argument s \in X (respectively, a set of
	 * arguments $S \subseteq X$). Decide whether s contained (respectively, S included)
	 * in some E \in E_\sigma(AF) (i.e., credulously justified).
	 */
	virtual void task_DC(const std::set<string>& argset)
	{ cerr << "Unimplemented!" << endl; }

	/**
	 * @brief Problem [DS-$\sigma$]
	 * Given an $AF = \left<X,R\right>$ and an argument s \in X (respectively, a set of
	 * arguments $S \subseteq X$). Decide whether s contained (respectively, S included)
	 * in each E \in E_\sigma(AF) (i.e., skeptically justified).
	 */
	virtual void task_DS(const std::set<string>& argset)
	{ cerr << "Unimplemented!" << endl; }

protected:
	bitmatrix m_BmAtkMtx;  /**< The bitmatrix of the Dung Abstract argumentation framework. We can
	* access all attackers of an argument. The attackers of the argument
	* with index i is m_BmAtkMtx[i].
	*/

	size_type m_argNum; /**< The number of arguments */

	const DungAF&	  m_daf;  /**< Dung's abstract argumentation framework */

	set< bitvector > m_extensions;

	vector< std::string > m_argLabels; /**< Dung's abstract argumentation framework */

	std::ostream m_output; /**< Where to output */
};

Reasoner::Reasoner(const DungAF& daf, streambuf* osbuff /*= std::cout.rdbuf()*/):
	m_daf(daf), m_output(osbuff)
{
	m_argNum = m_daf.getNumberOfArguments();
	m_BmAtkMtx = m_daf.getAttackMatrix();
	m_argLabels = m_daf.getArgumentLabels();
}

__inline
argumatrix::bitvector Reasoner::getAttacked(const bitvector& _bv)
{
	//assert( _bv.size() == m_argNum );

	return m_BmAtkMtx * _bv;
}

__inline
argumatrix::bitvector Reasoner::characteristic(const bitvector& _bv)
{
	// return ~(m_BmAtkMtx * (~(m_BmAtkMtx * _bv)));
	return neutrality(neutrality(_bv));
}

__inline
argumatrix::bitvector Reasoner::characteristic()
{
	return characteristic(bitvector::EmptySet(m_argNum));
}

__inline
bool Reasoner::is_conflict_free(const bitvector& _bv)
{
	// S is conflict-free iff $S \intersect R^+(S) = \emptyset$.
	return !_bv.intersects( getAttacked(_bv) );
}

__inline
bool Reasoner::is_conflict_free(const set<string>& argset)
{
	return is_conflict_free(m_daf.labelSet2bv(argset));
}

__inline
bool Reasoner::is_acceptable(const bitvector& S, const bitvector& A)
{
	return A.is_subset_of( characteristic(S) );
}

__inline
const set<bitvector>& Reasoner::getBvExtensions()
{
	return m_extensions;
}

__inline
bool Reasoner::is_self_attacking(size_type idx)
{
	// assert(idx < m_argNum);

	return m_BmAtkMtx[idx][idx];
}

__inline
argumatrix::bitvector Reasoner::getSelfAttackingArguments()
{
	//bitvector _bv(m_argNum, false);
	//for (size_type i=0; i<m_argNum; i++)
	//{
	//	_bv[i] = m_BmAtkMtx[i][i];
	//}

	//return _bv;

	return m_BmAtkMtx.diag();
}

__inline
void Reasoner::printBvExts()
{
	m_daf.outputBvSet(m_extensions, m_output);
}

void Reasoner::printLabSet(const bitvector& bv_ext)
{
	bool first = true;

	m_output << LEFT_LIMITER;
	for ( size_type i = bv_ext.find_first();
		i != bitvector::npos;
		i = bv_ext.find_next(i) )
	{
		if(first){
			first = false;
		}else{
			m_output << DELIMITER; // ","
		}
		m_output << m_argLabels[i];
	}
	m_output << RIGHT_LIMITER;

	// m_daf.outputBv(bv_ext, m_output);
}

void Reasoner::printLabSet(const std::set<string>& labset)
{
	bool first = true;

	m_output << LEFT_LIMITER;
	set<string>::iterator itr = labset.begin();
	for ( ; itr != labset.end(); ++itr )
	{
		if(first){
			first = false;
		}else{
			m_output << DELIMITER; // ","
		}
		m_output << *itr;
	}
	m_output << RIGHT_LIMITER;
}

__inline
streambuf* Reasoner::setOutput(streambuf* strbuf)
{
	return m_output.rdbuf(strbuf);
}

__inline
streambuf* Reasoner::setOutput(ostream& os /*= std::cout*/)
{
	return m_output.rdbuf(os.rdbuf());
}

argumatrix::bitvector Reasoner::getGroundedExtension()
{
	bitvector _bv = bitvector::EmptySet(m_argNum);
	bitvector _bv_next = characteristic(_bv);

	while (_bv != _bv_next)
	{
		_bv = _bv_next;
		_bv_next = characteristic(_bv);
	}

	return _bv_next;
}

vector<int> Reasoner::getGroundedIntVector()
{
	// create m_argNum length vector with initial value 2 -- Unknown
	vector<int> vecii(m_argNum, 2);

	bitvector gr_ext = bitvector::EmptySet(m_argNum);
	bitvector gr_out;
	bitvector _bv_last;
	do {
		_bv_last = gr_ext;
		//gr_out = (m_BmAtkMtx * _bv_last); // S =  R^+(X)
		//gr_ext = ~(m_BmAtkMtx * (~gr_out)); // G = ~R^+(~S)
		gr_out = getAttacked( _bv_last ); // S =  R^+(X)
		gr_ext = neutrality( ~gr_out ); // G = ~R^+(~S)
	} while (gr_ext != _bv_last);

	// cout << "gr_ext" << gr_ext << endl;

	for(size_type i=0; i<m_argNum; ++i)
	{
		if( gr_ext[i] ) { vecii[i] = 1; }
		else if ( gr_out[i] ) { vecii[i] = 0; }
	}

	return vecii;
}

vector<int> Reasoner::labelSet2IntVector(const std::set<string>& label_set)
{
	vector<int> vecI( m_argNum, 2 );
	set<string>::iterator _ls_itr;
	for (_ls_itr = label_set.begin(); _ls_itr != label_set.end(); ++_ls_itr)
	{
		vecI[ m_daf.getArgumentIdx( *_ls_itr ) ] = 1;
	}

	return vecI;
}

__inline
void Reasoner::printGroundedExt()
{
	m_output << LEFT_LIMITER;  // [
	printLabSet( getGroundedExtension() );
	m_output << RIGHT_LIMITER; // ]
}

__inline
argumatrix::bitvector Reasoner::neutrality(const bitvector& _bv)
{
	return ~getAttacked(_bv);
}

__inline
bool Reasoner::is_admissible(const bitvector& _bv)
{
	// $S$ is an admissible extension iff $S \subseteq F(S) \cap N(S)$.
	bitvector neu_s = neutrality(_bv);
	bitvector f_s = neutrality( neu_s );
	f_s &= neu_s;

	return _bv.is_subset_of(f_s);
}

__inline
bool Reasoner::is_complete(const bitvector& _bv)
{
	// // $S$ is a complete extension iff $S$ is conflict-free and $S == F(S)$.
	// if ( !is_conflict_free(_bv) )
	//	  return false;
	// bitvector neu_s = neutrality(_bv);
	// bitvector f_s = neutrality( neu_s );
	// return _bv == f_s;

	// This way may be more efficient.
	return is_conflict_free(_bv) && (_bv == characteristic(_bv));

	//// $S$ is a complete extension iff $S == F(S) \cap N(S)$.
	//bitvector neu_s = neutrality(_bv);
	//bitvector f_s = neutrality( neu_s );
	//f_s &= neu_s;

	//return _bv == f_s;
}

__inline
bool Reasoner::is_stable(const bitvector& _bv)
{
	// $S$ is a stable extension iff $S == N(S)$.
	bitvector neu_s = neutrality(_bv);

	return _bv == neu_s;
}

__inline
bool Reasoner::is_grounded(const bitvector& _bv)
{
	bitvector gr_ext = getGroundedExtension();

	return (_bv == gr_ext);
}

} // namespace argumatrix

#endif  //DUNG_REASONER_HPP


/*
 * VariableNode.cpp
 *
 *  Created on: 2013/06/23
 *      Author: kryozahiro
 */

#include "VariableNode.h"

#include <boost/lexical_cast.hpp>
using namespace std;

void VariableNode::randomize(const ProgramType& programType, mt19937_64& randomEngine) {
	uniform_int_distribution<int> dist(0, programType.getInputType().getSize() - 1);
	index = dist(randomEngine);
}

double VariableNode::operator()(const vector<double>& input) {
	return input[index];
}

int VariableNode::getArity() const {
	return 0;
}

string VariableNode::getName() const {
	return "var" + boost::lexical_cast<string>(index);
}


//---------------------------------------------------------------------------//
// Copyright (c) 2014 Roshan <thisisroshansmail@gmail.com>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
// See http://kylelutz.github.com/compute for more information.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE TestBernoulliDistribution
#include <boost/test/unit_test.hpp>

#include <boost/compute/system.hpp>
#include <boost/compute/command_queue.hpp>
#include <boost/compute/container/vector.hpp>
#include <boost/compute/random/default_random_engine.hpp>
#include <boost/compute/random/bernoulli_distribution.hpp>

#include "context_setup.hpp"

BOOST_AUTO_TEST_CASE(bernoulli_distribution_doctest)
{
    boost::compute::vector<bool> vec(10, context);

//! [generate]
// initialize the default random engine
boost::compute::default_random_engine engine(queue);

// setup the bernoulli distribution to produce booleans
// with parameter p = 0.25
boost::compute::bernoulli_distribution<float> distribution(0.25f);

// generate the random values and store them to 'vec'
distribution.generate(vec.begin(), vec.end(), engine, queue);
//! [generate]
}

BOOST_AUTO_TEST_SUITE_END()


/* boost random/cauchy_distribution.hpp header file
 *
 * Copyright Jens Maurer 2000-2001
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id: cauchy_distribution.hpp,v 1.15 2004/07/27 03:43:32 dgregor Exp $
 *
 * Revision history
 *  2001-02-18  moved to individual header files
 */

#ifndef BOOST_RANDOM_CAUCHY_DISTRIBUTION_HPP
#define BOOST_RANDOM_CAUCHY_DISTRIBUTION_HPP

#include <cmath>
#include <iostream>
#include <boost/limits.hpp>
#include <boost/static_assert.hpp>

namespace boost {

#if defined(__GNUC__) && (__GNUC__ < 3)
// Special gcc workaround: gcc 2.95.x ignores using-declarations
// in template classes (confirmed by gcc author Martin v. Loewis)
  using std::tan;
#endif

// Cauchy distribution: p(x) = sigma/(pi*(sigma**2 + (x-median)**2))
template<class RealType = double>
class cauchy_distribution
{
public:
  typedef RealType input_type;
  typedef RealType result_type;

#ifndef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
  BOOST_STATIC_ASSERT(!std::numeric_limits<RealType>::is_integer);
#endif

  explicit cauchy_distribution(result_type median = result_type(0),
                               result_type sigma = result_type(1))
    : _median(median), _sigma(sigma) { }

  // compiler-generated copy ctor and assignment operator are fine

  result_type median() const { return _median; }
  result_type sigma() const { return _sigma; }
  void reset() { }

  template<class Engine>
  result_type operator()(Engine& eng)
  {
    // Can we have a boost::mathconst please?
    const result_type pi = result_type(3.14159265358979323846);
#ifndef BOOST_NO_STDC_NAMESPACE
    using std::tan;
#endif
    return _median + _sigma * tan(pi*(eng()-result_type(0.5)));
  }

#if !defined(BOOST_NO_OPERATORS_IN_NAMESPACE) && !defined(BOOST_NO_MEMBER_TEMPLATE_FRIENDS)
  template<class CharT, class Traits>
  friend std::basic_ostream<CharT,Traits>&
  operator<<(std::basic_ostream<CharT,Traits>& os, const cauchy_distribution& cd)
  {
    os << cd._median << " " << cd._sigma;
    return os;
  }

  template<class CharT, class Traits>
  friend std::basic_istream<CharT,Traits>&
  operator>>(std::basic_istream<CharT,Traits>& is, cauchy_distribution& cd)
  {
    is >> std::ws >> cd._median >> std::ws >> cd._sigma;
    return is;
  }
#endif

private:
  result_type _median, _sigma;
};

} // namespace boost

#endif // BOOST_RANDOM_CAUCHY_DISTRIBUTION_HPP


// Copyright Louis Dionne 2013-2017
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/assert.hpp>
#include <boost/hana/integral_constant.hpp>

#include <boost/hana/and.hpp>
#include <boost/hana/div.hpp>
#include <boost/hana/equal.hpp>
#include <boost/hana/greater.hpp>
#include <boost/hana/greater_equal.hpp>
#include <boost/hana/less.hpp>
#include <boost/hana/less_equal.hpp>
#include <boost/hana/minus.hpp>
#include <boost/hana/mod.hpp>
#include <boost/hana/mult.hpp>
#include <boost/hana/negate.hpp>
#include <boost/hana/not.hpp>
#include <boost/hana/not_equal.hpp>
#include <boost/hana/or.hpp>
#include <boost/hana/plus.hpp>
namespace hana = boost::hana;

// Arithmetic operators
BOOST_HANA_CONSTANT_CHECK(+hana::int_c<1> == hana::int_c<1>);
BOOST_HANA_CONSTANT_CHECK(-hana::int_c<1> == hana::int_c<-1>);
BOOST_HANA_CONSTANT_CHECK(hana::int_c<1> + hana::int_c<2> == hana::int_c<3>);
BOOST_HANA_CONSTANT_CHECK(hana::int_c<1> - hana::int_c<2> == hana::int_c<-1>);
BOOST_HANA_CONSTANT_CHECK(hana::int_c<3> * hana::int_c<2> == hana::int_c<6>);
BOOST_HANA_CONSTANT_CHECK(hana::int_c<6> / hana::int_c<3> == hana::int_c<2>);
BOOST_HANA_CONSTANT_CHECK(hana::int_c<6> % hana::int_c<4> == hana::int_c<2>);
BOOST_HANA_CONSTANT_CHECK(~hana::int_c<6> == hana::int_c<~6>);
BOOST_HANA_CONSTANT_CHECK((hana::int_c<6> & hana::int_c<3>) == hana::int_c<6 & 3>);
BOOST_HANA_CONSTANT_CHECK((hana::int_c<4> | hana::int_c<2>) == hana::int_c<4 | 2>);
BOOST_HANA_CONSTANT_CHECK((hana::int_c<6> ^ hana::int_c<3>) == hana::int_c<6 ^ 3>);
BOOST_HANA_CONSTANT_CHECK((hana::int_c<6> << hana::int_c<3>) == hana::int_c<(6 << 3)>);
BOOST_HANA_CONSTANT_CHECK((hana::int_c<6> >> hana::int_c<3>) == hana::int_c<(6 >> 3)>);

// Comparison operators
BOOST_HANA_CONSTANT_CHECK(hana::int_c<0> == hana::int_c<0>);
BOOST_HANA_CONSTANT_CHECK(hana::int_c<1> != hana::int_c<0>);
BOOST_HANA_CONSTANT_CHECK(hana::int_c<0> < hana::int_c<1>);
BOOST_HANA_CONSTANT_CHECK(hana::int_c<0> <= hana::int_c<1>);
BOOST_HANA_CONSTANT_CHECK(hana::int_c<0> <= hana::int_c<0>);
BOOST_HANA_CONSTANT_CHECK(hana::int_c<1> > hana::int_c<0>);
BOOST_HANA_CONSTANT_CHECK(hana::int_c<1> >= hana::int_c<0>);
BOOST_HANA_CONSTANT_CHECK(hana::int_c<0> >= hana::int_c<0>);

// Logical operators
BOOST_HANA_CONSTANT_CHECK(hana::int_c<3> || hana::int_c<0>);
BOOST_HANA_CONSTANT_CHECK(hana::int_c<3> && hana::int_c<1>);
BOOST_HANA_CONSTANT_CHECK(!hana::int_c<0>);
BOOST_HANA_CONSTANT_CHECK(!!hana::int_c<3>);

int main() { }


/*
 * Bayes++ the Bayesian Filtering Library
 * Copyright (c) 2002 Michael Stevens
 * See accompanying Bayes++.htm for terms and conditions of use.
 *
 * $Id$
 */
#define NO_TESTS

#ifndef NO_TESTS
#include "BayesFilter/allFilters.hpp"
#include "BayesFilter/matSup.hpp"
#include "Test/random.hpp"
#include <cmath>
#include <iostream>
#include <exception>
#include <boost/numeric/ublas/io.hpp>
#include <boost/limits.hpp>
#include <boost/bind.hpp>

using namespace Bayesian_filter;
using namespace Bayesian_filter_matrix;

// Instantiate complete schemes to check the templates
#include "BayesFilter/filters/average1.hpp"
template Average1_filter<Covariance_scheme>;

#include "BayesFilter/filters/indirect.hpp"
template Indirect_state_filter<Covariance_scheme>;
template Indirect_kalman_filter<Covariance_scheme>;

void nested_prod ()
{
	Matrix R(1,1), A(1,1),B(1,1),C(1,1);
	R = prod(A, prod(B,C));
	R = prod(A, B);
}

#ifdef REMOVED

// Square
template <class scalar>
inline scalar sqr(scalar x)
{
	return x*x;
}

class Test_random : public Bayesian_filter_test::Boost_random, public SIR_random
/*
 * Random numbers for filters from Boost
 */
{
public:
	Float normal (const Float mean, const Float sigma)
	{
		return Boost_random::normal (mean, sigma);
	}
	void normal (DenseVec& v)
	{
		Boost_random::normal (v);
	}
	void uniform_01 (DenseVec& v)
	{
		Boost_random::uniform_01 (v);
	}
	void seed ()
	{
		Boost_random::seed();
	}
} Random;

void test_inverse()
{
	Matrix U (3,3);
	U.clear();
	U(0,0) = 1*1;
	U(1,1) = 1*1;
	U(2,2) = 1*1;
	U(0,1) = 2;
	U(0,2) = 3;
	U(1,2) = 5;
/*
	Vec rv(U.size1()*U.size2());
	Test_random rand;
	rand.exponential_1(rv);

	Vec::iterator rv_i = rv.begin();
	for (std::size_t r = 0; r < U.size1(); ++r)
		for (std::size_t c = 0; c < U.size2(); ++c)
		{
			U(r,c) = *rv_i; ++rv_i;
		}
	*/
	std::cout << U << std::endl;

	SymMatrix X(U.size1(),U.size2());
	X = prod(U, trans(U));
	std::cout << X << std::endl;

	SymMatrix XI(X.size1(), X.size2());
	SymMatrix XII(X.size1(), X.size2());
//	XI = X;
//	UdUfactor(XI, 2);
//	std::cout << XI << std::endl;
//	XI = X;
//	LdLfactor(XI, 2);
//	std::cout << XI << std::endl;

	UdUinversePD (XI, X);
	std::cout << XI << std::endl;
	std::cout << prod(X,XI) << std::endl;

	UdUinversePD (XII, XI);
	std::cout << XII << std::endl;
};

void test_small_inverse ()
{
	SymMatrix X(1,1);
	SymMatrix XI(X.size1(), X.size2());

	Float a = 0;
	Float rcond;
	X(0,0) = a;
	std::cout << X << std::endl;
	rcond = UdUinversePD (XI, X);
	std::cout << rcond << XI << std::endl;

	X(0,0) = 1./a;
	std::cout << X << std::endl;
	rcond = UdUinversePD (XI, X);
	std::cout << rcond << XI << std::endl;
}

void test_SPD()
{
	SymMatrix S(1,1);
	S(0,0) = 5.;
	Vec Stemp(1);
	SymMatrix P(2,2);

	RowMatrix R(2,1);
	R(0,0) = 1.;
	R(1,0) = 2.;

	// Try prod_SPD
	RowMatrix RStemp(R.size1(), S.size2());
	noalias(P) = prod_SPD(R,S, RStemp);
	std::cout << P << std::endl;

	// Try prod_SPD
	ColMatrix RT(1,2);
	noalias(P) = prod_SPDT (RT, Stemp);
}

void test_SPD_all()
{
	SymMatrix S(1,1);
	Vec x(1);
	S(0,0) = 5.;
	x[0] = 1.;

	Float v1 = ublas::inner_prod (x, ublas::prod(S,x) );
	Float v2 = prod_SPDT(x,S);
	(void)v1,(void)v2;

	v2 = prod_SPD(x,x);

	test_SPD();
}

void test_temp_prod()
{
	Matrix A(2,2);
	Matrix R (prod( ublas::prod<Matrix>(A,A), ublas::prod<Matrix>(A,A)));
}

void test_UdU()
{
	SymMatrix S(3,3);
	S(0,0) = 2.;
	S(2,2) = 3.;
	S(0,1) = S(1,0) = 5.;
//	S(2,1) = S(1,2) = 5.;
	S(0,2) = S(2,0) = 7.;
	std::cout << S << std::endl;

	UTriMatrix UD(3,3);
	Float rcond = UCfactor (UD, S);
	std::cout <<rcond << std::endl;

	std::cout << UD << std::endl;

//	UdUrecompose (UD);
//	std::cout << UD << std::endl;
}

void test_ident()
{
	SymMatrix S(3,3);
	FM::identity(S);
	std::cout << S << std::endl;
}

void test_info_init()
{
	Information_root_info_scheme iri(2);
	Information_scheme i(2);
	SymMatrix X(2,2);
	X(0,0) = 2.;
	X(1,0) = X(0,1) = 1.1;
	X(1,1) = 3.;
	Vec x(2);
	x[0] = 5.;
	x[1] = 7.;

	i.init_kalman (x,X);
	iri.init_kalman (x,X);
	i.update(); iri.update();
	std::cout << i.x << i.X <<std::endl;;
	std::cout << i.y << i.Y <<std::endl;;
	std::cout << iri.x << iri.X <<std::endl;;
	std::cout << iri.y << iri.Y <<std::endl;;

	i.init_information (x,X);
	iri.init_information (x,X);
	i.update(); iri.update();
	std::cout << i.x << i.X <<std::endl;;
	std::cout << i.y << i.Y <<std::endl;;
	std::cout << iri.x << iri.X <<std::endl;;
	std::cout << iri.y << iri.Y <<std::endl;;
}

void test_unique()
{
	SymMatrix X(2,2);
	X(0,0) = 2.;
	X(1,0) = X(0,1) = 1.1;
	X(1,1) = 3.;
	Vec x(2);
	x[0] = 5.;
	x[1] = 7.;

	Test_random<Float> r;
	SIR_kalman_scheme sf(2, 100, r);
	sf.init_kalman (x,X);

	std::cout << sf.unique_samples() <<',' << sf.stochastic_samples << std::endl;

	General_LiUnAd_observe_model obs(2,1);
	obs.Hx(0,0) = 1;
	obs.Hx(0,1) = 1;
	obs.Zv[0] = 1;
	Vec z(1);
	z[0] = 12.5;

	sf.observe (obs,z);
	sf.update();
	std::cout << sf.unique_samples() <<',' << sf.stochastic_samples << std::endl;

	sf.rougheningK = 0;
	sf.observe (obs,z);
	sf.update();
	std::cout << sf.unique_samples() <<',' << sf.stochastic_samples << std::endl;
}

void numeric_tested()
{
	float a = 0;
	float b = 1.f/a;
	float c = 1.f/b;
	float d = -1.f/b;
	std::cout << a <<','<< b <<','<<c <<','<< d <<std::endl;
	std::cout << a*1.f <<','<< b*1.f <<','<<c*1.f <<','<< d*1.f <<std::endl;
	std::cout << (c==0) <<','<< (c!=0) << std::endl;
	std::cout << (c<0) <<','<< (c>0) << std::endl;
	std::cout << (c>=0) <<','<< (c<=0) << std::endl;

	std::cout << (d==0) <<','<< (d!=0) << std::endl;
	std::cout << (d<0) <<','<< (d>0) << std::endl;
	std::cout << (d>=0) <<','<< (d<=0) << std::endl;
}

#endif

void test_sym_proxy()
/*
 * Problems accessing the symmetric half via a proxy
 * IF packed proxy assign is used then only the represented half is copied
 * in the assign. The internal equality test will pickup the error
 */
{
	ublas::symmetric_matrix<double,ublas::upper> S(2,2);
	ublas::vector<double> v(2);
	S(0,0) = 1; S(1,0) = 2; S(1,1) = 3;
	v(0) = 5; v(1) = 6;
	noalias(ublas::row(S, 0)) = v;		// OK top row is all in upper
	noalias(ublas::row(S, 1)) = v;		// ERROR only one element in upper for packed proxy assign
}

void utinverse_test()
{
	UTriMatrix m(3,3);
	m(0,0) = 2; m(0,1) = 4; m(0,2) = 5;
	m(1,1) = 3; m(1,2) = 7;
	m(2,2) = 5;
	std::cout << m << std::endl;
	std::cout << UTinverse(m) << std::endl;
	std::cout << m << std::endl;
	std::cout << UTinverse(m) << std::endl;
	std::cout << m << std::endl;
}

void other_tests()
{
	// Other things I might want to benchmark
	extern void other_bench();
//	other_bench();

	try {
		utinverse_test ();
		// Tests go here
	}
	catch (std::exception& e)
	{
		std::cerr << e.what() << std::endl;
	}
}

#else
void other_tests()
{
}
#endif


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_ICEIL_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_ICEIL_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-arithmetic
    Function object implementing iceil capabilities

    Computes the integer conversion of the ceil of its parameter.

    @par semantic:
    For any given value @c x of type @c T:

    @code
    as_integer_t<T> r = iceil(x);
    @endcode

    is equivalent to:

    @code
    as_integer_t<T> r = toints(ceil(x));
    @endcode

    @par Note:
    This operation is properly saturated

  **/
  const boost::dispatch::functor<tag::iceil_> iceil = {};
} }
#endif

#include <boost/simd/function/scalar/iceil.hpp>
#include <boost/simd/function/simd/iceil.hpp>

#endif


#pragma once
#include <vector>
#include <boost/serialization/vector.hpp>
#include <boost/serialization/access.hpp>
#include "BaseNeuron.hpp"
#include "NeuronModel.hpp"
#include "../../optimizer/StochasticGradientDescent.hpp"
#include "activation_function/ActivationFunction.hpp"
#include "../../../tools/Tools.hpp"

namespace snn::internal
{
    class Neuron
    {
    private:
        friend class boost::serialization::access;
        template <class Archive>
        void serialize(Archive& ar, unsigned version);

    protected:

        int numberOfInputs;
        std::vector<float> weights;
        float bias;

        std::vector<float> previousDeltaWeights;
        std::vector<float> lastInputs;
        std::vector<float> errors;

        float sum = 0;

        activation activationFunction;
        std::shared_ptr<ActivationFunction> outputFunction;
        std::shared_ptr<NeuralNetworkOptimizer> optimizer = nullptr;

        static float randomInitializeWeight(int numberOfInputs);

    public:
        Neuron() = default; // use restricted to Boost library only
        Neuron(NeuronModel model, std::shared_ptr<NeuralNetworkOptimizer> optimizer);
        Neuron(const Neuron& neuron) = default;
        ~Neuron() = default;

        [[nodiscard]] int isValid() const;

        [[nodiscard]] std::vector<float> getWeights() const;
        [[nodiscard]] int getNumberOfParameters() const;
        [[nodiscard]] int getNumberOfInputs() const;

        NeuralNetworkOptimizer* getOptimizer() const;
        void setOptimizer(std::shared_ptr<NeuralNetworkOptimizer> newOptimizer);

        bool operator==(const Neuron& neuron) const;
        bool operator!=(const Neuron& neuron) const;
    };

    template <class Archive>
    void Neuron::serialize(Archive& ar, [[maybe_unused]] const unsigned version)
    {
        ar.template register_type<StochasticGradientDescent>();
        ar & this->optimizer;
        ar & this->numberOfInputs;
        ar & this->weights;
        ar & this->bias;
        ar & this->previousDeltaWeights;
        ar & this->lastInputs;
        ar & this->errors;
        ar & this->sum;
        ar & this->activationFunction;
        this->outputFunction = ActivationFunction::get(activationFunction);
    }
}


#include "dynet/nodes.h"
#include "dynet/dynet.h"
#include "dynet/training.h"
#include "dynet/timing.h"
#include "dynet/rnn.h"
#include "dynet/gru.h"
#include "dynet/lstm.h"
#include "dynet/dict.h"
#include "dynet/expr.h"
#include "getpid.h"

#include <iostream>
#include <fstream>
#include <sstream>

#include <boost/archive/text_iarchive.hpp>
#include <boost/archive/text_oarchive.hpp>

using namespace std;
using namespace dynet;

unsigned LAYERS = 2;
unsigned INPUT_DIM = 8;  //256
unsigned HIDDEN_DIM = 24;  // 1024
unsigned VOCAB_SIZE = 0;

dynet::Dict d;
int kSOS;
int kEOS;

template <class Builder>
struct RNNLanguageModel {
  LookupParameter p_c;
  Parameter p_R;
  Parameter p_bias;
  Builder builder;
  explicit RNNLanguageModel(Model& model) : builder(LAYERS, INPUT_DIM, HIDDEN_DIM, &model) {
    p_c = model.add_lookup_parameters(VOCAB_SIZE, {INPUT_DIM});
    p_R = model.add_parameters({VOCAB_SIZE, HIDDEN_DIM});
    p_bias = model.add_parameters({VOCAB_SIZE});
  }

  // return Expression of total loss
  Expression BuildLMGraph(const vector<int>& sent, ComputationGraph& cg) {
    const unsigned slen = sent.size() - 1;
    builder.new_graph(cg);  // reset RNN builder for new graph
    builder.start_new_sequence();
    Expression i_R = parameter(cg, p_R); // hidden -> word rep parameter
    Expression i_bias = parameter(cg, p_bias);  // word bias
    vector<Expression> errs;
    for (unsigned t = 0; t < slen; ++t) {
      Expression i_x_t = lookup(cg, p_c, sent[t]);
      // y_t = RNN(x_t)
      Expression i_y_t = builder.add_input(i_x_t);
      Expression i_r_t =  i_bias + i_R * i_y_t;

      // LogSoftmax followed by PickElement can be written in one step
      // using PickNegLogSoftmax
#if 0
      Expression i_ydist = logsoftmax(i_r_t);
      errs.push_back(pick(i_ydist, sent[t+1]));
#if 0
      Expression i_ydist = softmax(i_r_t);
      i_ydist = log(i_ydist)
      errs.push_back(pick(i_ydist, sent[t+1]));
#endif
#else
      Expression i_err = pickneglogsoftmax(i_r_t, sent[t+1]);
      errs.push_back(i_err);
#endif
    }
    Expression i_nerr = sum(errs);
#if 0
    return -i_nerr;
#else
    return i_nerr;
#endif
  }

  // return Expression for total loss
  void RandomSample(int max_len = 150) {
    cerr << endl;
    ComputationGraph cg;
    builder.new_graph(cg);  // reset RNN builder for new graph
    builder.start_new_sequence();

    Expression i_R = parameter(cg, p_R);
    Expression i_bias = parameter(cg, p_bias);
    vector<Expression> errs;
    int len = 0;
    int cur = kSOS;
    while(len < max_len && cur != kEOS) {
      ++len;
      Expression i_x_t = lookup(cg, p_c, cur);
      // y_t = RNN(x_t)
      Expression i_y_t = builder.add_input(i_x_t);
      Expression i_r_t = i_bias + i_R * i_y_t;

      Expression ydist = softmax(i_r_t);

      unsigned w = 0;
      while (w == 0 || (int)w == kSOS) {
        auto dist = as_vector(cg.incremental_forward(ydist));
        double p = rand01();
        for (; w < dist.size(); ++w) {
          p -= dist[w];
          if (p < 0.0) { break; }
        }
        if (w == dist.size()) w = kEOS;
      }
      cerr << (len == 1 ? "" : " ") << d.convert(w);
      cur = w;
    }
    cerr << endl;
  }
};

int main(int argc, char** argv) {
  dynet::initialize(argc, argv);
  if (argc != 3 && argc != 4) {
    cerr << "Usage: " << argv[0] << " corpus.txt dev.txt [model.params]\n";
    return 1;
  }
  kSOS = d.convert("<s>");
  kEOS = d.convert("</s>");
  vector<vector<int>> training, dev;
  string line;
  int tlc = 0;
  int ttoks = 0;
  cerr << "Reading training data from " << argv[1] << "...\n";
  {
    ifstream in(argv[1]);
    assert(in);
    while(getline(in, line)) {
      ++tlc;
      training.push_back(read_sentence(line, &d));
      ttoks += training.back().size();
      if (training.back().front() != kSOS && training.back().back() != kEOS) {
        cerr << "Training sentence in " << argv[1] << ":" << tlc << " didn't start or end with <s>, </s>\n";
        abort();
      }
    }
    cerr << tlc << " lines, " << ttoks << " tokens, " << d.size() << " types\n";
  }
  d.freeze(); // no new word types allowed
  VOCAB_SIZE = d.size();

  int dlc = 0;
  int dtoks = 0;
  cerr << "Reading dev data from " << argv[2] << "...\n";
  {
    ifstream in(argv[2]);
    assert(in);
    while(getline(in, line)) {
      ++dlc;
      dev.push_back(read_sentence(line, &d));
      dtoks += dev.back().size();
      if (dev.back().front() != kSOS && dev.back().back() != kEOS) {
        cerr << "Dev sentence in " << argv[2] << ":" << tlc << " didn't start or end with <s>, </s>\n";
        abort();
      }
    }
    cerr << dlc << " lines, " << dtoks << " tokens\n";
  }
  ostringstream os;
  os << "lm"
     << '_' << LAYERS
     << '_' << INPUT_DIM
     << '_' << HIDDEN_DIM
     << "-pid" << getpid() << ".params";
  const string fname = os.str();
  cerr << "Parameters will be written to: " << fname << endl;
  double best = 9e+99;

  Model model;
  Trainer* sgd = nullptr;
  // bool use_momentum = false;
  // if (use_momentum)
  //   sgd = new MomentumSGDTrainer(&model);
  // else
  sgd = new SimpleSGDTrainer(&model);

  RNNLanguageModel<LSTMBuilder> lm(model);
  //RNNLanguageModel<SimpleRNNBuilder> lm(model);
  if (argc == 4) {
    string fname = argv[3];
    ifstream in(fname);
    boost::archive::text_iarchive ia(in);
    ia >> model;
  }

  unsigned report_every_i = 50;
  unsigned dev_every_i_reports = 500;
  unsigned si = training.size();
  vector<unsigned> order(training.size());
  for (unsigned i = 0; i < order.size(); ++i) order[i] = i;
  bool first = true;
  int report = 0;
  unsigned lines = 0;
  while(1) {
    Timer iteration("completed in");
    double loss = 0;
    unsigned chars = 0;
    for (unsigned i = 0; i < report_every_i; ++i) {
      if (si == training.size()) {
        si = 0;
        if (first) { first = false; } else { sgd->update_epoch(); }
        cerr << "**SHUFFLE\n";
        shuffle(order.begin(), order.end(), *rndeng);
      }

      // build graph for this instance
      ComputationGraph cg;
      auto& sent = training[order[si]];
      chars += sent.size() - 1;
      ++si;
      Expression loss_expr = lm.BuildLMGraph(sent, cg);
      loss += as_scalar(cg.forward(loss_expr));
      cg.backward(loss_expr);
      sgd->update();
      ++lines;
    }
    sgd->status();
    cerr << " E = " << (loss / chars) << " ppl=" << exp(loss / chars) << ' ';
    lm.RandomSample();

    // show score on dev data?
    report++;
    if (report % dev_every_i_reports == 0) {
      double dloss = 0;
      int dchars = 0;
      for (auto& sent : dev) {
        ComputationGraph cg;
        Expression loss_expr = lm.BuildLMGraph(sent, cg);
        dloss += as_scalar(cg.forward(loss_expr));
        dchars += sent.size() - 1;
      }
      if (dloss < best) {
        best = dloss;
        ofstream out(fname);
        boost::archive::text_oarchive oa(out);
        oa << model;
      }
      cerr << "\n***DEV [epoch=" << (lines / (double)training.size()) << "] E = " << (dloss / dchars) << " ppl=" << exp(dloss / dchars) << ' ';
    }
  }
  delete sgd;
}


#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graph_traits.hpp>

#include <arlib/esx.hpp>
#include <arlib/graph_utils.hpp>
#include <arlib/multi_predecessor_map.hpp>
#include <arlib/onepass_plus.hpp>
#include <arlib/path.hpp>
#include <arlib/penalty.hpp>
#include <arlib/routing_kernels/types.hpp>

#include <iostream>
#include <string>
#include <string_view>
#include <vector>

// Create type-aliases for the Graph type
using Graph = boost::adjacency_list<boost::vecS, boost::vecS,
                                    boost::bidirectionalS, boost::no_property,
                                    boost::property<boost::edge_weight_t, int>>;
using Vertex = typename boost::graph_traits<Graph>::vertex_descriptor;
using Edge = typename boost::graph_traits<Graph>::edge_descriptor;

/// Run the alternative routing algorithm named @p name.
template <typename WeightMap, typename MultiPredecessorMap>
void run_alt_routing(std::string_view name, Graph const &G,
                     WeightMap const &weight, MultiPredecessorMap &predecessors,
                     Vertex s, Vertex t, int k, double theta) {
  using arlib::routing_kernels;
  if (name == "onepass_plus") {
    arlib::onepass_plus(G, weight, predecessors, s, t, k, theta);
  } else if (name == "esx") {
    arlib::esx(G, weight, predecessors, s, t, k, theta,
               routing_kernels::astar);
  } else if (name == "penalty") {
    double p = 0.1, r = 0.1;
    int max_nb_updates = 10, max_nb_steps = 100000;
    arlib::penalty(G, weight, predecessors, s, t, k, theta, p, r,
                   max_nb_updates, max_nb_steps,
                   routing_kernels::bidirectional_dijkstra);
  } else {
    std::cout << "Unknown algorithm '" << name << "'. Exiting...\n";
    std::exit(1);
  }
}

/// Define a convenient function to compute the alternative routes and return
/// them as a view.
std::vector<arlib::Path<Graph>> get_alternative_routes(std::string_view alg,
                                                       Graph const &G, Vertex s,
                                                       Vertex t) {
  // Make output MultiPredecessorMap
  auto predecessors = arlib::multi_predecessor_map<Vertex>{};

  int k = 3;                                       // Nb alternative routes
  double theta = 0.5;                              // Overlapping threshold
  auto weight = boost::get(boost::edge_weight, G); // Get Edge WeightMap

  run_alt_routing(alg, G, weight, predecessors, s, t, k, theta);
  auto alt_routes = arlib::to_paths(G, predecessors, weight, s, t);
  return alt_routes;
}

void print_path(arlib::Path<Graph> const &path,
                std::vector<std::string> const &name) {
  using namespace boost;

  for (auto [v_it, v_end] = vertices(path); v_it != v_end; ++v_it) {
    for (auto [e_it, e_end] = out_edges(*v_it, path); e_it != e_end; ++e_it) {
      std::cout << name[source(*e_it, path)] << " -- "
                << name[target(*e_it, path)] << "\n";
    }
  }
}

int main() {
  // Make convenient labels for the vertices
  enum { S, N1, N2, N3, N4, N5, T };
  const long unsigned num_vertices = T;
  const auto name =
      std::vector<std::string>{"s", "n1", "n2", "n3", "n4", "n5", "t"};

  // Writing out the edges in the graph
  const auto edges = std::vector<std::pair<int, int>>{
      {S, N1},  {N1, S},  {S, N2},  {N2, S},  {S, N3},  {N3, S},
      {N1, T},  {T, N1},  {N3, N1}, {N1, N3}, {N3, N5}, {N5, N3},
      {N3, N2}, {N2, N3}, {N3, N4}, {N4, N3}, {N2, N4}, {N4, N2},
      {N5, T},  {T, N5},  {N5, N4}, {N4, N5}, {N4, T},  {T, N4}};

  const auto weights = std::vector<int>{6, 6, 4, 4, 3, 3, 6, 6, 2, 2, 3, 3,
                                        3, 3, 5, 5, 5, 5, 2, 2, 1, 1, 2, 2};
  auto G = Graph{edges.begin(), edges.end(), weights.begin(), num_vertices};

  //=-------------------------------------------------------------------------=

  // OnePass+
  auto res_opplus = get_alternative_routes("onepass_plus", G, S, T);
  // ESX
  auto res_esx = get_alternative_routes("esx", G, S, T);
  // Penalty
  auto res_penalty = get_alternative_routes("penalty", G, S, T);

  std::cout << "OnePass+ solutions...\n";
  for (auto const &route : res_opplus) {
    print_path(route, name);
    std::cout << "--------\n";
  }

  std::cout << "ESX solutions...\n";
  for (auto const &route : res_esx) {
    print_path(route, name);
    std::cout << "--------\n";
  }

  std::cout << "Penalty solutions...\n";
  for (auto const &route : res_penalty) {
    print_path(route, name);
    std::cout << "--------\n";
  }

  return 0;
}


#include "scope_axis_direction_predicate.h"

#include "geometry_component.h"
#include "geometry_core/scope.h"
#include "geometry_system.h"
#include "math_lib/radians.h"
#include "procedural_object_system.h"

#include <boost/qvm/vec_operations.hpp>

#include <cmath>
#include <sstream>

namespace pagoda
{
ScopeAxisDirectionPredicate::ScopeAxisDirectionPredicate(ProceduralObjectSystemPtr objectSystem, char scopeAxis,
                                                         const Vec3F& direction, const Degrees<float>& tolerance)
    : ProceduralObjectPredicate(objectSystem),
      m_scopeAxis(scopeAxis),
      m_direction(normalized(direction)),
      m_tolerance(tolerance)
{
	m_geometrySystem = objectSystem->GetComponentSystem<GeometrySystem>();
}

ScopeAxisDirectionPredicate::~ScopeAxisDirectionPredicate() {}

bool ScopeAxisDirectionPredicate::operator()(const ProceduralObjectPtr object)
{
	auto geometryComponent = m_geometrySystem->GetComponentAs<GeometryComponent>(object);
	if (geometryComponent == nullptr)
	{
		return false;
	}
	Scope scope = geometryComponent->GetScope();
	Vec3F axis = scope.GetAxis(m_scopeAxis);

	auto angle = std::acos(boost::qvm::dot(axis, m_direction));
	if (angle <= static_cast<float>(Radians<float>(m_tolerance)))
	{
		return true;
	}

	return false;
}

std::string ScopeAxisDirectionPredicate::ToString()
{
	std::stringstream ss;
	ss << "ScopeAxisDirectionPredicate. axis: " << m_scopeAxis << " direction: " << m_direction
	   << " tolerance: " << static_cast<float>(m_tolerance);
	return ss.str();
}
}  // namespace pagoda


#include <path_follower/controller/robotcontroller_2steer_stanley.h>

#include <path_follower/utils/pose_tracker.h>
#include <ros/ros.h>

#include <cslibs_navigation_utilities/MathHelper.h>

#include <path_follower/utils/visualizer.h>

#include <deque>

#include <limits>
#include <boost/algorithm/clamp.hpp>

#ifdef TEST_OUTPUT
#include <std_msgs/Float64MultiArray.h>
#endif

#include <path_follower/factory/controller_factory.h>

REGISTER_ROBOT_CONTROLLER(RobotController_2Steer_Stanley, 2steer_stanley, default_collision_avoider);

using namespace std;
using namespace Eigen;

RobotController_2Steer_Stanley::RobotController_2Steer_Stanley():
    RobotController()
{

    ROS_INFO("Parameters: k_forward=%f, k_backward=%f\n"
                "vehicle_length=%f\n"
                "goal_tolerance=%f",
                params_.k_forward(), params_.k_backward(),
                params_.vehicle_length(),
                params_.goal_tolerance());

#ifdef TEST_OUTPUT
    test_pub_ = nh_.advertise<std_msgs::Float64MultiArray>("test_output", 100);
#endif

}

void RobotController_2Steer_Stanley::stopMotion() {

    move_cmd_.setVelocity(0.f);
    move_cmd_.setDirection(0.f);

    MoveCommand cmd = move_cmd_;
    publishMoveCommand(cmd);
}

void RobotController_2Steer_Stanley::start() {

}

void RobotController_2Steer_Stanley::setPath(Path::Ptr path) {
    RobotController::setPath(path);
}

RobotController::MoveCommandStatus RobotController_2Steer_Stanley::computeMoveCommand(
        MoveCommand* cmd) {

    if(path_interpol.n() <= 2)
        return RobotController::MoveCommandStatus::ERROR;

    const Eigen::Vector3d pose = pose_tracker_->getRobotPose();
    const geometry_msgs::Twist velocity_measured = pose_tracker_->getVelocity();

    RobotController::findOrthogonalProjection();
    double d = -orth_proj_;

    if(RobotController::isGoalReached(cmd)){
       return RobotController::MoveCommandStatus::REACHED_GOAL;
    }

    // draw a line to the orthogonal projection
    geometry_msgs::Point from, to;
    from.x = pose[0]; from.y = pose[1];
    to.x = path_interpol.p(proj_ind_); to.y = path_interpol.q(proj_ind_);
    visualizer_->drawLine(12341234, from, to, getFixedFrame(), "kinematic", 1, 0, 0, 1, 0.01);

    // theta_e = theta_vehicle - theta_path (orientation error)
    double theta_e = MathHelper::AngleDelta(pose[2], path_interpol.theta_p(proj_ind_));

    // if we drive backwards invert d and set theta_e to the complementary angle
    if (getDirSign() < 0.)
        theta_e = theta_e > 0.? -M_PI + theta_e : M_PI + theta_e;

    const double k = getDirSign() > 0. ? params_.k_forward() : params_.k_backward();
    const double v = max(abs(velocity_measured.linear.x), 0.3);

    // steering angle
    double phi = theta_e + atan2(k * d, v);

    if (phi == NAN)
        phi = 0.;

    phi = boost::algorithm::clamp(phi, -params_.max_steering_angle(), params_.max_steering_angle());

    double exp_factor = RobotController::exponentialSpeedControl();
    move_cmd_.setDirection(getDirSign() * (float) phi);
    move_cmd_.setVelocity(getDirSign() * (float) velocity_ * exp_factor);

#ifdef TEST_OUTPUT
    publishTestOutput(proj_ind_, d, theta_e, phi, v);
#endif

    *cmd = move_cmd_;

    return RobotController::MoveCommandStatus::OKAY;
}

void RobotController_2Steer_Stanley::publishMoveCommand(
        const MoveCommand& cmd) const {

    geometry_msgs::Twist msg;
    msg.linear.x  = cmd.getVelocity();
    msg.linear.y  = 0;
    msg.angular.z = cmd.getDirectionAngle();

    cmd_pub_.publish(msg);
}

#ifdef TEST_OUTPUT
void RobotController_2Steer_Stanley::publishTestOutput(const unsigned int waypoint, const double d,
                                                                     const double theta_e,
                                                                     const double phi, const double v) const {
    std_msgs::Float64MultiArray msg;

    msg.data.push_back((double) waypoint);
    msg.data.push_back(d);
    msg.data.push_back(theta_e);
    msg.data.push_back(phi);
    msg.data.push_back(v);

    test_pub_.publish(msg);
}
#endif


#include <gtest/gtest.h>

#include <Eigen/Dense>
#include <stan/math/prim.hpp>

#include "algorithm_state.pb.h"
#include "ls_state.pb.h"
#include "src/hierarchies/lin_reg_uni_hierarchy.h"
#include "src/hierarchies/fa_hierarchy.h"
#include "src/hierarchies/nnig_hierarchy.h"
#include "src/hierarchies/nnw_hierarchy.h"
#include "src/includes.h"
#include "src/utils/proto_utils.h"
#include "src/utils/rng.h"

TEST(nnighierarchy, draw) {
  auto hier = std::make_shared<NNIGHierarchy>();
  bayesmix::NNIGPrior prior;
  double mu0 = 5.0;
  double lambda0 = 0.1;
  double alpha0 = 2.0;
  double beta0 = 2.0;
  prior.mutable_fixed_values()->set_mean(mu0);
  prior.mutable_fixed_values()->set_var_scaling(lambda0);
  prior.mutable_fixed_values()->set_shape(alpha0);
  prior.mutable_fixed_values()->set_scale(beta0);
  hier->get_mutable_prior()->CopyFrom(prior);
  hier->initialize();

  auto hier2 = hier->clone();
  hier2->sample_prior();

  bayesmix::AlgorithmState out;
  bayesmix::AlgorithmState::ClusterState* clusval = out.add_cluster_states();
  bayesmix::AlgorithmState::ClusterState* clusval2 = out.add_cluster_states();
  hier->write_state_to_proto(clusval);
  hier2->write_state_to_proto(clusval2);

  ASSERT_TRUE(clusval->DebugString() != clusval2->DebugString());
}

TEST(nnighierarchy, sample_given_data) {
  auto hier = std::make_shared<NNIGHierarchy>();
  bayesmix::NNIGPrior prior;
  double mu0 = 5.0;
  double lambda0 = 0.1;
  double alpha0 = 2.0;
  double beta0 = 2.0;
  prior.mutable_fixed_values()->set_mean(mu0);
  prior.mutable_fixed_values()->set_var_scaling(lambda0);
  prior.mutable_fixed_values()->set_shape(alpha0);
  prior.mutable_fixed_values()->set_scale(beta0);
  hier->get_mutable_prior()->CopyFrom(prior);

  hier->initialize();

  Eigen::VectorXd datum(1);
  datum << 4.5;

  auto hier2 = hier->clone();
  hier2->add_datum(0, datum, false);
  hier2->sample_full_cond();

  bayesmix::AlgorithmState out;
  bayesmix::AlgorithmState::ClusterState* clusval = out.add_cluster_states();
  bayesmix::AlgorithmState::ClusterState* clusval2 = out.add_cluster_states();
  hier->write_state_to_proto(clusval);
  hier2->write_state_to_proto(clusval2);

  ASSERT_TRUE(clusval->DebugString() != clusval2->DebugString());
}

TEST(nnwhierarchy, draw) {
  auto hier = std::make_shared<NNWHierarchy>();
  bayesmix::NNWPrior prior;
  Eigen::Vector2d mu0;
  mu0 << 5.5, 5.5;
  bayesmix::Vector mu0_proto;
  bayesmix::to_proto(mu0, &mu0_proto);
  double lambda0 = 0.2;
  double nu0 = 5.0;
  Eigen::Matrix2d tau0 = Eigen::Matrix2d::Identity() / nu0;
  bayesmix::Matrix tau0_proto;
  bayesmix::to_proto(tau0, &tau0_proto);
  *prior.mutable_fixed_values()->mutable_mean() = mu0_proto;
  prior.mutable_fixed_values()->set_var_scaling(lambda0);
  prior.mutable_fixed_values()->set_deg_free(nu0);
  *prior.mutable_fixed_values()->mutable_scale() = tau0_proto;
  hier->get_mutable_prior()->CopyFrom(prior);
  hier->initialize();

  auto hier2 = hier->clone();
  hier2->sample_prior();

  bayesmix::AlgorithmState out;
  bayesmix::AlgorithmState::ClusterState* clusval = out.add_cluster_states();
  bayesmix::AlgorithmState::ClusterState* clusval2 = out.add_cluster_states();
  hier->write_state_to_proto(clusval);
  hier2->write_state_to_proto(clusval2);

  ASSERT_TRUE(clusval->DebugString() != clusval2->DebugString());
}

TEST(nnwhierarchy, sample_given_data) {
  auto hier = std::make_shared<NNWHierarchy>();
  bayesmix::NNWPrior prior;
  Eigen::Vector2d mu0;
  mu0 << 5.5, 5.5;
  bayesmix::Vector mu0_proto;
  bayesmix::to_proto(mu0, &mu0_proto);
  double lambda0 = 0.2;
  double nu0 = 5.0;
  Eigen::Matrix2d tau0 = Eigen::Matrix2d::Identity() / nu0;
  bayesmix::Matrix tau0_proto;
  bayesmix::to_proto(tau0, &tau0_proto);
  *prior.mutable_fixed_values()->mutable_mean() = mu0_proto;
  prior.mutable_fixed_values()->set_var_scaling(lambda0);
  prior.mutable_fixed_values()->set_deg_free(nu0);
  *prior.mutable_fixed_values()->mutable_scale() = tau0_proto;
  hier->get_mutable_prior()->CopyFrom(prior);
  hier->initialize();

  Eigen::RowVectorXd datum(2);
  datum << 4.5, 4.5;

  auto hier2 = hier->clone();
  hier2->add_datum(0, datum, false);
  hier2->sample_full_cond();

  bayesmix::AlgorithmState out;
  bayesmix::AlgorithmState::ClusterState* clusval = out.add_cluster_states();
  bayesmix::AlgorithmState::ClusterState* clusval2 = out.add_cluster_states();
  hier->write_state_to_proto(clusval);
  hier2->write_state_to_proto(clusval2);

  ASSERT_TRUE(clusval->DebugString() != clusval2->DebugString());
}

TEST(lin_reg_uni_hierarchy, state_read_write) {
  Eigen::Vector2d beta;
  beta << 2, -1;
  double sigma2 = 9;

  bayesmix::LinRegUniLSState ls;
  bayesmix::to_proto(beta, ls.mutable_regression_coeffs());
  ls.set_var(sigma2);

  bayesmix::AlgorithmState::ClusterState state;
  state.mutable_lin_reg_uni_ls_state()->CopyFrom(ls);

  LinRegUniHierarchy hier;
  hier.set_state_from_proto(state);

  ASSERT_EQ(hier.get_state().regression_coeffs, beta);
  ASSERT_EQ(hier.get_state().var, sigma2);

  bayesmix::AlgorithmState outt;
  bayesmix::AlgorithmState::ClusterState* out = outt.add_cluster_states();
  hier.write_state_to_proto(out);
  ASSERT_EQ(beta, bayesmix::to_eigen(
                      out->lin_reg_uni_ls_state().regression_coeffs()));
  ASSERT_EQ(sigma2, out->lin_reg_uni_ls_state().var());
}

TEST(lin_reg_uni_hierarchy, misc) {
  // Build data
  int n = 5;
  int dim = 2;
  Eigen::Vector2d beta_true;
  beta_true << 10.0, 10.0;
  Eigen::MatrixXd cov = Eigen::MatrixXd::Random(n, dim);  // each in U[-1,1]
  double sigma2 = 1.0;
  Eigen::VectorXd data(n);
  auto& rng = bayesmix::Rng::Instance().get();
  for (int i = 0; i < n; i++) {
    data(i) = stan::math::normal_rng(cov.row(i).dot(beta_true), sigma2, rng);
  }
  // Initialize objects
  LinRegUniHierarchy hier;
  bayesmix::LinRegUniPrior prior;
  // Create prior parameters
  Eigen::Vector2d beta0 = 0 * beta_true;
  bayesmix::Vector beta0_proto;
  bayesmix::to_proto(beta0, &beta0_proto);
  auto Lambda0 = Eigen::Matrix2d::Identity();
  bayesmix::Matrix Lambda0_proto;
  bayesmix::to_proto(Lambda0, &Lambda0_proto);
  double a0 = 2.0;
  double b0 = 1.0;
  // Initialize hierarchy
  *prior.mutable_fixed_values()->mutable_mean() = beta0_proto;
  *prior.mutable_fixed_values()->mutable_var_scaling() = Lambda0_proto;
  prior.mutable_fixed_values()->set_shape(a0);
  prior.mutable_fixed_values()->set_scale(b0);
  hier.get_mutable_prior()->CopyFrom(prior);
  hier.initialize();
  // Extract hypers for reading test
  bayesmix::AlgorithmState::HierarchyHypers out;
  hier.write_hypers_to_proto(&out);
  ASSERT_EQ(beta0, bayesmix::to_eigen(out.lin_reg_uni_state().mean()));
  ASSERT_EQ(Lambda0,
            bayesmix::to_eigen(out.lin_reg_uni_state().var_scaling()));
  ASSERT_EQ(a0, out.lin_reg_uni_state().shape());
  ASSERT_EQ(b0, out.lin_reg_uni_state().scale());
  // Add data
  for (int i = 0; i < n; i++) {
    hier.add_datum(i, data.row(i), false, cov.row(i));
  }
  // Check summary statistics
  // for (int i = 0; i < dim; i++) {
  //   for (int j = 0; j < dim; j++) {
  //     ASSERT_DOUBLE_EQ(hier.get_covar_sum_squares()(i, j),
  //                     (cov.transpose() * cov)(i, j));
  //   }
  //   ASSERT_DOUBLE_EQ(hier.get_mixed_prod()(i), (cov.transpose() * data)(i));
  // }
  // Compute and check posterior values
  hier.sample_full_cond();
  auto state = hier.get_state();
  for (int i = 0; i < dim; i++) {
    ASSERT_GT(state.regression_coeffs(i), beta0(i));
  }
}

TEST(fahierarchy, draw) {
  auto hier = std::make_shared<FAHierarchy>();
  bayesmix::FAPrior prior;
  Eigen::VectorXd mutilde(4);
  mutilde << 3.0, 3.0, 4.0, 1.0;
  bayesmix::Vector mutilde_proto;
  bayesmix::to_proto(mutilde, &mutilde_proto);
  int q = 2;
  double phi = 1.0;
  double alpha0 = 5.0;
  Eigen::VectorXd beta(4);
  beta << 3.0, 3.0, 2.0, 2.1;
  bayesmix::Vector beta_proto;
  bayesmix::to_proto(beta, &beta_proto);
  *prior.mutable_fixed_values()->mutable_mutilde() = mutilde_proto;
  prior.mutable_fixed_values()->set_phi(phi);
  prior.mutable_fixed_values()->set_alpha0(alpha0);
  prior.mutable_fixed_values()->set_q(q);
  *prior.mutable_fixed_values()->mutable_beta() = beta_proto;
  hier->get_mutable_prior()->CopyFrom(prior);
  hier->initialize();

  auto hier2 = hier->clone();
  hier2->sample_prior();

  bayesmix::AlgorithmState out;
  bayesmix::AlgorithmState::ClusterState* clusval = out.add_cluster_states();
  bayesmix::AlgorithmState::ClusterState* clusval2 = out.add_cluster_states();
  hier->write_state_to_proto(clusval);
  hier2->write_state_to_proto(clusval2);

  ASSERT_TRUE(clusval->DebugString() != clusval2->DebugString());
}

TEST(fahierarchy, draw_auto) {
  auto hier = std::make_shared<FAHierarchy>();
  bayesmix::FAPrior prior;
  Eigen::VectorXd mutilde(0);
  bayesmix::Vector mutilde_proto;
  bayesmix::to_proto(mutilde, &mutilde_proto);
  int q = 2;
  double phi = 1.0;
  double alpha0 = 5.0;
  Eigen::VectorXd beta(0);
  bayesmix::Vector beta_proto;
  bayesmix::to_proto(beta, &beta_proto);
  Eigen::MatrixXd dataset(5, 5);
  dataset << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19,
      20, 1, 5, 7, 8, 9;
  hier->set_dataset(&dataset);
  *prior.mutable_fixed_values()->mutable_mutilde() = mutilde_proto;
  prior.mutable_fixed_values()->set_phi(phi);
  prior.mutable_fixed_values()->set_alpha0(alpha0);
  prior.mutable_fixed_values()->set_q(q);
  *prior.mutable_fixed_values()->mutable_beta() = beta_proto;
  hier->get_mutable_prior()->CopyFrom(prior);
  hier->initialize();

  auto hier2 = hier->clone();
  hier2->sample_prior();

  bayesmix::AlgorithmState out;
  bayesmix::AlgorithmState::ClusterState* clusval = out.add_cluster_states();
  bayesmix::AlgorithmState::ClusterState* clusval2 = out.add_cluster_states();
  hier->write_state_to_proto(clusval);
  hier2->write_state_to_proto(clusval2);

  ASSERT_TRUE(clusval->DebugString() != clusval2->DebugString())
      << clusval->DebugString() << clusval2->DebugString();
}

TEST(fahierarchy, sample_given_data) {
  auto hier = std::make_shared<FAHierarchy>();
  bayesmix::FAPrior prior;
  Eigen::VectorXd mutilde(4);
  mutilde << 3.0, 3.0, 4.0, 1.0;
  bayesmix::Vector mutilde_proto;
  bayesmix::to_proto(mutilde, &mutilde_proto);
  int q = 2;
  double phi = 1.0;
  double alpha0 = 5.0;
  Eigen::VectorXd beta(4);
  beta << 3.0, 3.0, 2.0, 2.1;
  bayesmix::Vector beta_proto;
  bayesmix::to_proto(beta, &beta_proto);
  *prior.mutable_fixed_values()->mutable_mutilde() = mutilde_proto;
  prior.mutable_fixed_values()->set_phi(phi);
  prior.mutable_fixed_values()->set_alpha0(alpha0);
  prior.mutable_fixed_values()->set_q(q);
  *prior.mutable_fixed_values()->mutable_beta() = beta_proto;
  hier->get_mutable_prior()->CopyFrom(prior);
  Eigen::MatrixXd dataset(5, 4);
  dataset << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19,
      20;
  hier->set_dataset(&dataset);
  hier->initialize();

  auto hier2 = hier->clone();
  hier2->add_datum(0, dataset.row(0), false);
  hier2->add_datum(1, dataset.row(1), false);
  hier2->sample_full_cond();
  bayesmix::AlgorithmState out;
  bayesmix::AlgorithmState::ClusterState* clusval = out.add_cluster_states();
  bayesmix::AlgorithmState::ClusterState* clusval2 = out.add_cluster_states();
  hier->write_state_to_proto(clusval);
  hier2->write_state_to_proto(clusval2);

  ASSERT_TRUE(clusval->DebugString() != clusval2->DebugString());
}