text:
#include "joint_tracker/PrismaticJointFilter.h"

#include <Eigen/Geometry>

#include <fstream>

#include <sstream>

#include <Eigen/Eigenvalues>

#include <boost/math/distributions/chi_squared.hpp>

#include <omip_common/OMIPUtils.h>

using namespace omip;
using namespace MatrixWrapper;
using namespace BFL;

// Dimensions of the system state of the filter that tracks a prismatic joint: orientation (2 values), joint variable, and joint velocity
#define PRISM_STATE_DIM 4
#define MEAS_DIM 6

/**
 * EKF internal state:
 *
 * x(1) =  PrismJointOrientation_phi
 * x(2) =  PrismJointOrientation_theta
 * PrismJointOrientation is represented in spherical coords
 * NOTE: I try to keep theta between 0 and pi and phi between 0 and two pi
 * x(4) = PrismJointVariable
 * x(5) = PrismJointVariable_d
 *
 * EKF measurement:
 *
 * m(1) = TwistLinearPart_x
 * m(2) = TwistLinearPart_y
 * m(3) = TwistLinearPart_z
 * m(4) = TwistAngularPart_x
 * m(5) = TwistAngularPart_y
 * m(6) = TwistAngularPart_z
 */

PrismaticJointFilter::PrismaticJointFilter():
    JointFilter(),
    _sys_PDF(NULL),
    _sys_MODEL(NULL),
    _meas_PDF(NULL),
    _meas_MODEL(NULL),
    _ekf(NULL),
    _sigma_delta_meas_uncertainty_linear(-1)
{

}

void PrismaticJointFilter::setCovarianceDeltaMeasurementLinear(double sigma_delta_meas_uncertainty_linear)
{
    this->_sigma_delta_meas_uncertainty_linear = sigma_delta_meas_uncertainty_linear;
}

void PrismaticJointFilter::initialize()
{
    JointFilter::initialize();

//    std::cout << "this->_srb_current_pose_in_sff" << std::endl;
//    std::cout << this->_srb_current_pose_in_sf << std::endl;
//    std::cout << this->_srb_current_pose_in_sf.exp(1e-9).toHomogeneousMatrix() << std::endl;
//    std::cout << "this->_srb_initial_pose_in_rrbf" << std::endl;
//    std::cout << this->_srb_initial_pose_in_rrbf << std::endl;
//    std::cout << this->_srb_initial_pose_in_rrbf.exp(1e-9).toHomogeneousMatrix() << std::endl;
//    std::cout << "this->_current_delta_pose_in_rrbf" << std::endl;
//    std::cout << this->_current_delta_pose_in_rrbf << std::endl;
//    std::cout << this->_current_delta_pose_in_rrbf.exp(1e-9).toHomogeneousMatrix() << std::endl;

    this->_joint_orientation = Eigen::Vector3d(
                this->_current_delta_pose_in_rrbf.vx(),
                this->_current_delta_pose_in_rrbf.vy(),
                this->_current_delta_pose_in_rrbf.vz());
    this->_joint_state = this->_joint_orientation.norm();

    this->_joint_states_all.push_back(this->_joint_state);
    //this->_joint_velocity = this->_joint_state/(this->_loop_period_ns/1e9);
    // Setting it to 0 is better.
    // The best approximation would be to (this->_rev_variable/num_steps_to_rev_variable)/(this->_loop_period_ns/1e9)
    // but we don't know how many steps passed since we estimated the first time the joint variable
    this->_joint_velocity = 0.0;
    this->_joint_orientation.normalize();

    // The position of the prismatic joint is the centroid of the features that belong to the second rigid body (in RBF)
    Eigen::Displacementd current_ref_pose_displ = this->_rrb_current_pose_in_sf.exp(1e-12);
    Eigen::Affine3d current_ref_pose;
    current_ref_pose.matrix() = current_ref_pose_displ.toHomogeneousMatrix();
    this->_joint_position = current_ref_pose.inverse() * this->_srb_centroid_in_sf;

    this->_initializeSystemModel();
    this->_initializeMeasurementModel();
    this->_initializeEKF();
}

void PrismaticJointFilter::_initializeSystemModel()
{
    // create SYSTEM MODEL
    Matrix A(PRISM_STATE_DIM, PRISM_STATE_DIM);
    A = 0.;
    for (unsigned int i = 1; i <= PRISM_STATE_DIM; i++)
    {
        A(i, i) = 1.0;
    }
    A(3, 4) = this->_loop_period_ns/1e9; //Adding the velocity (times the time) to the position of the joint variable

    ColumnVector sys_noise_MU(PRISM_STATE_DIM);
    sys_noise_MU = 0;

    SymmetricMatrix sys_noise_COV(PRISM_STATE_DIM);
    sys_noise_COV = 0.0;
    sys_noise_COV(1, 1) = this->_sigma_sys_noise_phi* (std::pow((this->_loop_period_ns/1e9),3) / 3.0);
    sys_noise_COV(2, 2) = this->_sigma_sys_noise_theta* (std::pow((this->_loop_period_ns/1e9),3) / 3.0);
    sys_noise_COV(3, 3) = this->_sigma_sys_noise_jv* (std::pow((this->_loop_period_ns/1e9),3) / 3.0);
    sys_noise_COV(4, 4) = this->_sigma_sys_noise_jvd*(this->_loop_period_ns/1e9);

    // Initialize System Model
    Gaussian system_uncertainty_PDF(sys_noise_MU, sys_noise_COV);
    this->_sys_PDF = new LinearAnalyticConditionalGaussian( A, system_uncertainty_PDF);
    this->_sys_MODEL = new LinearAnalyticSystemModelGaussianUncertainty( this->_sys_PDF);
}

void PrismaticJointFilter::_initializeMeasurementModel()
{
    // create MEASUREMENT MODEL
    ColumnVector meas_noise_MU(MEAS_DIM);
    meas_noise_MU = 0.0;
    SymmetricMatrix meas_noise_COV(MEAS_DIM);
    meas_noise_COV = 0.0;
    for (unsigned int i = 1; i <= MEAS_DIM; i++)
        meas_noise_COV(i, i) = this->_sigma_meas_noise;

    Gaussian meas_uncertainty_PDF(meas_noise_MU, meas_noise_COV);

    this->_meas_PDF = new NonLinearPrismaticMeasurementPdf(meas_uncertainty_PDF);
    this->_meas_MODEL = new AnalyticMeasurementModelGaussianUncertainty(this->_meas_PDF);

    // create MEASUREMENT MODEL Force Torque sensor
    ColumnVector meas_noise_ft_MU(6);
    meas_noise_ft_MU = 0;
    SymmetricMatrix meas_noise_ft_COV(6);
    meas_noise_ft_COV = 0;
    for (unsigned int i=1; i<=6; i++)
        meas_noise_ft_COV(i,i) = 1;
    Gaussian meas_uncertainty_ft_PDF(meas_noise_ft_MU, meas_noise_ft_COV);

    //    Matrix Himu(3,6);  Himu = 0;
    //    Himu(1,4) = 1;    Himu(2,5) = 1;    Himu(3,6) = 1;
    //    imu_meas_pdf_   = new LinearAnalyticConditionalGaussian(Himu, measurement_Uncertainty_Imu);
    //    imu_meas_model_ = new LinearAnalyticMeasurementModelGaussianUncertainty(imu_meas_pdf_);
}

void PrismaticJointFilter::_initializeEKF()
{
    ColumnVector prior_MU(PRISM_STATE_DIM);
    prior_MU = 0.0;

    SymmetricMatrix prior_COV(PRISM_STATE_DIM);
    prior_COV = 0.0;

    prior_MU(1) = atan2(this->_joint_orientation.y() , this->_joint_orientation.x());

    // NOTE: I make phi between 0 and 2pi
    //    if(prior_MU(1) < 0.0)
    //    {
    //        prior_MU(1) += 2*M_PI;
    //    }

    prior_MU(2) = acos(this->_joint_orientation.z());
    prior_MU(3) = this->_joint_state;
    prior_MU(4) = this->_joint_velocity;

    ROS_INFO_STREAM_NAMED( "PrismaticJointFilter::_initializeEKF",
                           "Prismatic initial state (phi, theta, jv, jv_dot): " << prior_MU(1) << " " << prior_MU(2) << " " << prior_MU(3) << " " << prior_MU(4));

    for (int i = 1; i <= PRISM_STATE_DIM; i++)
    {
        prior_COV(i, i) = this->_prior_cov_vel;
    }

    Gaussian prior_PDF(prior_MU, prior_COV);

    this->_ekf = new ExtendedKalmanFilter(&prior_PDF);
}

PrismaticJointFilter::~PrismaticJointFilter()
{
    if (this->_sys_PDF)
    {
        delete this->_sys_PDF;
        this->_sys_PDF = NULL;
    }
    if (this->_sys_MODEL)
    {
        delete this->_sys_MODEL;
        this->_sys_MODEL = NULL;
    }
    if (this->_meas_PDF)
    {
        delete this->_meas_PDF;
        this->_meas_PDF = NULL;
    }
    if (this->_meas_MODEL)
    {
        delete this->_meas_MODEL;
        this->_meas_MODEL = NULL;
    }
    if (this->_ekf)
    {
        delete this->_ekf;
        this->_ekf = NULL;
    }
}

PrismaticJointFilter::PrismaticJointFilter(const PrismaticJointFilter &prismatic_joint) :
    JointFilter(prismatic_joint)
{
    this->_sigma_delta_meas_uncertainty_linear = prismatic_joint._sigma_delta_meas_uncertainty_linear;
    this->_sys_PDF = new LinearAnalyticConditionalGaussian( *(prismatic_joint._sys_PDF));
    this->_sys_MODEL = new LinearAnalyticSystemModelGaussianUncertainty( *(prismatic_joint._sys_MODEL));
    this->_meas_PDF = new NonLinearPrismaticMeasurementPdf( *(prismatic_joint._meas_PDF));
    this->_meas_MODEL = new AnalyticMeasurementModelGaussianUncertainty( *(prismatic_joint._meas_MODEL));
    this->_ekf = new ExtendedKalmanFilter(*(prismatic_joint._ekf));
}

void PrismaticJointFilter::predictState(double time_interval_ns)
{
    // Estimate the new cov matrix depending on the time elapsed between the previous and the current measurement
    SymmetricMatrix sys_noise_COV(PRISM_STATE_DIM);
    sys_noise_COV = 0.0;
    sys_noise_COV(1, 1) = this->_sigma_sys_noise_phi* (std::pow((time_interval_ns/1e9),3) / 3.0);
    sys_noise_COV(2, 2) = this->_sigma_sys_noise_theta* (std::pow((time_interval_ns/1e9),3) / 3.0);
    sys_noise_COV(3, 3) = this->_sigma_sys_noise_jv* (std::pow((time_interval_ns/1e9),3) / 3.0);
    sys_noise_COV(4, 4) = this->_sigma_sys_noise_jvd*(time_interval_ns/1e9);

    // Estimate the new updating matrix which also depends on the time elapsed between the previous and the current measurement
    // x(t+1) = x(t) + v(t) * delta_t
    Matrix A(PRISM_STATE_DIM, PRISM_STATE_DIM);
    A = 0.;
    for (unsigned int i = 1; i <= PRISM_STATE_DIM; i++)
    {
        A(i, i) = 1.0;
    }
    A(3, 4) = time_interval_ns/1e9; //Adding the velocity (times the time) to the position of the joint variable

    this->_sys_PDF->MatrixSet(0, A);
    this->_sys_PDF->AdditiveNoiseSigmaSet(sys_noise_COV);
    this->_ekf->Update(this->_sys_MODEL);
}

geometry_msgs::TwistWithCovariance PrismaticJointFilter::getPredictedSRBDeltaPoseWithCovInSensorFrame()
{
    Eigen::Matrix<double, 6, 6> adjoint;
    computeAdjoint(this->_rrb_current_pose_in_sf, adjoint);
    Eigen::Twistd predicted_delta_pose_in_sf = adjoint*this->_predicted_delta_pose_in_rrbf;

    geometry_msgs::TwistWithCovariance hypothesis;

    hypothesis.twist.linear.x = predicted_delta_pose_in_sf.vx();
    hypothesis.twist.linear.y = predicted_delta_pose_in_sf.vy();
    hypothesis.twist.linear.z = predicted_delta_pose_in_sf.vz();
    hypothesis.twist.angular.x = predicted_delta_pose_in_sf.rx();
    hypothesis.twist.angular.y = predicted_delta_pose_in_sf.ry();
    hypothesis.twist.angular.z = predicted_delta_pose_in_sf.rz();

    // This call gives me the covariance of the predicted measurement: the relative pose between RBs
    ColumnVector empty;
    ColumnVector state_updated_state = this->_ekf->PostGet()->ExpectedValueGet();
    SymmetricMatrix measurement_cov = this->_meas_MODEL->CovarianceGet(empty, state_updated_state);
    for(int i=0; i<6; i++)
    {
        for(int j=0; j<6; j++)
        {
             hypothesis.covariance[6 * i + j] = measurement_cov(i+1,j+1);
        }
    }

    return hypothesis;
}

geometry_msgs::TwistWithCovariance PrismaticJointFilter::getPredictedSRBVelocityWithCovInSensorFrame()
{
    Eigen::Matrix<double, 6, 6> adjoint;
    computeAdjoint(this->_rrb_current_pose_in_sf, adjoint);
    Eigen::Twistd predicted_delta_pose_in_sf = adjoint*(this->_predicted_delta_pose_in_rrbf/(_loop_period_ns/1e9));

    geometry_msgs::TwistWithCovariance hypothesis;

    hypothesis.twist.linear.x = predicted_delta_pose_in_sf.vx();
    hypothesis.twist.linear.y = predicted_delta_pose_in_sf.vy();
    hypothesis.twist.linear.z = predicted_delta_pose_in_sf.vz();
    hypothesis.twist.angular.x = predicted_delta_pose_in_sf.rx();
    hypothesis.twist.angular.y = predicted_delta_pose_in_sf.ry();
    hypothesis.twist.angular.z = predicted_delta_pose_in_sf.rz();

    // This call gives me the covariance of the predicted measurement: the relative pose between RBs
    ColumnVector empty;
    ColumnVector state_updated_state = this->_ekf->PostGet()->ExpectedValueGet();
    SymmetricMatrix measurement_cov = this->_meas_MODEL->CovarianceGet(empty, state_updated_state);
    for(int i=0; i<6; i++)
    {
        for(int j=0; j<6; j++)
        {
             hypothesis.covariance[6 * i + j] = measurement_cov(i+1,j+1);
        }
    }

    return hypothesis;
}

void PrismaticJointFilter::predictMeasurement()
{
    ColumnVector empty;
    ColumnVector state_updated_state = this->_ekf->PostGet()->ExpectedValueGet();

    ROS_DEBUG_STREAM_NAMED( "PrismaticJointFilter::UpdateJointParameters",
                            "Prismatic state after state update: " << state_updated_state(1) << " " << state_updated_state(2) << " " << state_updated_state(3) << " " << state_updated_state(4));

    ColumnVector predicted_delta_pose_in_rrbf = this->_meas_MODEL->PredictionGet(empty, state_updated_state);

    this->_predicted_delta_pose_in_rrbf = Eigen::Twistd( predicted_delta_pose_in_rrbf(4), predicted_delta_pose_in_rrbf(5),
                                                         predicted_delta_pose_in_rrbf(6), predicted_delta_pose_in_rrbf(1),
                                                         predicted_delta_pose_in_rrbf(2), predicted_delta_pose_in_rrbf(3));

    Eigen::Displacementd predicted_delta = this->_predicted_delta_pose_in_rrbf.exp(1e-20);
    Eigen::Displacementd T_rrbf_srbf_t0 = this->_srb_initial_pose_in_rrbf.exp(1.0e-20);
    Eigen::Displacementd T_rrbf_srbf_t_next = predicted_delta * T_rrbf_srbf_t0;

    this->_srb_predicted_pose_in_rrbf = T_rrbf_srbf_t_next.log(1.0e-20);
}

void PrismaticJointFilter::correctState()
{
    // New 26.8.2016 -> There is small rotations in the reference frame that cause the prismatic joint to rotate
    // We eliminate this: we search for the closest motion without rotation that resembles the relative motion
    Eigen::Matrix4d  _srb_current_pose_in_rrbf_matrix;
    Twist2TransformMatrix( _srb_current_pose_in_rrbf, _srb_current_pose_in_rrbf_matrix);

    //Then set the rotation to the identity
    for(int i=0; i<3; i++)
    {
        for(int j=0; j<3; j++)
        {
            if(i==j)
            {
                _srb_current_pose_in_rrbf_matrix(i,j) = 1;
            }else{
                _srb_current_pose_in_rrbf_matrix(i,j) = 0;
            }
        }
    }

    Eigen::Twistd  _srb_current_pose_in_rrbf_no_rot;
    TransformMatrix2Twist(_srb_current_pose_in_rrbf_matrix, _srb_current_pose_in_rrbf_no_rot);

    Eigen::Twistd _current_delta_pose_in_rrbf_no_rot = (_srb_current_pose_in_rrbf_no_rot.exp(1e-12)*_srb_initial_pose_in_rrbf.exp(1e-12).inverse()).log(1e-12);

    ColumnVector rb2_measured_delta_relative_pose_cv(MEAS_DIM);
    rb2_measured_delta_relative_pose_cv = 0.;
    rb2_measured_delta_relative_pose_cv(1) = _current_delta_pose_in_rrbf_no_rot.vx();
    rb2_measured_delta_relative_pose_cv(2) = _current_delta_pose_in_rrbf_no_rot.vy();
    rb2_measured_delta_relative_pose_cv(3) = _current_delta_pose_in_rrbf_no_rot.vz();
    rb2_measured_delta_relative_pose_cv(4) = _current_delta_pose_in_rrbf_no_rot.rx();
    rb2_measured_delta_relative_pose_cv(5) = _current_delta_pose_in_rrbf_no_rot.ry();
    rb2_measured_delta_relative_pose_cv(6) = _current_delta_pose_in_rrbf_no_rot.rz();

    // Update the uncertainty on the measurement
    // NEW: The uncertainty on the measurement (the delta motion of the second rigid body wrt the reference rigid body) will be large if the measurement
    // is small and small if the measurement is large
    double meas_uncertainty_factor = 1.0 / (1.0 - exp(-this->_current_delta_pose_in_rrbf.norm()/this->_sigma_delta_meas_uncertainty_linear));

    // Truncate the factor
    meas_uncertainty_factor = std::min(meas_uncertainty_factor, 1e6);

    SymmetricMatrix current_delta_pose_cov_in_rrbf(6);
    for (unsigned int i = 0; i < 6; i++)
    {
        for (unsigned int j = 0; j < 6; j++)
        {
            current_delta_pose_cov_in_rrbf(i + 1, j + 1) = _current_delta_pose_cov_in_rrbf(i, j);
        }
    }
    this->_meas_PDF->AdditiveNoiseSigmaSet(current_delta_pose_cov_in_rrbf * meas_uncertainty_factor);

    this->_ekf->Update(this->_meas_MODEL, rb2_measured_delta_relative_pose_cv);

    ColumnVector updated_state = this->_ekf->PostGet()->ExpectedValueGet();

    // The joint is defined in the space of the relative motion
    this->_joint_orientation(0) = sin(updated_state(2)) * cos(updated_state(1));
    this->_joint_orientation(1) = sin(updated_state(2)) * sin(updated_state(1));
    this->_joint_orientation(2) = cos(updated_state(2));

    SymmetricMatrix updated_uncertainty = this->_ekf->PostGet()->CovarianceGet();

    this->_joint_state = updated_state(3);
    this->_uncertainty_joint_state = updated_uncertainty(3,3);
    this->_joint_velocity = updated_state(4);
    this->_uncertainty_joint_velocity = updated_uncertainty(4,4);

    this->_joint_orientation_phi = updated_state(1);
    this->_joint_orientation_theta = updated_state(2);
    this->_uncertainty_joint_orientation_phitheta(0,0) = updated_uncertainty(1, 1);
    this->_uncertainty_joint_orientation_phitheta(0,1) = updated_uncertainty(1, 2);
    this->_uncertainty_joint_orientation_phitheta(1,0) = updated_uncertainty(1, 2);
    this->_uncertainty_joint_orientation_phitheta(1,1) = updated_uncertainty(2, 2);
}

void PrismaticJointFilter::estimateMeasurementHistoryLikelihood()
{
    double accumulated_error = 0.;

    double p_one_meas_given_model_params = 0;
    double p_all_meas_given_model_params = 0;

    double sigma_translation = 0.05;
    double sigma_rotation = 0.2;

    ColumnVector state_updated_state = this->_ekf->PostGet()->ExpectedValueGet();
    double phi = state_updated_state(1);
    double theta = state_updated_state(2);

    this->_joint_states_all.push_back(state_updated_state(3));

    // This vector is in the ref RB with the initial relative transformation to the second RB
    Eigen::Vector3d prism_joint_translation_unitary = Eigen::Vector3d(cos(phi) * sin(theta), sin(phi) * sin(theta), cos(theta));
    prism_joint_translation_unitary.normalize();

    double frame_counter = 0.;
    size_t trajectory_length = this->_delta_poses_in_rrbf.size();
    size_t amount_samples = std::min(trajectory_length, (size_t)this->_likelihood_sample_num);
    double delta_idx_samples = (double)std::max(1., (double)trajectory_length/(double)this->_likelihood_sample_num);
    size_t current_idx = 0;

    double max_norm_of_deltas = 0;

    // Estimation of the quality of the parameters of the prismatic joint
    // If the joint is prismatic and the parameters are accurate, the joint axis orientation should not change over time
    // That means that the current orientation, multiplied by the amount of prismatic displacement at each time step, should provide the delta in the relative
    // pose between ref and second rb at each time step
    // We test amount_samples of the relative trajectory
    for (size_t sample_idx = 0; sample_idx < amount_samples; sample_idx++)
    {
        current_idx = boost::math::round(sample_idx*delta_idx_samples);
        Eigen::Displacementd rb2_last_delta_relative_displ = this->_delta_poses_in_rrbf.at(current_idx).exp(1e-12);

        max_norm_of_deltas = std::max(this->_delta_poses_in_rrbf.at(current_idx).norm(), max_norm_of_deltas);

        Eigen::Vector3d rb2_last_delta_relative_translation = rb2_last_delta_relative_displ.getTranslation();
        Eigen::Quaterniond rb2_last_delta_relative_rotation = Eigen::Quaterniond(rb2_last_delta_relative_displ.qw(),
                                                                                rb2_last_delta_relative_displ.qx(),
                                                                                rb2_last_delta_relative_displ.qy(),
                                                                                rb2_last_delta_relative_displ.qz());

        Eigen::Vector3d prism_joint_translation = this->_joint_states_all.at(current_idx) * prism_joint_translation_unitary;
        Eigen::Displacementd rb2_last_delta_relative_displ_prism_hyp = Eigen::Twistd(0.,
                                                                                0.,
                                                                                0.,
                                                                                prism_joint_translation.x(),
                                                                                prism_joint_translation.y(),
                                                                                prism_joint_translation.z()).exp(1e-12);
        Eigen::Vector3d rb2_last_delta_relative_translation_prism_hyp = rb2_last_delta_relative_displ_prism_hyp.getTranslation();
        Eigen::Quaterniond rb2_last_delta_relative_rotation_prism_hyp = Eigen::Quaterniond(rb2_last_delta_relative_displ_prism_hyp.qw(),
                                                                                rb2_last_delta_relative_displ_prism_hyp.qx(),
                                                                                rb2_last_delta_relative_displ_prism_hyp.qy(),
                                                                                rb2_last_delta_relative_displ_prism_hyp.qz());

        // Distance proposed by park and okamura in "Kinematic calibration using the product of exponentials formula"
        double translation_error = (rb2_last_delta_relative_translation - rb2_last_delta_relative_translation_prism_hyp).norm();
        // Actually both rotations should be zero because a prismatic joint constraints the rotation
        Eigen::Quaterniond rotation_error = rb2_last_delta_relative_rotation.inverse() * rb2_last_delta_relative_rotation_prism_hyp;
        double rotation_error_angle = Eigen::Displacementd(0., 0., 0., rotation_error.w(),rotation_error.x(), rotation_error.y(),rotation_error.z()).log(1e-12).norm();

        accumulated_error += translation_error + fabs(rotation_error_angle);

        p_one_meas_given_model_params = (1.0/(sigma_translation*sqrt(2.0*M_PI)))*exp((-1.0/2.0)*pow(translation_error/sigma_translation, 2)) *
                (1.0/(sigma_rotation*sqrt(2.0*M_PI)))*exp((-1.0/2.0)*pow(rotation_error_angle/sigma_rotation, 2));

        p_all_meas_given_model_params += (p_one_meas_given_model_params/(double)amount_samples);

        frame_counter++;
    }

    if(frame_counter != 0)
    {
        this->_measurements_likelihood = p_all_meas_given_model_params;
    }else{
        this->_measurements_likelihood = 1e-5;
    }
}

void PrismaticJointFilter::estimateUnnormalizedModelProbability()
{
    this->_unnormalized_model_probability = _model_prior_probability*_measurements_likelihood;
}

geometry_msgs::TwistWithCovariance PrismaticJointFilter::getPredictedSRBPoseWithCovInSensorFrame()
{
    Eigen::Twistd delta_rrb_in_sf = this->_rrb_current_vel_in_sf*(this->_loop_period_ns/1e9);
    Eigen::Twistd rrb_next_pose_in_sf = (delta_rrb_in_sf.exp(1e-12)*this->_rrb_current_pose_in_sf.exp(1e-12)).log(1e-12);

    //Eigen::Twistd rrb_next_pose_in_sf = this->_rrb_current_pose_in_sf + this->_rrb_current_vel_in_sf;
    Eigen::Displacementd T_sf_rrbf_next = rrb_next_pose_in_sf.exp(1e-12);
    Eigen::Displacementd T_rrbf_srbf_next = this->_srb_predicted_pose_in_rrbf.exp(1e-12);

    Eigen::Displacementd T_sf_srbf_next = T_rrbf_srbf_next*T_sf_rrbf_next;

    Eigen::Twistd srb_next_pose_in_sf = T_sf_srbf_next.log(1e-12);

    geometry_msgs::TwistWithCovariance hypothesis;

    hypothesis.twist.linear.x = srb_next_pose_in_sf.vx();
    hypothesis.twist.linear.y = srb_next_pose_in_sf.vy();
    hypothesis.twist.linear.z = srb_next_pose_in_sf.vz();
    hypothesis.twist.angular.x = srb_next_pose_in_sf.rx();
    hypothesis.twist.angular.y = srb_next_pose_in_sf.ry();
    hypothesis.twist.angular.z = srb_next_pose_in_sf.rz();

    // This call gives me the covariance of the predicted measurement: the relative pose between RBs
    ColumnVector empty;
    ColumnVector state_updated_state = this->_ekf->PostGet()->ExpectedValueGet();
    SymmetricMatrix measurement_cov = this->_meas_MODEL->CovarianceGet(empty, state_updated_state);
    Eigen::Matrix<double,6,6> measurement_cov_eigen;
    for(int i=0; i<6; i++)
    {
        for(int j=0; j<6; j++)
        {
            measurement_cov_eigen(i,j) = measurement_cov(i+1,j+1);
        }
    }
    // I need the covariance of the absolute pose of the second RB, so I add the cov of the relative pose to the
    // cov of the reference pose. I need to "move" the second covariance to align it to the reference frame (see Barfoot)
    Eigen::Matrix<double,6,6> tranformed_cov;
    adjointXcovXadjointT(_rrb_current_pose_in_sf, measurement_cov_eigen, tranformed_cov);
    Eigen::Matrix<double,6,6> new_pose_covariance = this->_rrb_pose_cov_in_sf + tranformed_cov;
    for (unsigned int i = 0; i < 6; i++)
    {
        for (unsigned int j = 0; j < 6; j++)
        {
            hypothesis.covariance[6 * i + j] = new_pose_covariance(i, j);
        }
    }

#ifdef PUBLISH_PREDICTED_POSE_AS_PWC
    // This is used to visualize the predictions based on the joint hypothesis
    geometry_msgs::PoseWithCovarianceStamped pose_with_cov_stamped;
    pose_with_cov_stamped.header.stamp = ros::Time::now();
    pose_with_cov_stamped.header.frame_id = "camera_rgb_optical_frame";

    Eigen::Displacementd displ_from_twist = srb_next_pose_in_sf.exp(1e-12);
    pose_with_cov_stamped.pose.pose.position.x = displ_from_twist.x();
    pose_with_cov_stamped.pose.pose.position.y = displ_from_twist.y();
    pose_with_cov_stamped.pose.pose.position.z = displ_from_twist.z();
    pose_with_cov_stamped.pose.pose.orientation.x = displ_from_twist.qx();
    pose_with_cov_stamped.pose.pose.orientation.y = displ_from_twist.qy();
    pose_with_cov_stamped.pose.pose.orientation.z = displ_from_twist.qz();
    pose_with_cov_stamped.pose.pose.orientation.w = displ_from_twist.qw();

    for (unsigned int i = 0; i < 6; i++)
        for (unsigned int j = 0; j < 6; j++)
            pose_with_cov_stamped.pose.covariance[6 * i + j] = new_pose_covariance(i, j);
    _predicted_next_pose_publisher.publish(pose_with_cov_stamped);
#endif

    return hypothesis;
}

std::vector<visualization_msgs::Marker> PrismaticJointFilter::getJointMarkersInRRBFrame() const
{
    // The class variable _prism_joint_orientation (also _uncertainty_o_phi and _uncertainty_o_theta) are defined in the frame of the
    // ref RB with the initial relative transformation to the second RB
    // We want the variables to be in the ref RB frame, without the initial relative transformation to the second RB
    Eigen::Vector3d prism_joint_ori_in_ref_rb = this->_joint_orientation;

    std::vector<visualization_msgs::Marker> prismatic_markers;
    // AXIS MARKER 1 -> The axis ///////////////////////////////////////////////////////////////////////////////////////////////////////////
    visualization_msgs::Marker axis_orientation_marker;
    axis_orientation_marker.ns = "kinematic_structure";
    axis_orientation_marker.action = visualization_msgs::Marker::ADD;
    axis_orientation_marker.type = visualization_msgs::Marker::ARROW;
    axis_orientation_marker.id = 3 * this->_joint_id;
    axis_orientation_marker.scale.x = JOINT_AXIS_AND_VARIABLE_MARKER_RADIUS;
    axis_orientation_marker.scale.y = 0.f;
    axis_orientation_marker.scale.z = 0.f;
    axis_orientation_marker.color.r = 0.f;
    axis_orientation_marker.color.g = 1.f;
    axis_orientation_marker.color.b = 0.f;
    axis_orientation_marker.color.a = 1.f;
    // Estimate position from supporting features:
    //    Eigen::Displacementd current_ref_pose_displ = this->_rrb_current_pose_in_sf.exp(1e-12);
    //    Eigen::Affine3d current_ref_pose;
    //    current_ref_pose.matrix() = current_ref_pose_displ.toHomogeneousMatrix();
    //    =current_ref_pose.inverse() * this->_srb_centroid_in_sf;
    Eigen::Vector3d second_centroid_relative_to_ref_body = this->getJointPositionInRRBFrame();
    Eigen::Vector3d position1 = second_centroid_relative_to_ref_body - this->_joint_state * prism_joint_ori_in_ref_rb;
    geometry_msgs::Point pt1;
    pt1.x = position1.x();
    pt1.y = position1.y();
    pt1.z = position1.z();
    axis_orientation_marker.points.push_back(pt1);
    Eigen::Vector3d position2 = second_centroid_relative_to_ref_body + this->_joint_state * prism_joint_ori_in_ref_rb;
    geometry_msgs::Point pt2;
    pt2.x = position2.x();
    pt2.y = position2.y();
    pt2.z = position2.z();
    axis_orientation_marker.points.push_back(pt2);

    prismatic_markers.push_back(axis_orientation_marker);

    // AXIS MARKER 2 -> Proportional to joint state ///////////////////////////////////////////////////////////////////////////////////////////////////////////
    axis_orientation_marker.points.clear();
    axis_orientation_marker.id = 3 * this->_joint_id + 1;
    axis_orientation_marker.scale.x = JOINT_AXIS_MARKER_RADIUS;
    // Estimate position from supporting features:
    Eigen::Vector3d position12 = second_centroid_relative_to_ref_body - 100 * prism_joint_ori_in_ref_rb;
    geometry_msgs::Point pt12;
    pt12.x = position12.x();
    pt12.y = position12.y();
    pt12.z = position12.z();
    axis_orientation_marker.points.push_back(pt12);
    Eigen::Vector3d position22 = second_centroid_relative_to_ref_body + 100 * prism_joint_ori_in_ref_rb;
    geometry_msgs::Point pt22;
    pt22.x = position22.x();
    pt22.y = position22.y();
    pt22.z = position22.z();
    axis_orientation_marker.points.push_back(pt22);

    prismatic_markers.push_back(axis_orientation_marker);

    // AXIS MARKER 3 -> Text with the joint state ///////////////////////////////////////////////////////////////////////////////////////////////////////////
    axis_orientation_marker.points.clear();
    axis_orientation_marker.id = 3 * this->_joint_id + 2;
    axis_orientation_marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
    axis_orientation_marker.scale.z = JOINT_VALUE_TEXT_SIZE;
    std::ostringstream oss_joint_value;
    oss_joint_value << std::fixed<< std::setprecision(1) << 100*this->_joint_state;
    axis_orientation_marker.text = oss_joint_value.str() + std::string(" cm");
    axis_orientation_marker.pose.position.x = second_centroid_relative_to_ref_body.x();
    axis_orientation_marker.pose.position.y = second_centroid_relative_to_ref_body.y();
    axis_orientation_marker.pose.position.z = second_centroid_relative_to_ref_body.z();
    axis_orientation_marker.pose.orientation.x = 0;
    axis_orientation_marker.pose.orientation.y = 0;
    axis_orientation_marker.pose.orientation.z = 0;
    axis_orientation_marker.pose.orientation.w = 1;

    prismatic_markers.push_back(axis_orientation_marker);

    // UNCERTAINTY MARKERS ///////////////////////////////////////////////////////////////////////////////////////////////////////

    // This first marker is just to match the number of markers of the revolute joint, but prismatic joint has no position
    visualization_msgs::Marker axis_position_uncertainty_marker;
    axis_position_uncertainty_marker.pose.position.x = second_centroid_relative_to_ref_body.x();
    axis_position_uncertainty_marker.pose.position.y = second_centroid_relative_to_ref_body.y();
    axis_position_uncertainty_marker.pose.position.z = second_centroid_relative_to_ref_body.z();
    axis_position_uncertainty_marker.ns = "kinematic_structure_uncertainty";
    axis_position_uncertainty_marker.type = visualization_msgs::Marker::SPHERE;
    axis_position_uncertainty_marker.action = visualization_msgs::Marker::DELETE;
    axis_position_uncertainty_marker.id = 3 * this->_joint_id ;

    prismatic_markers.push_back(axis_position_uncertainty_marker);

    visualization_msgs::Marker prism_axis_unc_cone1;
    prism_axis_unc_cone1.type = visualization_msgs::Marker::MESH_RESOURCE;
    prism_axis_unc_cone1.action = visualization_msgs::Marker::ADD;
    prism_axis_unc_cone1.mesh_resource = "package://joint_tracker/meshes/cone.stl";
    prism_axis_unc_cone1.pose.position.x = second_centroid_relative_to_ref_body.x();
    prism_axis_unc_cone1.pose.position.y = second_centroid_relative_to_ref_body.y();
    prism_axis_unc_cone1.pose.position.z = second_centroid_relative_to_ref_body.z();

    // NOTE:
    // Estimation of the uncertainty cones -----------------------------------------------
    // We estimate the orientation of the prismatic axis in spherical coordinates (r=1 always)
    // We estimate phi: angle from the x axis to the projection of the prismatic joint axis to the xy plane
    // We estimate theta: angle from the z axis to the prismatic joint axis
    // [TODO: phi and theta are in the reference rigid body. Do we need to transform it (adding uncertainty) to the reference frame?]
    // The covariance of phi and theta (a 2x2 matrix) gives us the uncertainty of the orientation of the joint
    // If we look from the joint axis, we would see an ellipse given by this covariance matrix [http://www.visiondummy.com/2014/04/draw-error-ellipse-representing-covariance-matrix/]
    // But in RVIZ we can only set the scale of our cone mesh in x and y, not in a different axis
    // The first thing is then to estimate the direction of the major and minor axis of the ellipse and their size
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> eigensolver(this->_uncertainty_joint_orientation_phitheta);

    // The sizes of the major and minor axes of the ellipse are given by the eigenvalues and the chi square distribution P(x<critical_value) = confidence_value
    // For 50% of confidence on the cones shown
    double confidence_value = 0.5;
    boost::math::chi_squared chi_sq_dist(2);
    double critical_value = boost::math::quantile(chi_sq_dist, confidence_value);
    double major_axis_length = 2*eigensolver.eigenvalues()[1]*std::sqrt(critical_value);
    double minor_axis_length = 2*eigensolver.eigenvalues()[0]*std::sqrt(critical_value);

    // If z is pointing in the direction of the joint, the angle between the x axis and the largest axis of the ellipse is arctg(v1_y/v1_x) where v1 is the eigenvector of
    // largest eigenvalue (the last column in the matrix returned by eigenvectors() in eigen library):
    double alpha = atan2(eigensolver.eigenvectors().col(1)[1],eigensolver.eigenvectors().col(1)[0]);
    // We create a rotation around the z axis to align the ellipse to have the major axis aligned to the x-axis (we UNDO the rotation of the ellipse):
    Eigen::AngleAxisd init_rot(-alpha, Eigen::Vector3d::UnitZ());

    // Now I need to rotate the mesh so that:
    // 1) The z axis of the mesh points in the direction of the joint
    // 2) The x axis of the mesh is contained in the x-y plane of the reference frame

    // To get the z axis of the mesh to point in the direction of the joint
    Eigen::Quaterniond ori_quat;
    ori_quat.setFromTwoVectors(Eigen::Vector3d::UnitZ(), prism_joint_ori_in_ref_rb);

    // To get the x axis of the mesh to be contained in the x-y plane of the reference frame
    // First, find a vector that is orthogonal to the joint orientation and also to the z_axis (this latter implies to be contained in the xy plane)
    Eigen::Vector3d coplanar_xy_orthogonal_to_joint_ori = prism_joint_ori_in_ref_rb.cross(Eigen::Vector3d::UnitZ());
    // Normalize it -> Gives me the desired x axis after the rotation
    coplanar_xy_orthogonal_to_joint_ori.normalize();

    // Then find the corresponding y axis after the rotation as the cross product of the z axis after rotation (orientation of the joint)
    // and the x axis after rotation
    Eigen::Vector3d y_pos = prism_joint_ori_in_ref_rb.cross(coplanar_xy_orthogonal_to_joint_ori);

    // Create a matrix with the values of the vectors after rotation
    Eigen::Matrix3d rotation_pos;
    rotation_pos << coplanar_xy_orthogonal_to_joint_ori.x(),y_pos.x(),prism_joint_ori_in_ref_rb.x(),
            coplanar_xy_orthogonal_to_joint_ori.y(),y_pos.y(),prism_joint_ori_in_ref_rb.y(),
            coplanar_xy_orthogonal_to_joint_ori.z(),y_pos.z(),prism_joint_ori_in_ref_rb.z();

    // Create a quaternion with the matrix
    Eigen::Quaterniond ori_quat_final(rotation_pos);

    Eigen::Quaterniond ori_quat_final_ellipse(ori_quat_final.toRotationMatrix()*init_rot.toRotationMatrix());

    prism_axis_unc_cone1.pose.orientation.x = ori_quat_final_ellipse.x();
    prism_axis_unc_cone1.pose.orientation.y = ori_quat_final_ellipse.y();
    prism_axis_unc_cone1.pose.orientation.z = ori_quat_final_ellipse.z();
    prism_axis_unc_cone1.pose.orientation.w = ori_quat_final_ellipse.w();
    prism_axis_unc_cone1.ns = "kinematic_structure_uncertainty";
    prism_axis_unc_cone1.id = 3 * this->_joint_id + 1;
    prism_axis_unc_cone1.color.a = 0.4;
    prism_axis_unc_cone1.color.r = 0.0;
    prism_axis_unc_cone1.color.g = 1.0;
    prism_axis_unc_cone1.color.b = 0.0;

    // If the uncertainty is pi/6 (30 degrees) the scale in this direction should be 1
    // If the uncertainty is pi/12 (15 degrees) the scale in this direction should be 0.5
    // If the uncertainty is close to 0 the scale in this direction should be 0
    // If the uncertainty is close to pi the scale in this direction should be inf

    prism_axis_unc_cone1.scale.x = major_axis_length / (M_PI / 6.0);
    prism_axis_unc_cone1.scale.y = minor_axis_length / (M_PI / 6.0);
    prism_axis_unc_cone1.scale.z = 1.;

    prismatic_markers.push_back(prism_axis_unc_cone1);

    // We repeat the process for the cone in the other direction
    // To get the z axis of the mesh to point in the direction of the joint (negative)
    Eigen::Vector3d prism_joint_ori_in_ref_rb_neg = -prism_joint_ori_in_ref_rb;
    Eigen::Quaterniond ori_quat_neg;
    ori_quat_neg.setFromTwoVectors(Eigen::Vector3d::UnitZ(), prism_joint_ori_in_ref_rb_neg);

    // To get the x axis of the mesh to be contained in the x-y plane of the reference frame
    // First, find a vector that is orthogonal to the joint orientation and also to the z_axis (this latter implies to be contained in the xy plane)
    Eigen::Vector3d coplanar_xy_orthogonal_to_joint_ori_neg = prism_joint_ori_in_ref_rb_neg.cross(Eigen::Vector3d::UnitZ());
    // Normalize it -> Gives me the desired x axis after the rotation
    coplanar_xy_orthogonal_to_joint_ori_neg.normalize();

    // Then find the corresponding y axis after the rotation as the cross product of the z axis after rotation (orientation of the joint)
    // and the x axis after rotation
    Eigen::Vector3d y_neg = prism_joint_ori_in_ref_rb_neg.cross(coplanar_xy_orthogonal_to_joint_ori_neg);

    // Create a matrix with the values of the vectors after rotation
    Eigen::Matrix3d rotation_neg;
    rotation_neg << coplanar_xy_orthogonal_to_joint_ori_neg.x(),y_neg.x(),prism_joint_ori_in_ref_rb_neg.x(),
            coplanar_xy_orthogonal_to_joint_ori_neg.y(),y_neg.y(),prism_joint_ori_in_ref_rb_neg.y(),
            coplanar_xy_orthogonal_to_joint_ori_neg.z(),y_neg.z(),prism_joint_ori_in_ref_rb_neg.z();

    // Create a quaternion with the matrix
    Eigen::Quaterniond ori_quat_neg_final(rotation_neg);

    // We undo the rotation of the ellipse (but negative!):
    Eigen::AngleAxisd init_rot_neg(alpha, Eigen::Vector3d::UnitZ());
    Eigen::Quaterniond ori_quat_neg_final_ellipse(ori_quat_neg_final.toRotationMatrix()*init_rot_neg.toRotationMatrix());

    prism_axis_unc_cone1.pose.orientation.x = ori_quat_neg_final_ellipse.x();
    prism_axis_unc_cone1.pose.orientation.y = ori_quat_neg_final_ellipse.y();
    prism_axis_unc_cone1.pose.orientation.z = ori_quat_neg_final_ellipse.z();
    prism_axis_unc_cone1.pose.orientation.w = ori_quat_neg_final_ellipse.w();
    prism_axis_unc_cone1.scale.x = major_axis_length / (M_PI / 6.0);
    prism_axis_unc_cone1.scale.y = minor_axis_length / (M_PI / 6.0);
    prism_axis_unc_cone1.scale.z = 1.;
    prism_axis_unc_cone1.id =3 * this->_joint_id + 2;

    prismatic_markers.push_back(prism_axis_unc_cone1);

    return prismatic_markers;
}

JointFilterType PrismaticJointFilter::getJointFilterType() const
{
    return PRISMATIC_JOINT;
}

std::string PrismaticJointFilter::getJointFilterTypeStr() const
{
    return std::string("PrismaticJointFilter");
}

max_stars_repo_name: tu-rbo/omip

max_issues_repo_name: tu-rbo/omip

max_forks_repo_name: tu-rbo/omip


text:
#pragma once

#include <deip/token_sale_contribution_history/tsc_operation_object.hpp>
#include <boost/multi_index/composite_key.hpp>

namespace deip {
namespace tsc_history {

template <uint16_t TSCHistoryType> struct tsc_history_object : public object<TSCHistoryType, tsc_history_object<TSCHistoryType>>
{
    CHAINBASE_DEFAULT_CONSTRUCTOR(tsc_history_object)

    typedef typename object<TSCHistoryType, tsc_history_object<TSCHistoryType>>::id_type id_type;

    id_type id;

    account_name_type contributor;
    research_id_type research_id;
    protocol::external_id_type research_external_id;
    research_token_sale_id_type research_token_sale_id;
    protocol::external_id_type research_token_sale_external_id;
    protocol::asset amount;
    tsc_operation_object::id_type op;
};

struct by_account_and_research;
struct by_account_and_research_external_id;
struct by_account;
struct by_research;
struct by_token_sale;

struct by_research_external_id;
struct by_token_sale_external_id;

template <typename tsc_history_object_t>
using tsc_history_index
    = chainbase::shared_multi_index_container<tsc_history_object_t,
                                   indexed_by<ordered_unique<tag<by_id>,
                                                              member<tsc_history_object_t,
                                                                    typename tsc_history_object_t::id_type,
                                                                    &tsc_history_object_t::id>>,
                                              ordered_non_unique<tag<by_research>,
                                                              member<tsc_history_object_t,
                                                                    research_id_type,
                                                                    &tsc_history_object_t::research_id>>,
                                              ordered_non_unique<tag<by_research_external_id>,
                                                              member<tsc_history_object_t,
                                                                    protocol::external_id_type,
                                                                    &tsc_history_object_t::research_external_id>>,
                                              ordered_non_unique<tag<by_token_sale>,
                                                              member<tsc_history_object_t,
                                                                    research_token_sale_id_type,
                                                                    &tsc_history_object_t::research_token_sale_id>>,
                                              ordered_non_unique<tag<by_token_sale_external_id>,
                                                              member<tsc_history_object_t,
                                                                    protocol::external_id_type,
                                                                    &tsc_history_object_t::research_token_sale_external_id>>,
                                              ordered_non_unique<tag<by_account_and_research>,
                                                  composite_key<tsc_history_object_t,
                                                              member<tsc_history_object_t,
                                                                    account_name_type,
                                                                    &tsc_history_object_t::contributor>,
                                                              member<tsc_history_object_t,
                                                                     research_id_type,
                                                                    &tsc_history_object_t::research_id>>>,
                                              ordered_non_unique<tag<by_account_and_research_external_id>,
                                                  composite_key<tsc_history_object_t,
                                                              member<tsc_history_object_t,
                                                                    account_name_type,
                                                                    &tsc_history_object_t::contributor>,
                                                              member<tsc_history_object_t,
                                                                     protocol::external_id_type,
                                                                    &tsc_history_object_t::research_external_id>>>,
                                              ordered_non_unique<tag<by_account>,
                                                              member<tsc_history_object_t,
                                                                     account_name_type,
                                                                    &tsc_history_object_t::contributor>>>>;

using all_tsc_operations_history_object = tsc_history_object<all_tsc_operations_history>;
using contribute_to_token_sale_history_object = tsc_history_object<contribute_to_token_sale_history>;

using tsc_operations_full_history_index = tsc_history_index<all_tsc_operations_history_object>;
using contribute_to_token_sale_history_index = tsc_history_index<contribute_to_token_sale_history_object>;

//
} // namespace tsc_history
} // namespace deip

FC_REFLECT(deip::tsc_history::all_tsc_operations_history_object, (id)(contributor)(research_id)(research_external_id)(research_token_sale_id)(research_token_sale_external_id)(amount)(op))
FC_REFLECT(deip::tsc_history::contribute_to_token_sale_history_object, (id)(contributor)(research_id)(research_external_id)(research_token_sale_id)(research_token_sale_external_id)(amount)(op))

CHAINBASE_SET_INDEX_TYPE(deip::tsc_history::all_tsc_operations_history_object,
                         deip::tsc_history::tsc_operations_full_history_index)

CHAINBASE_SET_INDEX_TYPE(deip::tsc_history::contribute_to_token_sale_history_object,
                         deip::tsc_history::contribute_to_token_sale_history_index)

ext: hpp

max_stars_repo_name: DEIPworld/deip-chain

max_issues_repo_name: DEIPworld/deip-chain

max_forks_repo_name: DEIPworld/deip-chain


text:
// Copyright (c) 2018-2019 The Kron Core developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.
#include "validation.h"
#include "myassetsdb.h"
#include "messages.h"
#include <boost/thread.hpp>

#include <boost/thread.hpp>

static const char MESSAGE_FLAG = 'Z'; // Message
static const char MY_MESSAGE_CHANNEL = 'C'; // My followed Channels
static const char MY_SEEN_ADDRESSES = 'S'; // Addresses that have been seen on the chain
static const char DB_FLAG = 'D'; // Database Flags

static const char MY_TAGGED_ADDRESSES = 'T'; // Addresses that have been tagged
static const char MY_RESTRICTED_ADDRESSES = 'R'; // Addresses that have been restricted

CMessageDB::CMessageDB(size_t nCacheSize, bool fMemory, bool fWipe) : CDBWrapper(GetDataDir() / "messages" / "messages", nCacheSize, fMemory, fWipe) {
}

bool CMessageDB::WriteMessage(const CMessage &message)
{
    return Write(std::make_pair(MESSAGE_FLAG, message.out), message);
}

bool CMessageDB::ReadMessage(const COutPoint &out, CMessage &message)
{
    return Read(std::make_pair(MESSAGE_FLAG, out), message);
}

bool CMessageDB::EraseMessage(const COutPoint &out)
{
    return Erase(std::make_pair(MESSAGE_FLAG, out));
}

bool CMessageDB::LoadMessages(std::set<CMessage>& setMessages)
{
    std::unique_ptr<CDBIterator> pcursor(NewIterator());

    pcursor->Seek(std::make_pair(MESSAGE_FLAG, COutPoint()));

    // Load messages
    while (pcursor->Valid()) {
        boost::this_thread::interruption_point();
        std::pair<char, COutPoint> key;
        if (pcursor->GetKey(key) && key.first == MESSAGE_FLAG) {
            CMessage message;
            if (pcursor->GetValue(message)) {
                setMessages.insert(message);
                pcursor->Next();
            } else {
                LogPrintf("%s: failed to read message\n", __func__);
                pcursor->Next();
            }
        } else {
            break;
        }
    }
    return true;
}

bool CMessageDB::EraseAllMessages(int& count)
{
    std::set<CMessage> setMessages;
    std::unique_ptr<CDBIterator> pcursor(NewIterator());

    pcursor->Seek(std::make_pair(MESSAGE_FLAG, COutPoint()));

    // Load messages
    while (pcursor->Valid()) {
        boost::this_thread::interruption_point();
        std::pair<char, COutPoint> key;
        if (pcursor->GetKey(key) && key.first == MESSAGE_FLAG) {
            CMessage message;
            if (pcursor->GetValue(message)) {
                setMessages.insert(message);
                pcursor->Next();
            } else {
                LogPrintf("%s: failed to read message\n", __func__);
                pcursor->Next();
            }
        } else {
            break;
        }
    }

    count += setMessages.size();
    for (auto message : setMessages)
        EraseMessage(message.out);

    return true;
}

bool CMessageDB::Flush() {
    try {

        for (auto messageRemove : setDirtyMessagesRemove) {
            if (!EraseMessage(messageRemove))
                return error("%s: failed to erase message %s", __func__, messageRemove.ToString());
        }

        for (auto messageAdd : mapDirtyMessagesAdd) {
            if (!WriteMessage(messageAdd.second))
                return error("%s: failed to write message %s", __func__, messageAdd.second.ToString());

            mapDirtyMessagesOrphaned.erase(messageAdd.first);
        }

        for (auto orphans : mapDirtyMessagesOrphaned) {
            CMessage msg = orphans.second;
            msg.status = MessageStatus::ORPHAN;
            if (!WriteMessage(msg))
                return error("%s: failed to write message orphan %s", __func__, msg.ToString());
        }

        setDirtyMessagesRemove.clear();
        mapDirtyMessagesAdd.clear();
        mapDirtyMessagesOrphaned.clear();
    } catch (const std::runtime_error& e) {
        return error("%s : %s ", __func__, std::string("System error while flushing messages: ") + e.what());
    }

    return true;
}

CMessageChannelDB::CMessageChannelDB(size_t nCacheSize, bool fMemory, bool fWipe) : CDBWrapper(GetDataDir() / "messages" / "channels", nCacheSize, fMemory, fWipe) {
}

bool CMessageChannelDB::WriteMyMessageChannel(const std::string& channelname)
{
    return Write(std::make_pair(MY_MESSAGE_CHANNEL, channelname), 1);
}

bool CMessageChannelDB::ReadMyMessageChannel(const std::string& channelname)
{
    int i = 1;
    return Read(std::make_pair(MY_MESSAGE_CHANNEL, channelname), i);
}

bool CMessageChannelDB::EraseMyMessageChannel(const std::string& channelname)
{
    return Erase(std::make_pair(MY_MESSAGE_CHANNEL, channelname));
}

bool CMessageChannelDB::LoadMyMessageChannels(std::set<std::string>& setChannels)
{
    setChannels.clear();
    std::unique_ptr<CDBIterator> pcursor(NewIterator());

    pcursor->Seek(std::make_pair(MY_MESSAGE_CHANNEL, std::string()));

    // Load messages
    while (pcursor->Valid()) {
        boost::this_thread::interruption_point();
        std::pair<char, std::string> key;
        if (pcursor->GetKey(key) && key.first == MY_MESSAGE_CHANNEL) {
            setChannels.insert(key.second);
            pcursor->Next();
        } else {
            break;
        }
    }

    return true;
}

bool CMessageDB::WriteFlag(const std::string &name, bool fValue)
{
    return Write(std::make_pair(DB_FLAG, name), fValue ? '1' : '0');
}

bool CMessageDB::ReadFlag(const std::string &name, bool &fValue)
{
    char ch;
    if (!Read(std::make_pair(DB_FLAG, name), ch))
        return false;
    fValue = ch == '1';
    return true;
}

bool CMessageChannelDB::WriteUsedAddress(const std::string& address)
{
    return Write(std::make_pair(MY_SEEN_ADDRESSES, address), 1);
}
bool CMessageChannelDB::ReadUsedAddress(const std::string& address)
{
    int i;
    return Read(std::make_pair(MY_SEEN_ADDRESSES, address), i);
}
bool CMessageChannelDB::EraseUsedAddress(const std::string& address)
{
    return Erase(std::make_pair(MY_SEEN_ADDRESSES, address));
}

bool CMessageChannelDB::WriteFlag(const std::string &name, bool fValue)
{
    return Write(std::make_pair(DB_FLAG, name), fValue ? '1' : '0');
}

bool CMessageChannelDB::ReadFlag(const std::string &name, bool &fValue)
{
    char ch;
    if (!Read(std::make_pair(DB_FLAG, name), ch))
        return false;
    fValue = ch == '1';
    return true;
}

bool CMessageChannelDB::Flush() {
    try {
        LogPrintf("%s: Flushing messagechannelsdb addSize:%u, removeSize:%u, seenAddressSize:%u\n", __func__, setDirtyChannelsAdd.size(), setDirtyChannelsRemove.size(), setDirtySeenAddressAdd.size());

        for (auto channelRemove : setDirtyChannelsRemove) {
            if (!EraseMyMessageChannel(channelRemove))
                return error("%s: failed to erase messagechannel %s", __func__, channelRemove);
        }

        for (auto channelAdd : setDirtyChannelsAdd) {
            if (!WriteMyMessageChannel(channelAdd))
                return error("%s: failed to write messagechannel %s", __func__, channelAdd);
        }

        for (auto seenAddress : setDirtySeenAddressAdd) {
            if (!WriteUsedAddress(seenAddress))
                return error("%s: failed to write seenaddress %s", __func__, seenAddress);
        }

        setDirtyChannelsRemove.clear();
        setDirtyChannelsAdd.clear();
        setDirtySeenAddressAdd.clear();
        setSubscribedChannelsAskedForFalse.clear();
        setAddressAskedForFalse.clear();
    } catch (const std::runtime_error& e) {
        return error("%s : %s ", __func__, std::string("System error while flushing messagechannels: ") + e.what());
    }

    return true;
}

CMyRestrictedDB::CMyRestrictedDB(size_t nCacheSize, bool fMemory, bool fWipe) : CDBWrapper(GetDataDir() / "myrestricted", nCacheSize, fMemory, fWipe) {
}

bool CMyRestrictedDB::WriteTaggedAddress(const std::string& address, const std::string& tag_name, const bool fAdd, const uint32_t& nHeight)
{
    return Write(std::make_pair(MY_TAGGED_ADDRESSES, std::make_pair(address, tag_name)), std::make_pair(fAdd ? 1 : 0, nHeight));
}
bool CMyRestrictedDB::ReadTaggedAddress(const std::string& address, const std::string& tag_name, bool& fAdd, uint32_t& nHeight)
{
    std::pair<int, uint32_t> value;
    bool ret = Read(std::make_pair(MY_TAGGED_ADDRESSES, std::make_pair(address, tag_name)), value);
    fAdd = value.first;
    nHeight = value.second;
    return ret;
}
bool CMyRestrictedDB::EraseTaggedAddress(const std::string& address, const std::string& tag_name)
{
    return Erase(std::make_pair(MY_TAGGED_ADDRESSES, std::make_pair(address, tag_name)));
}
bool CMyRestrictedDB::LoadMyTaggedAddresses(std::vector<std::tuple<std::string, std::string, bool, uint32_t> >& vecTaggedAddresses)
{
    vecTaggedAddresses.clear();
    std::unique_ptr<CDBIterator> pcursor(NewIterator());

    pcursor->Seek(std::make_pair(MY_TAGGED_ADDRESSES, std::make_pair(std::string(), std::string())));

    // Load messages
    while (pcursor->Valid()) {
        boost::this_thread::interruption_point();
        std::pair<char, std::pair<std::string, std::string>> key;
        if (pcursor->GetKey(key) && key.first == MY_TAGGED_ADDRESSES) {
            std::pair<int, uint32_t> value;
            if (pcursor->GetValue(value)) {
                vecTaggedAddresses.emplace_back(std::make_tuple(key.second.first, key.second.second, value.first ? true : false, value.second));
            }
            pcursor->Next();
        } else {
            break;
        }
    }

    return true;
}

bool CMyRestrictedDB::WriteRestrictedAddress(const std::string& address, const std::string& tag_name, const bool fAdd, const uint32_t& nHeight)
{
    return Write(std::make_pair(MY_RESTRICTED_ADDRESSES, std::make_pair(address, tag_name)), std::make_pair(fAdd ? 1 : 0, nHeight));
}

bool CMyRestrictedDB::ReadRestrictedAddress(const std::string& address, const std::string& tag_name, bool& fAdd, uint32_t& nHeight)
{
    std::pair<int, uint32_t> value;
    bool ret = Read(std::make_pair(MY_RESTRICTED_ADDRESSES, std::make_pair(address, tag_name)), value);
    fAdd = value.first;
    nHeight = value.second;
    return ret;
}

bool CMyRestrictedDB::EraseRestrictedAddress(const std::string& address, const std::string& tag_name)
{
    return Erase(std::make_pair(MY_RESTRICTED_ADDRESSES, std::make_pair(address, tag_name)));
}

bool CMyRestrictedDB::LoadMyRestrictedAddresses(std::vector<std::tuple<std::string, std::string, bool, uint32_t> >& vecRestrictedAddresses)
{
    vecRestrictedAddresses.clear();
    std::unique_ptr<CDBIterator> pcursor(NewIterator());

    pcursor->Seek(std::make_pair(MY_RESTRICTED_ADDRESSES, std::make_pair(std::string(), std::string())));

    // Load messages
    while (pcursor->Valid()) {
        boost::this_thread::interruption_point();
        std::pair<char, std::pair<std::string, std::string>> key;
        if (pcursor->GetKey(key) && key.first == MY_RESTRICTED_ADDRESSES) {
            std::pair<int, uint32_t> value;
            if (pcursor->GetValue(value)) {
                vecRestrictedAddresses.emplace_back(std::make_tuple(key.second.first, key.second.second, value.first ? true : false, value.second));
            }
            pcursor->Next();
        } else {
            break;
        }
    }

    return true;
}

bool CMyRestrictedDB::WriteFlag(const std::string &name, bool fValue)
{
    return Write(std::make_pair(DB_FLAG, name), fValue ? '1' : '0');
}

bool CMyRestrictedDB::ReadFlag(const std::string &name, bool &fValue)
{
    char ch;
    if (!Read(std::make_pair(DB_FLAG, name), ch))
        return false;
    fValue = ch == '1';
    return true;
}

ext: cpp

max_issues_repo_name: BaymaxValero/Kron-Project

max_forks_repo_name: BaymaxValero/Kron-Project


text:
//  (C) Copyright Thomas Witt 2003.
//  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.

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

#include <algorithm>
#include <functional>
#include <numeric>

#include <boost/iterator/iterator_adaptor.hpp>
#if !BOOST_WORKAROUND(__MWERKS__, <= 0x2407)
# include <boost/iterator/is_readable_iterator.hpp>
# include <boost/iterator/is_lvalue_iterator.hpp>
#endif
#include <boost/pending/iterator_tests.hpp>

# include <boost/core/lightweight_test.hpp>

#include <stdlib.h>
#include <vector>
#include <deque>
#include <set>
#include <list>

#include "static_assert_same.hpp"

#include <boost/iterator/detail/config_def.hpp>

using boost::dummyT;

typedef std::deque<int> storage;
typedef std::deque<int*> pointer_deque;
typedef std::set<storage::iterator> iterator_set;

template <class T> struct foo;

void blah(int) { }

struct my_gen
{
  typedef int result_type;
  my_gen() : n(0) { }
  int operator()() { return ++n; }
  int n;
};

template <class V>
struct ptr_iterator
  : boost::iterator_adaptor<
        ptr_iterator<V>
      , V*
      , V
      , boost::random_access_traversal_tag
#if BOOST_WORKAROUND(BOOST_BORLANDC, BOOST_TESTED_AT(0x551))
      , V&
#endif
   >
{
private:
  typedef boost::iterator_adaptor<
        ptr_iterator<V>
      , V*
      , V
      , boost::random_access_traversal_tag
#if BOOST_WORKAROUND(BOOST_BORLANDC, BOOST_TESTED_AT(0x551))
      , V&
#endif
    > super_t;

public:
  ptr_iterator() { }
  ptr_iterator(V* d) : super_t(d) { }

  template <class V2>
  ptr_iterator(
        const ptr_iterator<V2>& x
      , typename boost::enable_if_convertible<V2*, V*>::type* = 0
  )
    : super_t(x.base())
  {}
};

// Non-functional iterator for category modification checking
template <class Iter, class Traversal>
struct modify_traversal
  :  boost::iterator_adaptor<
         modify_traversal<Iter, Traversal>
       , Iter
       , boost::use_default
       , Traversal
     >
{};

template <class T>
struct fwd_iterator
  : boost::iterator_adaptor<
        fwd_iterator<T>
      , boost::forward_iterator_archetype<T>
    >
{
private:
  typedef boost::iterator_adaptor<
        fwd_iterator<T>
      , boost::forward_iterator_archetype<T>
  > super_t;

public:
  fwd_iterator() { }
  fwd_iterator(boost::forward_iterator_archetype<T> d) : super_t(d) { }
};

template <class T>
struct in_iterator
  : boost::iterator_adaptor<
        in_iterator<T>
      , boost::input_iterator_archetype_no_proxy<T>
    >
{
private:
  typedef boost::iterator_adaptor<
        in_iterator<T>
      , boost::input_iterator_archetype_no_proxy<T>
  > super_t;

public:
  in_iterator() { }
  in_iterator(boost::input_iterator_archetype_no_proxy<T> d) : super_t(d) { }
};

template <class Iter>
struct constant_iterator
  : boost::iterator_adaptor<
        constant_iterator<Iter>
      , Iter
      , typename std::iterator_traits<Iter>::value_type const
    >
{
    typedef boost::iterator_adaptor<
        constant_iterator<Iter>
      , Iter
      , typename std::iterator_traits<Iter>::value_type const
    > base_t;

  constant_iterator() {}
  constant_iterator(Iter it)
    : base_t(it) {}
};

char (& traversal2(boost::incrementable_traversal_tag) )[1];
char (& traversal2(boost::single_pass_traversal_tag  ) )[2];
char (& traversal2(boost::forward_traversal_tag      ) )[3];
char (& traversal2(boost::bidirectional_traversal_tag) )[4];
char (& traversal2(boost::random_access_traversal_tag) )[5];

template <class Cat>
struct traversal3
{
    static typename boost::iterator_category_to_traversal<Cat>::type x;
    BOOST_STATIC_CONSTANT(std::size_t, value = sizeof(traversal2(x)));
    typedef char (&type)[value];
};

template <class Cat>
typename traversal3<Cat>::type traversal(Cat);

template <class Iter, class Trav>
int static_assert_traversal(Iter* = 0, Trav* = 0)
{
    typedef typename boost::iterator_category_to_traversal<
        BOOST_DEDUCED_TYPENAME Iter::iterator_category
        >::type t2;

    return static_assert_same<Trav,t2>::value;
}

int
main()
{
  dummyT array[] = { dummyT(0), dummyT(1), dummyT(2),
                     dummyT(3), dummyT(4), dummyT(5) };
  const int N = sizeof(array)/sizeof(dummyT);

  // sanity check, if this doesn't pass the test is buggy
  boost::random_access_iterator_test(array, N, array);

  // Test the iterator_adaptor
  {
    ptr_iterator<dummyT> i(array);
    boost::random_access_iterator_test(i, N, array);

    ptr_iterator<const dummyT> j(array);
    boost::random_access_iterator_test(j, N, array);
    boost::const_nonconst_iterator_test(i, ++j);
  }

  int test;
  // Test the iterator_traits
  {
    // Test computation of defaults
    typedef ptr_iterator<int> Iter1;
    // don't use std::iterator_traits here to avoid VC++ problems
    test = static_assert_same<Iter1::value_type, int>::value;
    test = static_assert_same<Iter1::reference, int&>::value;
    test = static_assert_same<Iter1::pointer, int*>::value;
    test = static_assert_same<Iter1::difference_type, std::ptrdiff_t>::value;
#if !BOOST_WORKAROUND(__MWERKS__, <= 0x2407)
    BOOST_STATIC_ASSERT((boost::is_convertible<Iter1::iterator_category, std::random_access_iterator_tag>::value));
#endif
  }

  {
    // Test computation of default when the Value is const
    typedef ptr_iterator<int const> Iter1;
    test = static_assert_same<Iter1::value_type, int>::value;
    test = static_assert_same<Iter1::reference, const int&>::value;

#if !BOOST_WORKAROUND(__MWERKS__, <= 0x2407)
    BOOST_STATIC_ASSERT(boost::is_readable_iterator<Iter1>::value);
# ifndef BOOST_NO_LVALUE_RETURN_DETECTION
    BOOST_STATIC_ASSERT(boost::is_lvalue_iterator<Iter1>::value);
# endif
#endif

#if !BOOST_WORKAROUND(BOOST_BORLANDC, BOOST_TESTED_AT(0x564)) // borland drops constness
    test = static_assert_same<Iter1::pointer, int const*>::value;
#endif
  }

  {
    // Test constant iterator idiom
    typedef ptr_iterator<int> BaseIter;
    typedef constant_iterator<BaseIter> Iter;

    test = static_assert_same<Iter::value_type, int>::value;
    test = static_assert_same<Iter::reference, int const&>::value;
#if !BOOST_WORKAROUND(BOOST_BORLANDC, BOOST_TESTED_AT(0x564)) // borland drops constness
    test = static_assert_same<Iter::pointer, int const*>::value;
#endif

#ifndef BOOST_NO_LVALUE_RETURN_DETECTION
    BOOST_STATIC_ASSERT(boost::is_non_const_lvalue_iterator<BaseIter>::value);
    BOOST_STATIC_ASSERT(boost::is_lvalue_iterator<Iter>::value);
#endif

    typedef modify_traversal<BaseIter, boost::incrementable_traversal_tag> IncrementableIter;

    static_assert_traversal<BaseIter,boost::random_access_traversal_tag>();
    static_assert_traversal<IncrementableIter,boost::incrementable_traversal_tag>();
  }

  // Test the iterator_adaptor
  {
    ptr_iterator<dummyT> i(array);
    boost::random_access_iterator_test(i, N, array);

    ptr_iterator<const dummyT> j(array);
    boost::random_access_iterator_test(j, N, array);
    boost::const_nonconst_iterator_test(i, ++j);
  }

  // check operator-> with a forward iterator
  {
    boost::forward_iterator_archetype<dummyT> forward_iter;

    typedef fwd_iterator<dummyT> adaptor_type;

    adaptor_type i(forward_iter);
    int zero = 0;
    if (zero) // don't do this, just make sure it compiles
      BOOST_TEST((*i).m_x == i->foo());
  }

  // check operator-> with an input iterator
  {
    boost::input_iterator_archetype_no_proxy<dummyT> input_iter;
    typedef in_iterator<dummyT> adaptor_type;
    adaptor_type i(input_iter);
    int zero = 0;
    if (zero) // don't do this, just make sure it compiles
      BOOST_TEST((*i).m_x == i->foo());
  }

  // check that base_type is correct
  {
    // Test constant iterator idiom
    typedef ptr_iterator<int> BaseIter;

    test = static_assert_same<BaseIter::base_type,int*>::value;
    test = static_assert_same<constant_iterator<BaseIter>::base_type,BaseIter>::value;

    typedef modify_traversal<BaseIter, boost::incrementable_traversal_tag> IncrementableIter;

    test = static_assert_same<IncrementableIter::base_type,BaseIter>::value;
  }

  std::cout << "test successful " << std::endl;
  (void)test;
  return boost::report_errors();
}


text:
/*#include <boost/log/core/record.hpp>
#include <boost/log/sources/record_ostream.hpp>
#include <boost/preprocessor/seq/enum.hpp>
#include <boost/preprocessor/seq/size.hpp>

#include <boost/beast/core.hpp>
#include <boost/beast/http.hpp>
#include <boost/beast/version.hpp>
#include <boost/asio/bind_executor.hpp>
#include <boost/asio/strand.hpp>
#include <boost/asio/ip/tcp.hpp>
#include <boost/config.hpp>

#include <cstdlib>
#include <iostream>
#include <memory>
#include <string>
#include <thread>
#include <algorithm>
#include <cstdlib>
#include <functional>
#include <iostream>
#include <memory>
#include <string>
#include <thread>
#include <vector>
#include <iostream>
#include <string>
#include <sstream>
#include <fstream>
#include <streambuf>

#include "mime_type.h"*/
#include "http_callbacks.h"

#include <iostream>

int http_callbacks1 = 122;

int http_callbacks2() {
    return 54;
}

#if defined(CLING_IS_ON)
static void http_callbacks_module_cling_prepare() {
    printf("called http_callbacks_prepare\n");
}

/*static void http_callbacks_module_cling_run_update() {
    printf("called http_callbacks_run_update\n");
}*/

static void http_callbacks_module_cling_run() {
    printf("called http_callbacks_run\n");
    setAnotherGlobal(1.43);
}

static void http_callbacks_module_cling_shutdown() {
    printf("called http_callbacks_shutdown\n");
}

void http_callbacks() {
    printf("called http_callbacks\n");
}
#endif // CLING_IS_ON

max_stars_repo_name: derofim/cling-cmake

max_issues_repo_name: derofim/cling-cmake

max_forks_repo_name: derofim/cling-cmake


text:
#include <gtest\gtest.h>
#include <boost\shared_ptr.hpp>
#include <boost\make_shared.hpp>
#include "container.hpp"
#include "consumer_types.h"

//////////////////////////////////////////////////////////////////////////
// Tests object lifetimes using various lifestyle approaches.

typedef consumer_types<ioc::transient_t> transient_types;
typedef consumer_types<ioc::singleton_t> singleton_types;
typedef consumer_types<ioc::singleton_t, ioc::singleton_t, ioc::transient_t> mixed_types;

class Test_Lifetime : public testing::Test
{
public:

    virtual void SetUp()
    {
        ConsumerTypesResetCounters();
    }
};

TEST_F(Test_Lifetime, TransientConstructors)
{
    ioc::container container;
    container.registerType<transient_types::_Consumer3>();
    boost::shared_ptr<Consumer3> obj = container.resolve<Consumer3>();

    EXPECT_TRUE(obj->Svc1->SubSvc1 != obj->Svc2->SubSvc1);
    EXPECT_TRUE(obj->Svc2->SubSvc1 != obj->Svc3->SubSvc1);
    EXPECT_TRUE(obj->Svc3->SubSvc1 != obj->Svc1->SubSvc1);

    EXPECT_EQ(1, Consumer3::CtorCount);
    EXPECT_EQ(1, Svc1::CtorCount);
    EXPECT_EQ(1, Svc2::CtorCount);
    EXPECT_EQ(1, Svc3::CtorCount);
    EXPECT_EQ(3, SubSvc1::CtorCount);
    EXPECT_EQ(2, SubSvc2::CtorCount);
    EXPECT_EQ(1, SubSvc3::CtorCount);
}

TEST_F(Test_Lifetime, TransientDestructors)
{
    ioc::container container;
    container.registerType<transient_types::_Consumer1>();
    boost::shared_ptr<Consumer1> obj = container.resolve<Consumer1>();

    EXPECT_EQ(0, Consumer1::DtorCount);
    EXPECT_EQ(0, Svc1::DtorCount);

    obj.reset();

    EXPECT_EQ(1, Consumer1::DtorCount);
    EXPECT_EQ(1, Svc1::DtorCount);
}

TEST_F(Test_Lifetime, SingletonConstructors)
{
    ioc::container container;
    container.registerType<singleton_types::_Consumer3>();
    boost::shared_ptr<Consumer3> obj = container.resolve<Consumer3>();

    EXPECT_TRUE(obj->Svc1->SubSvc1 == obj->Svc2->SubSvc1);
    EXPECT_TRUE(obj->Svc2->SubSvc1 == obj->Svc3->SubSvc1);
    EXPECT_TRUE(obj->Svc3->SubSvc1 == obj->Svc1->SubSvc1);

    EXPECT_EQ(1, Consumer3::CtorCount);
    EXPECT_EQ(1, Svc1::CtorCount);
    EXPECT_EQ(1, Svc2::CtorCount);
    EXPECT_EQ(1, Svc3::CtorCount);
    EXPECT_EQ(1, SubSvc1::CtorCount);
    EXPECT_EQ(1, SubSvc2::CtorCount);
    EXPECT_EQ(1, SubSvc3::CtorCount);
}

TEST_F(Test_Lifetime, SingletonDestructors)
{
    boost::shared_ptr<ioc::container> container = boost::make_shared<ioc::container>();
    container->registerType<singleton_types::_Consumer1>();
    boost::shared_ptr<Consumer1> obj = container->resolve<Consumer1>();

    obj.reset();

    EXPECT_EQ(0, Consumer1::DtorCount);
    EXPECT_EQ(0, Svc1::DtorCount);

    container.reset();

    EXPECT_EQ(1, Consumer1::DtorCount);
    EXPECT_EQ(1, Svc1::DtorCount);
}

TEST_F(Test_Lifetime, SingletonConsumerAndTransientService)
{
    ioc::container container;
    container.registerType<mixed_types::_Svc1>();

    boost::shared_ptr<Svc1> firstSvc1 = container.resolve<Svc1>();
    boost::shared_ptr<Svc1> secondSvc1 = container.resolve<Svc1>();
    EXPECT_EQ(1, SubSvc1::CtorCount);
}

max_stars_repo_name: yvz/iocplusplus

max_issues_repo_name: yvz/iocplusplus

max_forks_repo_name: yvz/iocplusplus


text:
/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file NoiseModel.cpp
 * @date Jan 13, 2010
 * @author Richard Roberts
 * @author Frank Dellaert
 */

#include <limits>
#include <iostream>
#include <typeinfo>
#include <stdexcept>

#include <boost/foreach.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>

#include <gtsam/base/timing.h>
#include <gtsam/linear/NoiseModel.h>

static double inf = std::numeric_limits<double>::infinity();
using namespace std;

namespace gtsam {
namespace noiseModel {

/* ************************************************************************* */
// update A, b
// A' \define A_{S}-ar and b'\define b-ad
// Linear algebra: takes away projection on latest orthogonal
// Graph: make a new factor on the separator S
// __attribute__ ((noinline))  // uncomment to prevent inlining when profiling
template<class MATRIX>
void updateAb(MATRIX& Ab, int j, const Vector& a, const Vector& rd) {
  size_t n = Ab.cols()-1;
  Ab.middleCols(j+1,n-j) -= a * rd.segment(j+1, n-j).transpose();
}

/* ************************************************************************* */
// check *above the diagonal* for non-zero entries
boost::optional<Vector> checkIfDiagonal(const Matrix M) {
  size_t m = M.rows(), n = M.cols();
  // check all non-diagonal entries
  bool full = false;
  size_t i, j;
  for (i = 0; i < m; i++)
    if (!full)
      for (j = i + 1; j < n; j++)
        if (fabs(M(i, j)) > 1e-9) {
          full = true;
          break;
        }
  if (full) {
    return boost::none;
  } else {
    Vector diagonal(n);
    for (j = 0; j < n; j++)
      diagonal(j) = M(j, j);
    return diagonal;
  }
}

/* ************************************************************************* */
Gaussian::shared_ptr Gaussian::SqrtInformation(const Matrix& R, bool smart) {
  size_t m = R.rows(), n = R.cols();
  if (m != n)
    throw invalid_argument("Gaussian::SqrtInformation: R not square");
  boost::optional<Vector> diagonal = boost::none;
  if (smart)
    diagonal = checkIfDiagonal(R);
  if (diagonal)
    return Diagonal::Sigmas(reciprocal(*diagonal), true);
  else
    return shared_ptr(new Gaussian(R.rows(), R));
}

/* ************************************************************************* */
Gaussian::shared_ptr Gaussian::Information(const Matrix& M, bool smart) {
  size_t m = M.rows(), n = M.cols();
  if (m != n)
    throw invalid_argument("Gaussian::Information: R not square");
  boost::optional<Vector> diagonal = boost::none;
  if (smart)
    diagonal = checkIfDiagonal(M);
  if (diagonal)
    return Diagonal::Precisions(*diagonal, true);
  else {
    Matrix R = RtR(M);
    return shared_ptr(new Gaussian(R.rows(), R));
  }
}

/* ************************************************************************* */
Gaussian::shared_ptr Gaussian::Covariance(const Matrix& covariance,
    bool smart) {
  size_t m = covariance.rows(), n = covariance.cols();
  if (m != n)
    throw invalid_argument("Gaussian::Covariance: covariance not square");
  boost::optional<Vector> variances = boost::none;
  if (smart)
    variances = checkIfDiagonal(covariance);
  if (variances)
    return Diagonal::Variances(*variances, true);
  else
    return shared_ptr(new Gaussian(n, inverse_square_root(covariance)));
}

/* ************************************************************************* */
void Gaussian::print(const string& name) const {
  gtsam::print(thisR(), "Gaussian");
}

/* ************************************************************************* */
bool Gaussian::equals(const Base& expected, double tol) const {
  const Gaussian* p = dynamic_cast<const Gaussian*> (&expected);
  if (p == NULL) return false;
  if (typeid(*this) != typeid(*p)) return false;
  //if (!sqrt_information_) return true; // ALEX todo;
  return equal_with_abs_tol(R(), p->R(), sqrt(tol));
}

/* ************************************************************************* */
Vector Gaussian::whiten(const Vector& v) const {
  return thisR() * v;
}

/* ************************************************************************* */
Vector Gaussian::unwhiten(const Vector& v) const {
  return backSubstituteUpper(thisR(), v);
}

/* ************************************************************************* */
double Gaussian::Mahalanobis(const Vector& v) const {
  // Note: for Diagonal, which does ediv_, will be correct for constraints
  Vector w = whiten(v);
  return w.dot(w);
}

/* ************************************************************************* */
Matrix Gaussian::Whiten(const Matrix& H) const {
  return thisR() * H;
}

/* ************************************************************************* */
void Gaussian::WhitenInPlace(Matrix& H) const {
  H = thisR() * H;
}

/* ************************************************************************* */
void Gaussian::WhitenInPlace(Eigen::Block<Matrix> H) const {
  H = thisR() * H;
}

/* ************************************************************************* */
// General QR, see also special version in Constrained
SharedDiagonal Gaussian::QR(Matrix& Ab) const {

  gttic(Gaussian_noise_model_QR);

  static const bool debug = false;

  // get size(A) and maxRank
  // TODO: really no rank problems ?
  // size_t m = Ab.rows(), n = Ab.cols()-1;
  // size_t maxRank = min(m,n);

  // pre-whiten everything (cheaply if possible)
  WhitenInPlace(Ab);

  if(debug) gtsam::print(Ab, "Whitened Ab: ");

  // Eigen QR - much faster than older householder approach
  inplace_QR(Ab);

  // hand-coded householder implementation
  // TODO: necessary to isolate last column?
  // householder(Ab, maxRank);

  return SharedDiagonal();
}

void Gaussian::WhitenSystem(vector<Matrix>& A, Vector& b) const {
  BOOST_FOREACH(Matrix& Aj, A) { WhitenInPlace(Aj); }
  whitenInPlace(b);
}

void Gaussian::WhitenSystem(Matrix& A, Vector& b) const {
  WhitenInPlace(A);
  whitenInPlace(b);
}

void Gaussian::WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const {
  WhitenInPlace(A1);
  WhitenInPlace(A2);
  whitenInPlace(b);
}

void Gaussian::WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const{
  WhitenInPlace(A1);
  WhitenInPlace(A2);
  WhitenInPlace(A3);
  whitenInPlace(b);
}

/* ************************************************************************* */
// Diagonal
/* ************************************************************************* */
Diagonal::Diagonal() :
    Gaussian(1) // TODO: Frank asks: really sure about this?
{
}

/* ************************************************************************* */
Diagonal::Diagonal(const Vector& sigmas) :
    Gaussian(sigmas.size()), sigmas_(sigmas), invsigmas_(reciprocal(sigmas)), precisions_(
        emul(invsigmas_, invsigmas_)) {
}

/* ************************************************************************* */
Diagonal::shared_ptr Diagonal::Variances(const Vector& variances, bool smart) {
  if (smart) {
    // check whether all the same entry
    size_t n = variances.size();
    for (size_t j = 1; j < n; j++)
      if (variances(j) != variances(0)) goto full;
    return Isotropic::Variance(n, variances(0), true);
  }
  full: return shared_ptr(new Diagonal(esqrt(variances)));
}

/* ************************************************************************* */
Diagonal::shared_ptr Diagonal::Sigmas(const Vector& sigmas, bool smart) {
  if (smart) {
    size_t n = sigmas.size();
    if (n==0) goto full;
    // look for zeros to make a constraint
    for (size_t j=0; j< n; ++j)
      if (sigmas(j)<1e-8)
        return Constrained::MixedSigmas(sigmas);
    // check whether all the same entry
    for (size_t j = 1; j < n; j++)
      if (sigmas(j) != sigmas(0)) goto full;
    return Isotropic::Sigma(n, sigmas(0), true);
  }
  full: return Diagonal::shared_ptr(new Diagonal(sigmas));
}

/* ************************************************************************* */
void Diagonal::print(const string& name) const {
  gtsam::print(sigmas_, name + "diagonal sigmas");
}

/* ************************************************************************* */
Vector Diagonal::whiten(const Vector& v) const {
  return emul(v, invsigmas());
}

/* ************************************************************************* */
Vector Diagonal::unwhiten(const Vector& v) const {
  return emul(v, sigmas_);
}

/* ************************************************************************* */
Matrix Diagonal::Whiten(const Matrix& H) const {
  return vector_scale(invsigmas(), H);
}

/* ************************************************************************* */
void Diagonal::WhitenInPlace(Matrix& H) const {
  vector_scale_inplace(invsigmas(), H);
}

/* ************************************************************************* */
void Diagonal::WhitenInPlace(Eigen::Block<Matrix> H) const {
  H = invsigmas().asDiagonal() * H;
}

/* ************************************************************************* */
// Constrained
/* ************************************************************************* */

/* ************************************************************************* */
Constrained::Constrained(const Vector& sigmas)
  : Diagonal(sigmas), mu_(repeat(sigmas.size(), 1000.0)) {
  for (int i=0; i<sigmas.size(); ++i) {
    if (!std::isfinite(1./sigmas(i))) {
      precisions_(i) = 0.0; // Set to finite value
      invsigmas_(i) = 0.0;
    }
  }
}

/* ************************************************************************* */
Constrained::Constrained(const Vector& mu, const Vector& sigmas)
  : Diagonal(sigmas), mu_(mu) {
//  assert(sigmas.size() == mu.size());
  for (int i=0; i<sigmas.size(); ++i) {
    if (!std::isfinite(1./sigmas(i))) {
      precisions_(i) = 0.0; // Set to finite value
      invsigmas_(i) = 0.0;
    }
  }
}

/* ************************************************************************* */
Constrained::shared_ptr Constrained::MixedSigmas(const Vector& mu, const Vector& sigmas, bool smart) {
  // FIXME: can't return a diagonal shared_ptr due to conversion
//  if (smart) {
//    // look for zeros to make a constraint
//    for (size_t i=0; i< (size_t) sigmas.size(); ++i)
//      if (sigmas(i)<1e-8)
//        return MixedSigmas(mu, sigmas);
//    return Diagonal::Sigmas(sigmas);
//  }
  return shared_ptr(new Constrained(mu, sigmas));
}

/* ************************************************************************* */
void Constrained::print(const std::string& name) const {
  gtsam::print(sigmas_, name + "constrained sigmas");
  gtsam::print(mu_, name + "constrained mu");
}

/* ************************************************************************* */
Vector Constrained::whiten(const Vector& v) const {
  // If sigmas[i] is not 0 then divide v[i] by sigmas[i], as usually done in
  // other normal Gaussian noise model. Otherwise, sigmas[i] = 0 indicating
  // a hard constraint, we don't do anything.
  const Vector& a = v;
  const Vector& b = sigmas_;
  size_t n = a.size();
  assert (b.size()==a.size());
  Vector c(n);
  for( size_t i = 0; i < n; i++ ) {
    const double& ai = a(i), &bi = b(i);
    c(i) = (bi==0.0) ? ai : ai/bi; // NOTE: not ediv_()
  }
  return c;
}

/* ************************************************************************* */
double Constrained::distance(const Vector& v) const {
  Vector w = Diagonal::whiten(v); // get noisemodel for constrained elements
  // TODO Find a better way of doing these checks
  for (size_t i=0; i<dim_; ++i)  // add mu weights on constrained variables
    if (!std::isfinite(1./sigmas_[i])) // whiten makes constrained variables zero
      w[i] = v[i] * sqrt(mu_[i]); // TODO: may want to store sqrt rather than rebuild
  return w.dot(w);
}

/* ************************************************************************* */
Matrix Constrained::Whiten(const Matrix& H) const {
  // selective scaling
  return vector_scale(invsigmas(), H, true);
}

/* ************************************************************************* */
void Constrained::WhitenInPlace(Matrix& H) const {
  // selective scaling
  // Scale row i of H by sigmas[i], basically multiplying H with diag(sigmas)
  // Set inf_mask flag is true so that if invsigmas[i] is inf, i.e. sigmas[i] = 0,
  // indicating a hard constraint, we leave H's row i in place.
//  Inlined: vector_scale_inplace(invsigmas(), H, true);
  // vector_scale_inplace(v, A, true);
  for (DenseIndex i=0; i<(DenseIndex)dim_; ++i) {
    const double& invsigma = invsigmas_(i);
    if (std::isfinite(1./sigmas_(i)))
      H.row(i) *= invsigma;
  }
}

/* ************************************************************************* */
void Constrained::WhitenInPlace(Eigen::Block<Matrix> H) const {
  // selective scaling
  // Scale row i of H by sigmas[i], basically multiplying H with diag(sigmas)
  // Set inf_mask flag is true so that if invsigmas[i] is inf, i.e. sigmas[i] = 0,
  // indicating a hard constraint, we leave H's row i in place.
  const Vector& _invsigmas = invsigmas();
  for(DenseIndex row = 0; row < _invsigmas.size(); ++row)
    if(isfinite(_invsigmas(row)))
      H.row(row) *= _invsigmas(row);
}

/* ************************************************************************* */
Constrained::shared_ptr Constrained::unit() const {
  Vector sigmas = ones(dim());
  for (size_t i=0; i<dim(); ++i)
    if (this->sigmas_(i) == 0.0)
      sigmas(i) = 0.0;
  return MixedSigmas(mu_, sigmas);
}

/* ************************************************************************* */
// Special version of QR for Constrained calls slower but smarter code
// that deals with possibly zero sigmas
// It is Gram-Schmidt orthogonalization rather than Householder
// Previously Diagonal::QR
SharedDiagonal Constrained::QR(Matrix& Ab) const {
  bool verbose = false;
  if (verbose) cout << "\nStarting Constrained::QR" << endl;

  // get size(A) and maxRank
  size_t m = Ab.rows(), n = Ab.cols()-1;
  size_t maxRank = min(m,n);

  // create storage for [R d]
  typedef boost::tuple<size_t, Vector, double> Triple;
  list<Triple> Rd;

  Vector pseudo(m); // allocate storage for pseudo-inverse
  Vector invsigmas = reciprocal(sigmas_);
  Vector weights = emul(invsigmas,invsigmas); // calculate weights once

  // We loop over all columns, because the columns that can be eliminated
  // are not necessarily contiguous. For each one, estimate the corresponding
  // scalar variable x as d-rS, with S the separator (remaining columns).
  // Then update A and b by substituting x with d-rS, zero-ing out x's column.
  for (size_t j=0; j<n; ++j) {
    // extract the first column of A
    Vector a = Ab.col(j);

    // Calculate weighted pseudo-inverse and corresponding precision
    gttic(constrained_QR_weightedPseudoinverse);
    double precision = weightedPseudoinverse(a, weights, pseudo);
    gttoc(constrained_QR_weightedPseudoinverse);

    // If precision is zero, no information on this column
    // This is actually not limited to constraints, could happen in Gaussian::QR
    // In that case, we're probably hosed. TODO: make sure Householder is rank-revealing
    if (precision < 1e-8) continue;

    gttic(constrained_QR_create_rd);
    // create solution [r d], rhs is automatically r(n)
    Vector rd(n+1); // uninitialized !
    rd(j)=1.0; // put 1 on diagonal
    for (size_t j2=j+1; j2<n+1; ++j2) // and fill in remainder with dot-products
      rd(j2) = pseudo.dot(Ab.col(j2));
    gttoc(constrained_QR_create_rd);

    // construct solution (r, d, sigma)
    Rd.push_back(boost::make_tuple(j, rd, precision));

    // exit after rank exhausted
    if (Rd.size()>=maxRank) break;

    // update Ab, expensive, using outer product
    gttic(constrained_QR_update_Ab);
    Ab.middleCols(j+1,n-j) -= a * rd.segment(j+1, n-j).transpose();
    gttoc(constrained_QR_update_Ab);
  }

  // Create storage for precisions
  Vector precisions(Rd.size());

  gttic(constrained_QR_write_back_into_Ab);
  // Write back result in Ab, imperative as we are
  // TODO: test that is correct if a column was skipped !!!!
  size_t i = 0; // start with first row
  bool mixed = false;
  BOOST_FOREACH(const Triple& t, Rd) {
    const size_t& j  = t.get<0>();
    const Vector& rd = t.get<1>();
    precisions(i)    = t.get<2>();
    if (precisions(i)==inf) mixed = true;
    for (size_t j2=0; j2<j; ++j2)
      Ab(i,j2) = 0.0; // fill in zeros below diagonal anway
    for (size_t j2=j; j2<n+1; ++j2)
      Ab(i,j2) = rd(j2);
    i+=1;
  }
  gttoc(constrained_QR_write_back_into_Ab);

  // Must include mu, as the defaults might be higher, resulting in non-convergence
  return mixed ? Constrained::MixedPrecisions(mu_, precisions) : Diagonal::Precisions(precisions);
}

/* ************************************************************************* */
// Isotropic
/* ************************************************************************* */
Isotropic::shared_ptr Isotropic::Sigma(size_t dim, double sigma, bool smart)  {
  if (smart && fabs(sigma-1.0)<1e-9) return Unit::Create(dim);
  return shared_ptr(new Isotropic(dim, sigma));
}

/* ************************************************************************* */
Isotropic::shared_ptr Isotropic::Variance(size_t dim, double variance, bool smart)  {
  if (smart && fabs(variance-1.0)<1e-9) return Unit::Create(dim);
  return shared_ptr(new Isotropic(dim, sqrt(variance)));
}

/* ************************************************************************* */
void Isotropic::print(const string& name) const {
  cout << name << "isotropic sigma " << " " << sigma_ << endl;
}

/* ************************************************************************* */
double Isotropic::Mahalanobis(const Vector& v) const {
  return v.dot(v) * invsigma_ * invsigma_;
}

/* ************************************************************************* */
Vector Isotropic::whiten(const Vector& v) const {
  return v * invsigma_;
}

/* ************************************************************************* */
Vector Isotropic::unwhiten(const Vector& v) const {
  return v * sigma_;
}

/* ************************************************************************* */
Matrix Isotropic::Whiten(const Matrix& H) const {
  return invsigma_ * H;
}

/* ************************************************************************* */
void Isotropic::WhitenInPlace(Matrix& H) const {
  H *= invsigma_;
}

/* ************************************************************************* */
void Isotropic::WhitenInPlace(Eigen::Block<Matrix> H) const {
  H *= invsigma_;
}

/* ************************************************************************* */
// Unit
/* ************************************************************************* */
void Unit::print(const std::string& name) const {
  cout << name << "unit (" << dim_ << ") " << endl;
}

/* ************************************************************************* */
// M-Estimator
/* ************************************************************************* */

namespace mEstimator {

/** produce a weight vector according to an error vector and the implemented
 * robust function */
Vector Base::weight(const Vector &error) const {
  const size_t n = error.rows();
  Vector w(n);
  for ( size_t i = 0 ; i < n ; ++i )
    w(i) = weight(error(i));
  return w;
}

/** square root version of the weight function */
Vector Base::sqrtWeight(const Vector &error) const {
  const size_t n = error.rows();
  Vector w(n);
  for ( size_t i = 0 ; i < n ; ++i )
    w(i) = sqrtWeight(error(i));
  return w;
}

/** The following three functions reweight block matrices and a vector
 * according to their weight implementation */

void Base::reweight(Vector& error) const {
  if(reweight_ == Block) {
    const double w = sqrtWeight(error.norm());
    error *= w;
  } else {
    const Vector w = sqrtWeight(error);
    error.array() *= w.array();
  }
}

/** Reweight n block matrices with one error vector */
void Base::reweight(vector<Matrix> &A, Vector &error) const {
  if ( reweight_ == Block ) {
    const double w = sqrtWeight(error.norm());
    BOOST_FOREACH(Matrix& Aj, A) {
      Aj *= w;
    }
    error *= w;
  }
  else {
    const Vector W = sqrtWeight(error);
    BOOST_FOREACH(Matrix& Aj, A) {
      vector_scale_inplace(W,Aj);
    }
    error = emul(W, error);
  }
}

/** Reweight one block matrix with one error vector */
void Base::reweight(Matrix &A, Vector &error) const {
  if ( reweight_ == Block ) {
    const double w = sqrtWeight(error.norm());
    A *= w;
    error *= w;
  }
  else {
    const Vector W = sqrtWeight(error);
    vector_scale_inplace(W,A);
    error = emul(W, error);
  }
}

/** Reweight two block matrix with one error vector */
void Base::reweight(Matrix &A1, Matrix &A2, Vector &error) const {
  if ( reweight_ == Block ) {
    const double w = sqrtWeight(error.norm());
    A1 *= w;
    A2 *= w;
    error *= w;
  }
  else {
    const Vector W = sqrtWeight(error);
    vector_scale_inplace(W,A1);
    vector_scale_inplace(W,A2);
    error = emul(W, error);
  }
}

/** Reweight three block matrix with one error vector */
void Base::reweight(Matrix &A1, Matrix &A2, Matrix &A3, Vector &error) const {
  if ( reweight_ == Block ) {
    const double w = sqrtWeight(error.norm());
    A1 *= w;
    A2 *= w;
    A3 *= w;
    error *= w;
  }
  else {
    const Vector W = sqrtWeight(error);
    vector_scale_inplace(W,A1);
    vector_scale_inplace(W,A2);
    vector_scale_inplace(W,A3);
    error = emul(W, error);
  }
}

/* ************************************************************************* */
// Null model
/* ************************************************************************* */

void Null::print(const std::string &s="") const
{ cout << s << "null ()" << endl; }

Null::shared_ptr Null::Create()
{ return shared_ptr(new Null()); }

Fair::Fair(const double c, const ReweightScheme reweight)
  : Base(reweight), c_(c) {
  if ( c_ <= 0 ) {
    cout << "mEstimator Fair takes only positive double in constructor. forced to 1.0" << endl;
    c_ = 1.0;
  }
}

/* ************************************************************************* */
// Fair
/* ************************************************************************* */

double Fair::weight(const double &error) const
{ return 1.0 / (1.0 + fabs(error)/c_); }

void Fair::print(const std::string &s="") const
{ cout << s << "fair (" << c_ << ")" << endl; }

bool Fair::equals(const Base &expected, const double tol) const {
  const Fair* p = dynamic_cast<const Fair*> (&expected);
  if (p == NULL) return false;
  return fabs(c_ - p->c_ ) < tol;
}

Fair::shared_ptr Fair::Create(const double c, const ReweightScheme reweight)
{ return shared_ptr(new Fair(c, reweight)); }

/* ************************************************************************* */
// Huber
/* ************************************************************************* */

Huber::Huber(const double k, const ReweightScheme reweight)
  : Base(reweight), k_(k) {
  if ( k_ <= 0 ) {
    cout << "mEstimator Huber takes only positive double in constructor. forced to 1.0" << endl;
    k_ = 1.0;
  }
}

double Huber::weight(const double &error) const {
  return (error < k_) ? (1.0) : (k_ / fabs(error));
}

void Huber::print(const std::string &s="") const {
  cout << s << "huber (" << k_ << ")" << endl;
}

bool Huber::equals(const Base &expected, const double tol) const {
  const Huber* p = dynamic_cast<const Huber*>(&expected);
  if (p == NULL) return false;
  return fabs(k_ - p->k_) < tol;
}

Huber::shared_ptr Huber::Create(const double c, const ReweightScheme reweight) {
  return shared_ptr(new Huber(c, reweight));
}

/* ************************************************************************* */
// Cauchy
/* ************************************************************************* */

Cauchy::Cauchy(const double k, const ReweightScheme reweight)
  : Base(reweight), k_(k) {
  if ( k_ <= 0 ) {
    cout << "mEstimator Cauchy takes only positive double in constructor. forced to 1.0" << endl;
    k_ = 1.0;
  }
}

double Cauchy::weight(const double &error) const {
  return k_*k_ / (k_*k_ + error*error);
}

void Cauchy::print(const std::string &s="") const {
  cout << s << "cauchy (" << k_ << ")" << endl;
}

bool Cauchy::equals(const Base &expected, const double tol) const {
  const Cauchy* p = dynamic_cast<const Cauchy*>(&expected);
  if (p == NULL) return false;
  return fabs(k_ - p->k_) < tol;
}

Cauchy::shared_ptr Cauchy::Create(const double c, const ReweightScheme reweight) {
  return shared_ptr(new Cauchy(c, reweight));
}

/* ************************************************************************* */
// Tukey
/* ************************************************************************* */
Tukey::Tukey(const double c, const ReweightScheme reweight)
  : Base(reweight), c_(c) {
}

double Tukey::weight(const double &error) const {
  if (fabs(error) <= c_) {
    double xc2 = (error/c_)*(error/c_);
    double one_xc22 = (1.0-xc2)*(1.0-xc2);
    return one_xc22;
  }
  return 0.0;
}

void Tukey::print(const std::string &s="") const {
  std::cout << s << ": Tukey (" << c_ << ")" << std::endl;
}

bool Tukey::equals(const Base &expected, const double tol) const {
  const Tukey* p = dynamic_cast<const Tukey*>(&expected);
  if (p == NULL) return false;
  return fabs(c_ - p->c_) < tol;
}

Tukey::shared_ptr Tukey::Create(const double c, const ReweightScheme reweight) {
  return shared_ptr(new Tukey(c, reweight));
}

/* ************************************************************************* */
// Welsh
/* ************************************************************************* */
Welsh::Welsh(const double c, const ReweightScheme reweight)
  : Base(reweight), c_(c) {
}

double Welsh::weight(const double &error) const {
  double xc2 = (error/c_)*(error/c_);
  return std::exp(-xc2);
}

void Welsh::print(const std::string &s="") const {
  std::cout << s << ": Welsh (" << c_ << ")" << std::endl;
}

bool Welsh::equals(const Base &expected, const double tol) const {
  const Welsh* p = dynamic_cast<const Welsh*>(&expected);
  if (p == NULL) return false;
  return fabs(c_ - p->c_) < tol;
}

Welsh::shared_ptr Welsh::Create(const double c, const ReweightScheme reweight) {
  return shared_ptr(new Welsh(c, reweight));
}

} // namespace mEstimator

/* ************************************************************************* */
// Robust
/* ************************************************************************* */

void Robust::print(const std::string& name) const {
  robust_->print(name);
  noise_->print(name);
}

bool Robust::equals(const Base& expected, double tol) const {
  const Robust* p = dynamic_cast<const Robust*> (&expected);
  if (p == NULL) return false;
  return noise_->equals(*p->noise_,tol) && robust_->equals(*p->robust_,tol);
}

void Robust::WhitenSystem(Vector& b) const {
  noise_->whitenInPlace(b);
  robust_->reweight(b);
}

void Robust::WhitenSystem(vector<Matrix>& A, Vector& b) const {
  noise_->WhitenSystem(A,b);
  robust_->reweight(A,b);
}

void Robust::WhitenSystem(Matrix& A, Vector& b) const {
  noise_->WhitenSystem(A,b);
  robust_->reweight(A,b);
}

void Robust::WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const {
  noise_->WhitenSystem(A1,A2,b);
  robust_->reweight(A1,A2,b);
}

void Robust::WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const{
  noise_->WhitenSystem(A1,A2,A3,b);
  robust_->reweight(A1,A2,A3,b);
}

Robust::shared_ptr Robust::Create(
  const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise){
  return shared_ptr(new Robust(robust,noise));
}

/* ************************************************************************* */

}
} // gtsam