#include "tuw_marker_slam/ekf_slam.h"
#include "tuw_marker_slam/munkre.h"
#include <boost/math/distributions/chi_squared.hpp>

using namespace tuw;

EKFSLAM::EKFSLAM( const std::vector<double> beta ) :
    SLAMTechnique ( EKF ),
    beta_ ( beta ) {
}

void EKFSLAM::init() {
    reset_ = false;

    // initial mean
    y = cv::Mat_<double> ( 3, 1, 0.0 );

    // initial covariance
    C_Y = cv::Mat_<double> ( 3, 3, 0.0 );

    // initialize covariance submatrices pointing to the same data (no copy!)
    x = cv::Mat_<double> ( y, cv::Range ( 0, 3 ), cv::Range ( 0, 1 ) );
    C_X = cv::Mat_<double> ( C_Y, cv::Range ( 0, 3 ), cv::Range ( 0, 3 ) );

    // reset marker landmark correspondences
    f_kj.clear();
}

void EKFSLAM::cycle ( std::vector<Pose2D> &yt, cv::Mat_<double> &C_Yt, const Command &ut, const MeasurementConstPtr &zt ) {
    if ( reset_ ) init();

    // invariance check
    assert ( y.rows % 3 == 0 && y.cols == 1 );
    assert ( x.rows == 3 && x.cols == 1 );
    assert ( C_Y.rows == C_Y.cols && C_Y.rows == y.rows );
    assert ( C_X.rows == C_X.cols && C_X.rows == x.rows );

    prediction ( ut );
    if ( zt->getType() == tuw::Measurement::Type::MARKER && updateTimestamp ( zt->stamp() ) ) {
        MeasurementMarkerConstPtr z = std::static_pointer_cast<MeasurementMarker const> ( zt );

        data_association ( z );
        update();
        integration ( z );
    }

    // implicit return
    yt.resize ( y.rows/3 );
    for ( size_t i = 0; i < yt.size(); i++ ) {
        yt[i].x() = y[3*i + 0][0];
        yt[i].y() = y[3*i + 1][0];
        yt[i].theta() = y[3*i + 2][0];
    }
    C_Yt = C_Y;
}

void EKFSLAM::setConfig ( const void *config ) {
    config_ = * ( ( tuw_marker_slam::EKFSLAMConfig* ) config );
}

void EKFSLAM::prediction ( const Command &ut ) {
    if ( !config_.enable_prediction ) return;

    // control noise
    cv::Matx<double, 2, 2> M = cv::Matx<double, 2, 2> ( config_.alpha_1*ut.v() *ut.v() + config_.alpha_2*ut.w() *ut.w(), 0,
                               0, config_.alpha_3*ut.v() *ut.v() + config_.alpha_4*ut.w() *ut.w() );

    // pre-calculate needed data
    double dt = duration_last_update_.total_microseconds() /1000000.0;
    double r = ut.v() /ut.w();
    double s = sin ( x[2][0] );
    double c = cos ( x[2][0] );
    double s_dt = sin ( x[2][0] + ut.w() *dt );
    double c_dt = cos ( x[2][0] + ut.w() *dt );

    // calculate update and its derivatives (= jacobian matrices)
    cv::Vec<double, 3> dx_;
    cv::Matx<double, 3, 3> G_x_;
    cv::Matx<double, 3, 2> G_u;
    if ( std::abs ( ut.w() ) > __DBL_MIN__ ) {
        dx_ = cv::Vec<double, 3> ( -r*s + r*s_dt,
                                   r*c - r*c_dt,
                                   ut.w() *dt );
        G_x_ = cv::Matx<double, 3, 3> ( 1, 0, -r*c + r*c_dt,
                                        0, 1, -r*s + r*s_dt,
                                        0, 0, 1 );
        G_u = cv::Matx<double, 3, 2> ( ( -s + s_dt ) /ut.w(),  r * ( s - s_dt ) / ut.w() + r*c_dt*dt,
                                       ( c - c_dt ) /ut.w(), -r * ( c - c_dt ) / ut.w() + r*s_dt*dt,
                                       0, dt );
    } else {
        dx_ = cv::Vec<double, 3> ( ut.v() *dt*c,
                                   ut.v() *dt*s,
                                   0 );
        G_x_ = cv::Matx<double, 3, 3> ( 1, 0, -ut.v() *dt*s,
                                        0, 1,  ut.v() *dt*c,
                                        0, 0, 1 );
        G_u = cv::Matx<double, 3, 2> ( dt*c, -0.5*dt*dt*ut.v() *s,
                                       dt*s, 0.5*dt*dt*ut.v() *c,
                                       0, dt );
    }

    // convert from Matx/Vec to Mat_ in order to use +/* operations on submatrices
    cv::Mat_<double> dx = cv::Mat_<double> ( dx_ );
    cv::Mat_<double> G_x = cv::Mat_<double> ( G_x_ );
    cv::Mat_<double> R = cv::Mat_<double> ( G_u * M * G_u.t() );

    // update mean and covariance
    x = x + dx;
    C_X = G_x*C_X*G_x.t() + R;
    for ( size_t i = 3; i < C_Y.cols; i += 3 ) {
        // references to submatrices
        cv::Mat_<double> C_XM_i = cv::Mat_<double> ( C_Y, cv::Range ( 0, 3 ), cv::Range ( i, i + 3 ) );
        cv::Mat_<double> C_MX_i = cv::Mat_<double> ( C_Y, cv::Range ( i, i + 3 ), cv::Range ( 0, 3 ) );

        // calculation & matrix update
        C_XM_i = G_x*C_XM_i;
        C_MX_i = C_XM_i.t();
    }

    // normalize angle
    x[2][0] = angle_normalize ( x[2][0] );
}

void EKFSLAM::data_association ( const MeasurementMarkerConstPtr &zt ) {
    // initialization
    c_ij = std::vector<CorrDataPtr> ( zt->size() );
    c_ji = std::vector<CorrDataPtr> ( y.rows/3 );
    z_known.clear();
    z_new.clear();

    // gamma is a threshold such that 100*(1-alpha)% of true measurements are rejected
    boost::math::chi_squared chi_squared = boost::math::chi_squared ( 3 );
    double gamma = boost::math::quantile(chi_squared, config_.alpha);

    // find correspondences based on measurement marker IDs
    for ( size_t i = 0; i < zt->size(); i++ ) {
        if ( zt->operator[] ( i ).ids.size() == 0 ) {
            // invalid measurement
        } else {
            // valid measurement
            std::map<int, size_t>::iterator it = f_kj.find ( zt->operator[] ( i ).ids[0] );

            if ( it != f_kj.end() ) {
                // known landmark j
                size_t j = it->second;
                assert ( 0 < j && j < c_ji.size() );

                // create correspondence data
                CorrDataPtr c = std::make_shared<CorrData>();
                c->ij = std::pair<size_t, size_t> ( i, j );
                measurement ( zt, c );

                // document founded correspondence
                assert ( c_ij[i] == nullptr && c_ji[j] == nullptr );
                c_ij[i] = c;
                c_ji[j] = c;
                z_known.push_back(i);
            } else {
                // new landmark
                z_new.push_back( i );
            }
        }
    }

    // complement correspondences based on probability
    switch (config_.data_association_mode) {
        case ID:
            // Nothing to do anymore
            break;
        case NNSF_LOCAL:
            NNSF_local ( zt, gamma );
            break;
        case NNSF_GLOBAL:
            NNSF_global ( zt, gamma );
            break;
        default:
            assert ( false );
    }
}

void EKFSLAM::NNSF_local ( const MeasurementMarkerConstPtr &zt, const double gamma ) {
    std::vector<size_t> m_reject;

    // find correspondences based on Mahalanobis distance
    for ( size_t i = 0; i < c_ij.size(); i++ ) {
        assert ( c_ij[i] == nullptr || c_ij[i]->ij.first == i );

        // consider only measurements without marker IDs
        if ( zt->operator[] ( i ).ids.size() > 0) continue;
        assert ( c_ij[i] == nullptr );

        double min_d_2 = std::numeric_limits<double>::infinity();
        CorrDataPtr min_c = nullptr;
        for ( size_t j = 1; j < c_ji.size(); j++ ) {
            assert ( c_ji[j] == nullptr || c_ji[j]->ij.second == j );

            // consider only landmarks without correspondences based on measurement marker IDs
            if ( c_ji[j] != nullptr && zt->operator[] ( c_ji[j]->ij.first ).ids.size() > 0 ) continue;

            // create correspondence data
            CorrDataPtr c = std::make_shared<CorrData>();
            c->ij = std::pair<size_t, size_t> ( i, j );
            measurement ( zt, c );

            // calculate Mahalanobis distance: d = sqrt(v^T S^(-1) v)
            double d_2 = ( c->v.t() * c->S_inv * c->v ) [0];
            if (d_2 < min_d_2) {
                min_d_2 = d_2;
                min_c = c;
            }
        }

        // update correspondences
        if ( min_c == nullptr || min_d_2 > gamma ) continue;

        // new (possible) correspondence found
        size_t min_i = min_c->ij.first;
        size_t min_j = min_c->ij.second;
        assert ( i == min_i && 0 < min_j && min_j < c_ji.size() );

        if ( c_ji[min_j] != nullptr ) {
            // landmark already correspond to another measurement
            size_t old_i = c_ji[min_j]->ij.first;
            ROS_INFO ( "measurements %lu and %lu correspond both to landmark %lu", old_i, min_i, min_j );
            if ( zt->operator[] ( old_i ).ids.size() > 0 ) {
                // old correspondence is fix -> reject current measurement
                ROS_INFO ( "reject measurement %lu", min_i );
            } else {
                // old correspondence is just probably -> reject both measurements
                ROS_INFO ( "reject measurement %lu (conservative approach)", i );

                if ( c_ij[old_i] != nullptr ) {
                    ROS_INFO ( "reject measurement %lu (conservative approach)", old_i );

                    // remove already added old measurement from known measurements
                    std::vector<size_t>::iterator it = std::find ( z_known.begin(), z_known.end(), old_i );
                    assert ( it != z_known.end() );
                    z_known.erase( it );

                    // reset old correspondence c_ij at the first time and
                    // remember old correspondence c_ji for later reset
                    c_ij[old_i] = nullptr;
                    m_reject.push_back( min_j );
                }
            }
        } else {
            // document founded correspondence
            assert ( c_ij[min_i] == nullptr && c_ji[min_j] == nullptr );
            c_ij[min_i] = min_c;
            c_ji[min_j] = min_c;
            z_known.push_back( min_i );
        }
    }

    // reset correspondences c_ji found but invalidated again during corresponde update
    for (auto j: m_reject) c_ji[j] = nullptr;
}

void EKFSLAM::NNSF_global ( const MeasurementMarkerConstPtr &zt, const double gamma ) {
    // book keeping: measurements
    std::vector<size_t> map_i;
    for ( size_t i = 0; i < c_ij.size(); i++ ) {
        assert ( c_ij[i] == nullptr || c_ij[i]->ij.first == i );

        // consider only measurements without marker IDs
        if ( zt->operator[] ( i ).ids.size() > 0 ) continue;
        assert ( c_ij[i] == nullptr );

        map_i.push_back(i);
    }

    // no further correspondences possible
    if (map_i.size() == 0) return;

    // book keeping: landmarks
    std::vector<size_t> map_j;
    for ( size_t j = 1; j < c_ji.size(); j++ ) {
        assert ( c_ji[j] == nullptr || c_ji[j]->ij.second == j );

        // consider only landmarks without correspondences based on measurement marker IDs
        assert ( c_ji[j] == nullptr || zt->operator[] ( c_ji[j]->ij.first ).ids.size() > 0);
        if ( c_ji[j] != nullptr ) continue;

        map_j.push_back(j);
    }

    // no further correspondences possible
    if (map_j.size() == 0) return;

    // build assignment matrix (Mahalanobis distance)
    std::vector<std::vector<CorrDataPtr>> C = std::vector<std::vector<CorrDataPtr>> ( map_i.size() );
    cv::Mat_<double> D_2 = cv::Mat_<double> ( map_i.size(), map_j.size() );
    for ( size_t x = 0; x < map_i.size(); x++ ) {
        C[x] = std::vector<CorrDataPtr> ( map_j.size() );
        for ( size_t y = 0; y < map_j.size(); y++ ) {
            // create correspondence data
            CorrDataPtr c = std::make_shared<CorrData>();
            c->ij = std::pair<size_t, size_t> ( map_i[x], map_j[y] );
            measurement ( zt, c );

            // calculate Mahalanobis distance: d = sqrt(v^T S^(-1) v)
            double d_2 = ( c->v.t() * c->S_inv * c->v ) [0];
            assert ( d_2 >= 0 );

            // book keeping: correspondence data
            C[x][y] = c;
            D_2[x][y] = d_2;
        }
    }

    // update correspondences
    std::vector<std::pair<size_t, size_t>> assignment = Munkre::find_minimum_assignment(D_2);
    for (size_t k = 0; k < assignment.size(); k++) {
        // book keeping measurement and landmark
        size_t x = assignment[k].first;
        size_t y = assignment[k].second;

        if ( D_2[x][y] > gamma ) continue;

        // new correspondence found
        assert ( x < C.size() && y < C[x].size() );
        size_t i = C[x][y]->ij.first;
        size_t j = C[x][y]->ij.second;
        assert ( i < c_ij.size() && 0 < j && j < c_ji.size() );

        // document founded correspondence
        assert ( c_ij[i] == nullptr && c_ji[j] == nullptr );
        c_ij[i] = C[x][y];
        c_ji[j] = C[x][y];
        z_known.push_back( i );
    }
}

void EKFSLAM::measurement ( const MeasurementMarkerConstPtr &zt, const CorrDataPtr &corr ) {
    const size_t i = corr->ij.first;
    const size_t j = corr->ij.second;
    assert ( i < zt->size() && 0 < j && j < y.rows/3 );

    // transformation matrix from robot coordinates in global coordinates
    Pose2D p(x[0][0], x[1][0], x[2][0] );
    cv::Matx<double, 4, 4> T_x = cv::Matx44d ( p.theta_cos(), -p.theta_sin(), 0, p.x(), p.theta_sin(), p.theta_cos(), 0, p.y(), 0, 0, 1, p.theta(), 0, 0, 0, 1 );

    // measurement prediction of landmark j
    cv::Vec<double, 4> homogenious_stage_vector = append(zt->pose2d().state_vector(), 1);
    cv::Vec<double, 4> sensor = T_x * homogenious_stage_vector;
    cv::Vec<double, 2> delta = cv::Vec<double, 2> ( y[3*j + 0][0] - sensor[0],
                               y[3*j + 1][0] - sensor[1] );
    double q = ( delta.t() * delta ) [0];

    // predicted measurement
    cv::Vec<double, 3> z_ = cv::Vec<double, 3> ( sqrt ( q ),
                            angle_difference ( atan2 ( delta[1], delta[0] ), sensor[2] ),
                            angle_difference ( y[3*j + 2][0], sensor[2] ) );

    // obtained measurement
    cv::Vec<double, 3> z = cv::Vec<double, 3> ( zt->operator[] ( i ).length,
                           zt->operator[] ( i ).angle ,
                           zt->operator[] ( i ).orientation );

    // compare predicted and obtainend measurement
    corr->v[0] = z[0] - z_[0];
    corr->v[1] = angle_difference ( z[1], z_[1] );
    corr->v[2] = angle_difference ( z[2], z_[2] );
    //corr->v[2] = 0;

    // pre-calculate needed data
    assert ( q > 0 );
    double s = sin ( x[2][0] );
    double c = cos ( x[2][0] );
    double ddeltax =  zt->pose2d().x() *s + zt->pose2d().y() *c;
    double ddeltay = -zt->pose2d().x() *c + zt->pose2d().y() *s;
    double dx_sq = delta[0]/sqrt ( q );
    double dy_sq = delta[1]/sqrt ( q );
    double dx_q = delta[0]/q;
    double dy_q = delta[1]/q;

    //H_ij = (dx 0 .. 0 dm 0 .. 0)
    corr->dx = cv::Matx<double, 3, 3> ( -dx_sq, -dy_sq, dx_sq*ddeltax + dy_sq*ddeltay,
                                        dy_q, -dx_q, ddeltay*dx_q - ddeltax*dy_q - 1,
                                        0, 0, -1 );

    corr->dm = cv::Matx<double, 3, 3> ( dx_sq, dy_sq, 0,
                                        -dy_q, dx_q, 0,
                                        0, 0, 1 );

    // measurement noise
    corr->Q = measurement_noise( zt->operator[]( i ) );

    // S_i = H_i*C_Y*H_i^T + Q
    cv::Matx<double, 3, 3> C_X_ = cv::Mat_<double> ( C_Y, cv::Range ( 0, 3 ), cv::Range ( 0, 3 ) );
    cv::Matx<double, 3, 3> C_M = cv::Mat_<double> ( C_Y, cv::Range ( 3*j, 3*j + 3 ), cv::Range ( 3*j, 3*j + 3 ) );
    cv::Matx<double, 3, 3> C_XM = cv::Mat_<double> ( C_Y, cv::Range ( 0, 3 ), cv::Range ( 3*j, 3*j + 3 ) );

    cv::Matx<double, 3, 3> tmp = corr->dx*C_XM*corr->dm.t();
    cv::Matx<double, 3, 3> S = corr->dx*C_X_*corr->dx.t() + tmp + tmp.t() + corr->dm*C_M*corr->dm.t() + corr->Q;
    corr->S_inv = S.inv();
}

cv::Matx<double, 3, 3> EKFSLAM::measurement_noise ( MeasurementMarker::Marker zi ) {
    double l = zi.length;
    double a = zi.angle;
    double o = zi.orientation;

    double var_l = std::max<double> ( beta_[0]*l*l  + beta_[1], 0 )  + std::max<double> ( std::min<double> ( beta_[2]*a*a  + beta_[3], M_PI*M_PI ), 0 )  + std::max<double> ( std::min<double> ( beta_[4]*o*o  + beta_[5], M_PI*M_PI ), 0 );
    double var_a = std::min<double> ( std::max<double> ( beta_[6]/l/l  + beta_[7], 0 )  + std::max<double> ( std::min<double> ( beta_[8]*a*a  + beta_[9], M_PI*M_PI ), 0 )  + std::max<double> ( std::min<double> ( beta_[10]*o*o + beta_[11], M_PI*M_PI ), 0 ), M_PI*M_PI );
    double var_o = std::min<double> ( std::max<double> ( beta_[12]*l*l + beta_[13], 0 ) + std::max<double> ( std::min<double> ( beta_[14]*a*a + beta_[15], M_PI*M_PI ), 0 ) + std::max<double> ( std::min<double> ( beta_[16]*o*o + beta_[17], M_PI*M_PI ), 0 ), M_PI*M_PI );

    return cv::Matx<double, 3,3> ( var_l, 0, 0,
                                   0, var_a, 0,
                                   0, 0, var_o );
}

void EKFSLAM::update() {
    switch (config_.update_mode) {
        case None:
            // Nothing to do
            break;
        case Single:
            update_single();
            break;
        case Combined:
            update_combined();
            break;
        default:
            assert ( false );
    }
}

void EKFSLAM::update_single() {
    if ( z_known.size() == 0 ) return;

    for ( auto i: z_known ) {
        assert ( i < c_ij.size() && c_ij[i] != nullptr );

        // obtained measurement i corresponds to landmark j
        int j = c_ij[i]->ij.second;
        assert ( 0 < j && j < y.rows/3 && c_ji[j] == c_ij[i] );

        // fetch needed data
        cv::Mat_<double> v = cv::Mat_<double> ( c_ij[i]->v );
        cv::Mat_<double> S_inv = cv::Mat_<double> ( c_ij[i]->S_inv );

        // K_i = C_Y*H_i^T*S_i^(-1)
        cv::Mat_<double> K_i = cv::Mat_<double> ( C_Y.rows, 3, 0.0 );
        for ( size_t k = 0; k < K_i.rows; k += 3 ) {
            // reference to submatrix
            cv::Mat_<double> K_i_k = cv::Mat_<double> ( K_i, cv::Range ( k, k + 3 ), cv::Range ( 0, 3 ) );

            // calculation & matrix update
            cv::Matx<double, 3, 3> C_MX_k = cv::Mat_<double> ( C_Y, cv::Range ( k, k + 3 ), cv::Range ( 0, 3 ) );
            cv::Matx<double, 3, 3> C_M_kj = cv::Mat_<double> ( C_Y, cv::Range ( k, k + 3 ), cv::Range ( 3*j, 3*j + 3 ) );
            cv::Mat_<double> tmp = cv::Mat_<double> ( C_MX_k*c_ij[i]->dx.t() + C_M_kj*c_ij[i]->dm.t() );
            K_i_k = tmp * S_inv;
        }

        // I - K_i*H_i
        cv::Mat_<double> I_K_i_H_i = cv::Mat_<double>::eye ( C_Y.rows,  C_Y.cols );
        for ( size_t k = 0; k < C_Y.rows; k += 3 ) {
            // references to submatrices
            cv::Mat_<double> I_K_i_H_i_k0 = cv::Mat_<double> ( I_K_i_H_i, cv::Range ( k, k + 3 ), cv::Range ( 0, 3 ) );
            cv::Mat_<double> I_K_i_H_i_kj = cv::Mat_<double> ( I_K_i_H_i, cv::Range ( k, k + 3 ), cv::Range ( 3*j, 3*j + 3 ) );

            // calculation & matrix update
            cv::Matx<double, 3, 3> K_i_k = cv::Mat_<double> ( K_i, cv::Range ( k, k + 3 ), cv::Range ( 0, 3 ) );
            I_K_i_H_i_k0 -= cv::Mat_<double> ( K_i_k * c_ij[i]->dx );
            I_K_i_H_i_kj -= cv::Mat_<double> ( K_i_k * c_ij[i]->dm );
        }

        // update mean and covariance
        y = y + K_i*v;
        C_Y = I_K_i_H_i*C_Y;

        // normalize angles
        for ( size_t i = 2; i < y.rows; i += 3 )
            y[i][0] = angle_normalize ( y[i][0] );

        // re-establish diagonalization
        C_Y = 0.5 * ( C_Y + C_Y.t() );
    }
}

void EKFSLAM::update_combined() {
    if ( z_known.size() == 0 ) return;

    // initialization
    cv::Mat_<double> v = cv::Mat_<double> ( 3*z_known.size(), 1, 0.0 );
    cv::Mat_<double> H = cv::Mat_<double> ( 3*z_known.size(), C_Y.cols, 0.0 );
    cv::Mat_<double> R = cv::Mat_<double> ( 3*z_known.size(), 3*z_known.size(), 0.0 );
    size_t k = 0;

    for ( auto i: z_known ) {
        assert ( i < c_ij.size() && c_ij[i] != nullptr );

        // obtained measurement i corresponds to landmark j
        int j = c_ij[i]->ij.second;
        assert ( 0 < j && j < y.rows/3 && c_ji[j] == c_ij[i] );

        // references to submatrices
        cv::Mat_<double> v_k = cv::Mat_<double> ( v, cv::Range ( k, k + 3 ), cv::Range ( 0, 1 ) );
        cv::Mat_<double> Hx_kj = cv::Mat_<double> ( H, cv::Range ( k, k + 3 ), cv::Range ( 0, 3 ) );
        cv::Mat_<double> Hm_kj = cv::Mat_<double> ( H, cv::Range ( k, k + 3 ), cv::Range ( 3*j, 3*j + 3 ) );
        cv::Mat_<double> R_k = cv::Mat_<double> ( R, cv::Range ( k, k + 3 ), cv::Range ( k, k + 3 ) );

        // calculation & update
        // H = (H_1j H_2j' ... H_Nj'')^T
        // H_kj = (dx_kj 0 .. 0 dm_kj 0 .. 0)
        v_k += cv::Mat_<double> ( c_ij[i]->v );
        Hx_kj += cv::Mat_<double> ( c_ij[i]->dx );
        Hm_kj += cv::Mat_<double> ( c_ij[i]->dm );
        R_k += cv::Mat_<double> ( c_ij[i]->Q );
        k += 3;
    }

    // calculation
    cv::Mat_<double> S = R + H*C_Y*H.t();
    cv::Mat_<double> K = C_Y*H.t() *S.inv();

    // update mean and covariance
    y = y + K*v;
    C_Y = ( cv::Mat_<double>::eye ( C_Y.rows, C_Y.cols ) - K*H ) *C_Y;

    // normalize angles
    for ( size_t i = 2; i < y.rows; i += 3 )
        y[i][0] = angle_normalize ( y[i][0] );

    // re-establish diagonalization
    C_Y = 0.5 * ( C_Y + C_Y.t() );
}

void EKFSLAM::integration ( const MeasurementMarkerConstPtr &zt ) {
    if ( !config_.enable_integration || z_new.size() == 0 ) return;

    for ( auto i: z_new ) {
        // consider only measurements with marker IDs for new landmarks
        assert ( zt->operator[] ( i ).ids.size() > 0 );

        // measurement noise
        cv::Matx<double, 3,3> Q = measurement_noise( zt->operator[]( i ) );

        // number landmarks found in ascending order
        int j = y.rows/3;
        f_kj.insert ( std::pair<int,size_t> ( zt->operator[] ( i ).ids[0], j ) );
        ROS_INFO ( "new landmark found: marker %d corresponds to landmark %d", zt->operator[] ( i ).ids[0], j );

        // pre-calculate needed data
        double r = zt->operator[] ( i ).length;
        double T_s = sin ( x[2][0] );
        double T_c = cos ( x[2][0] );
        double F_s = sin ( zt->operator[] ( i ).angle );
        double F_c = cos ( zt->operator[] ( i ).angle );

        // transformation matrix from robot coordinates in global coordinates
        Pose2D px(x[0][0], x[1][0], x[2][0] );
        cv::Matx<double, 4, 4> T_x = cv::Matx44d ( px.theta_cos(), -px.theta_sin(), 0, px.x(), px.theta_sin(), px.theta_cos(), 0, px.y(), 0, 0, 1, px.theta(), 0, 0, 0, 1 );

        // transformation matrix from sensor coordinates in robot coordinates
        Pose2D pz = zt->pose2d();
        cv::Matx<double, 4, 4> T_z = cv::Matx44d ( pz.theta_cos(), -pz.theta_sin(), 0, pz.x(), pz.theta_sin(), pz.theta_cos(), 0, pz.y(), 0, 0, 1, pz.theta(), 0, 0, 0, 1 );

        // transformation matrix from sensor coordinates in global coordinates
        cv::Matx<double, 4, 4> T_x_T_z = T_x * T_z;

        // derivation of T_x by theta
        cv::Matx<double, 4, 4> T_x_theta = cv::Matx<double, 4, 4> ( -T_s, -T_c, 0, 0,
                                           T_c, -T_s, 0, 0,
                                           0, 0, 0, 1,
                                           0, 0, 0, 0 );

        // adapt size of global mean
        y.resize ( y.rows + 3 );

        // update gloabl mean with mean of landmark j: m_j = g(x, z) = T_x(x) * T_z * f(z)
        // with f(z) = (r*cos(a), r*sin(a), o), r..radius, a..alpha, o..orientation
        // (function f transforms measurements from spheric coordinates into cartesian coordinates)
        cv::Vec<double, 4> homogenious_stage_vector_i = append(zt->operator[] ( i ).pose.state_vector(), 1);
        cv::Vec<double, 4> m_j = T_x_T_z * homogenious_stage_vector_i;
        y[3*j + 0][0] = m_j[0];
        y[3*j + 1][0] = m_j[1];
        y[3*j + 2][0] = m_j[2];

        // jacobian matrix of f
        cv::Matx<double, 4, 3> F_z = cv::Matx<double, 4, 3> ( F_c, -r*F_s, 0,
                                     F_s,  r*F_c, 0,
                                     0,      0, 1,
                                     0,      0, 0 );

        // jacobian matrix of G in respect to x
        cv::Mat_<double> G_x = cv::Mat_<double>::eye ( 3, 3 );
        cv::Vec<double, 4> G_theta = T_x_theta * T_z * homogenious_stage_vector_i;
        G_x[0][2] = G_theta[0];
        G_x[1][2] = G_theta[1];

        // jacobian matrix of G in respect to z
        cv::Matx<double, 3, 3> G_z = cv::Mat_<double> ( cv::Mat_<double> ( T_x_T_z * F_z ), cv::Range ( 0, 3 ), cv::Range ( 0, 3 ) );

        // create horizontal covariance of landmark j
        cv::Mat_<double> C_M_j = cv::Mat_<double> ( 3, C_Y.cols, 0.0 );
        for ( size_t k = 0; k < C_M_j.cols; k += 3 ) {
            // references to submatrices
            cv::Mat_<double> C_M_jk = cv::Mat_<double> ( C_M_j, cv::Range ( 0, 3 ), cv::Range ( k, k + 3 ) );
            cv::Mat_<double> C_XM_k = cv::Mat_<double> ( C_Y, cv::Range ( 0, 3 ), cv::Range ( k, k + 3 ) );

            // calculation & matrix update
            C_M_jk = G_x*C_XM_k;
        }

        // append horizontal covariance of landmark j to global covariance
        cv::Mat tmp_v[] = { C_Y, C_M_j };
        cv::vconcat ( tmp_v, 2, C_Y );

        // create vertical covariance of landmark j
        C_M_j = C_M_j.t();
        C_M_j.resize ( C_M_j.rows + 3 );

        // reference to submatrix
        cv::Mat_<double> C_M_jj = cv::Mat_<double> ( C_M_j, cv::Range ( 3*j, 3*j + 3 ), cv::Range ( 0, 3 ) );

        // calculation & matrix update
        cv::Mat_<double> R = cv::Mat_<double> ( G_z*Q*G_z.t() );
        C_M_jj = G_x*C_X*G_x.t() + R;

        // append vertical covariance of landmark j to global covariance
        cv::Mat tmp_h[] = { C_Y, C_M_j };
        cv::hconcat ( tmp_h, 2, C_Y );

        // reinitialize covariance submatrices pointing to the same data (no copy!)
        x = cv::Mat_<double> ( y, cv::Range ( 0, 3 ), cv::Range ( 0, 1 ) );
        C_X = cv::Mat_<double> ( C_Y, cv::Range ( 0, 3 ), cv::Range ( 0, 3 ) );
    }
}


#include <boost/foreach.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/make_shared.hpp>
#include <boost/ptr_container/ptr_vector.hpp>
#include <algorithm>
#include <fstream>
#include <cmath>

#include "kinect_view.h"

namespace kinect_merge {

// This is used for deciding whether two points are similar enough to be merged.
static const float SIMILARITY_THRESHOLD = 3.0f; // in units of standard deviations

// Measurement variance constants
static const float ALPHA_0 = 0.0032225f;
static const float ALPHA_1 = -0.0020925f;
static const float ALPHA_2 = 0.0022078f;
static const float BETA_X = 0.0017228f;
static const float BETA_Y = 0.0017092f;

CPoint::CPoint() {}

CPoint::CPoint(const cv::Matx31f& position, const cv::Matx33f& covariance, const cv::Matx<unsigned char, 3, 1>& color) :
    pos(position),
    cov(covariance),
    col_sum(color),
    num_merged(1) {}

cv::Matx<unsigned char, 3, 1> CPoint::get_color() const {
    // NOTE: The following line would segfault on cv::Mat(col_sum). An alternate method follows.
    // return cv::Mat(col_sum) / num_merged;
    cv::Matx<unsigned char, 3, 1> m;
    for(int i = 0; i < 3; i++)
        m(i) = col_sum(i) / num_merged;
    return m;
}

CView::CView(const std::string &extrinsic_file,
             const std::string &disparity_file,
             const std::string &color_file,
             float covariance_scale_xy,
             float covariance_scale_z,
             CKinectCalibration &calibration_) :
    calibration(calibration_),
    original_pointmap(boost::extents[IMAGE_HEIGHT][IMAGE_WIDTH]),
    pointmap(boost::extents[IMAGE_HEIGHT][IMAGE_WIDTH]),
    num_missing(0),
#ifdef DEBUG // The measurement_used vector is used for outputting debug images.
    num_added(0),
    measurement_used(IMAGE_HEIGHT) {
#else
    num_added(0) {
#endif

    // Initialize the depth map.
    for(int v = 0; v < IMAGE_HEIGHT; v++) {
        for(int u = 0; u < IMAGE_WIDTH; u++) {
            depthmap(v, u) = -1;
        }
    }

    // Load extrinsic data.

    cv::Mat inv_transformation_tmp = cv::Mat_<float>::eye(4, 4);
    cv::Mat inv_rotation(inv_transformation_tmp, cv::Rect(0, 0, 3, 3));
    cv::Mat inv_translation(inv_transformation_tmp, cv::Rect(3, 0, 1, 3));

    try {
        cv::FileStorage fs(extrinsic_file, cv::FileStorage::READ);
        fs["R"] >> inv_rotation;
        fs["T"] >> inv_translation;
    } catch(cv::Exception &e) {
        std::cerr << std::endl << "Error reading extrinsic data: " << e.what() << std::endl;
        exit(4);
    }

    inv_transformation = inv_transformation_tmp;
    cv::Mat transformation = inv_transformation_tmp.inv();
    cv::Mat rotation = inv_rotation.t();

    // Load disparity map.
    int flags = -1; // Load as-is (greyscale). Using 0 (force grayscale) results in a segfault.
    cv::Mat1s disparitymap = cv::imread(disparity_file, flags);

    if(disparitymap.cols != IMAGE_WIDTH || disparitymap.rows != IMAGE_HEIGHT) {
        std::cerr << std::endl << "Disparity map has dimensions " << disparitymap.cols << "x" << disparitymap.rows <<
                                  " instead of " << IMAGE_WIDTH << "x" << IMAGE_HEIGHT << std::endl;
        exit(5);
    }

    // Load color map.
    cv::Mat3b colormap = cv::imread(color_file, flags);

    if(colormap.cols != IMAGE_WIDTH || colormap.rows != IMAGE_HEIGHT) {
        std::cerr << std::endl << "Color map has dimensions " << colormap.cols << "x" << colormap.rows <<
                                  " instead of " << IMAGE_WIDTH << "x" << IMAGE_HEIGHT << std::endl;
        exit(6);
    }

    // Convert to a point cloud in global coordinates and calculate the covariance matrix.
    cv::Matx31f local_point;
    cv::Mat local_point_tmp = (cv::Mat_<float>(4, 1) << 0, 0, 0, 1);
    cv::Mat global_point;
    cv::Mat covariance;
    for(int v = 0; v < disparitymap.rows; v++) {
        for(int u = 0; u < disparitymap.cols; u++) {
            const short disparity = disparitymap(v, u);
            if(disparity == 2047) {
                continue;
            }

            // Convert to local and then global coordinates.
            calibration.disparity2point(u, v, disparity, local_point);
            local_point_tmp.at<float>(0) = local_point(0);
            local_point_tmp.at<float>(1) = local_point(1);
            local_point_tmp.at<float>(2) = local_point(2);
            global_point = transformation * local_point_tmp;
            global_point.pop_back(); // Turn into a 3 by 1 matrix.

            float depth = local_point(2);
            if(depth < 0) {
                // The point projects behind the camera. Ignore it.
                continue;
            }

            // Calculate the covariance matrix.
            covariance = cv::Mat_<float>::eye(3, 3);
            covariance.at<float>(0, 0) = BETA_X * BETA_X * depth * depth / 12;
            covariance.at<float>(1, 1) = BETA_Y * BETA_Y * depth * depth / 12;
            covariance.at<float>(2, 2) = pow(ALPHA_2 * depth * depth + ALPHA_1 * depth + ALPHA_0, 2);
            covariance.at<float>(0, 0) *= covariance_scale_xy;
            covariance.at<float>(1, 1) *= covariance_scale_xy;
            covariance.at<float>(2, 2) *= covariance_scale_z;

            // Convert the covariance matrix to the global reference frame.
            covariance = rotation * covariance * inv_rotation;
            assert(covariance.at<float>(0, 0) >= 0);
            assert(covariance.at<float>(1, 1) >= 0);
            assert(covariance.at<float>(2, 2) >= 0);

            // Get the pixel coordinates for the measurement.
            int uc, vc;
            to_image_plane(local_point_tmp, uc, vc);
            if(uc < 0 || uc >= IMAGE_WIDTH || vc < 0 || vc >= IMAGE_HEIGHT) {
                continue;
            }

            if(!original_pointmap[vc][uc] || depthmap(vc, uc) > depth) {
                depthmap(vc, uc) = depth;
                original_pointmap[vc][uc] = boost::make_shared<CPoint>(global_point,
                                                                       covariance,
                                                                       colormap(vc, uc));
            }
        }
    }
}

void CView::to_image_plane(const cv::Matx41f &local_pos, int &uc, int &vc) const {
    cv::Matx31f local_pos_tmp(local_pos(0), local_pos(1), local_pos(2));
    cv::Matx21f cf;
    calibration.point2rgb(local_pos_tmp, cf);
    uc = cf(0) + 0.5;
    vc = cf(1) + 0.5;
}

static float calculate_mahalanobis_distance(const CPoint &refined_point, const CPoint &p) {
    cv::Matx<float, 1, 1> operand = (p.pos - refined_point.pos).t() * p.cov.inv() * (p.pos - refined_point.pos);
    return sqrt(operand(0));
}

// Calculate a new position estimate, covariance matrix and color for an existing point given a new measurement.
static void refine_point(const CPoint &existing_point, const CPoint &new_measurement, CPoint &refined_point) {
    cv::Matx33f new_measurement_cov_inv = new_measurement.cov.inv();

    // Combine the covariance matrices.
    refined_point.cov = (existing_point.cov.inv() + new_measurement_cov_inv).inv();

    // Calculate the refined position estimate.
    refined_point.pos = existing_point.pos + refined_point.cov * new_measurement_cov_inv * (new_measurement.pos - existing_point.pos);

    // Add the color values.
    refined_point.col_sum = existing_point.col_sum + new_measurement.col_sum;
    refined_point.num_merged = existing_point.num_merged + new_measurement.num_merged;
}

void CView::refine_points(CView &connected_view, std::vector<std::vector<bool> > &measurement_used) const {
    for(int v = 0; v < IMAGE_HEIGHT; v++) {
        for(int u = 0; u < IMAGE_WIDTH; u++) {
            if(!connected_view.has_point(u, v)) {
                continue;
            }

            // Project the point onto the current view image plane.
            // NOTE: Only points which were added to cloud from the connected view are projected.
            // Each point in the cloud is pointed to by exactly one view. This means that a single
            // point in the cloud will NOT be refined multiple times with the same new measurement
            // through different connected views.
            int proj_u, proj_v;
            float proj_depth;
            CPoint &connected_view_point = connected_view.get_point(u, v);
            project(connected_view_point, proj_u, proj_v, proj_depth);

            if(proj_depth < 0) {
                // The point projects outside the image plane.
                continue;
            }

            if(!has_measurement(proj_u, proj_v)) {
                // There is nothing to merge with at this pixel.
                continue;
            }

            const CPoint &current_view_point = get_original_point(proj_u, proj_v);

            // Calculate the refined point and use it if it's close enough to the existing point and
            // new measurement.
            CPoint refined_point;
            refine_point(connected_view_point, current_view_point, refined_point);
            if(calculate_mahalanobis_distance(refined_point, connected_view_point) < SIMILARITY_THRESHOLD &&
               calculate_mahalanobis_distance(refined_point, current_view_point) < SIMILARITY_THRESHOLD) {
                connected_view_point = refined_point;
                measurement_used[proj_v][proj_u] = true;
            }
        }
    }
}

void CView::insert_into_point_cloud(std::vector<CPoint::const_ptr> &point_cloud) {
    for(int v = 0; v < IMAGE_HEIGHT; v++) {
        for(int u = 0; u < IMAGE_WIDTH; u++) {
            if(has_measurement(u, v)) {
                point_cloud.push_back(original_pointmap[v][u]);
            }
        }
    }
}

void CView::project(const CPoint &global_point, int &proj_u, int &proj_v, float &proj_depth) const {
    cv::Matx41f global_pos(global_point.pos(0),
                           global_point.pos(1),
                           global_point.pos(2),
                           1);
    cv::Matx41f local_pos = inv_transformation * global_pos;
    to_image_plane(local_pos, proj_u, proj_v);

    if(proj_u < 0 || proj_u >= IMAGE_WIDTH ||
       proj_v < 0 || proj_v >= IMAGE_HEIGHT) {
        // The point projects outside the image plane.
        proj_depth = -1;
    } else {
        proj_depth = local_pos(2);
    }
}

void CView::merge(boost::ptr_vector<CView> &views,
                  unsigned int view_idx,
                  const cv::Mat view_connectivity,
                  std::vector<CPoint::ptr> &global_point_cloud) {
    // Initialize vectors.
    // A measurement is marked as used if an existing point is refined with it.
#ifndef DEBUG
    // Use a local variable instead of a member variable.
    std::vector<std::vector<bool> > measurement_used(IMAGE_HEIGHT);
#endif
    for(int v = 0; v < IMAGE_HEIGHT; v++) {
        measurement_used[v].resize(IMAGE_WIDTH);
        for(int u = 0; u < IMAGE_WIDTH; u++) {
            measurement_used[v][u] = false;
        }
    }

    // Project all connected views into the current view to find points to be refined.
    for(unsigned int connected_idx = 0; connected_idx < view_idx; connected_idx++) {
        if(!view_connectivity.at<bool>(view_idx, connected_idx)) {
            // The view is not connected. Skip it.
            continue;
        }

        CView &connected_view = views[connected_idx];

        refine_points(connected_view, measurement_used);
    }

    // Add all the points that weren't used to refine existing points to the point cloud.
    for(int v = 0; v < IMAGE_HEIGHT; v++) {
        for(int u = 0; u < IMAGE_WIDTH; u++) {
            if(has_measurement(u, v)) {
                if(!measurement_used[v][u]) {
                    pointmap[v][u] = original_pointmap[v][u];
                    global_point_cloud.push_back(pointmap[v][u]);
                    num_added++;
                }
            } else {
                num_missing++;
            }
        }
    }
}

}


/*
	Satellite Attitude Dynamics Stepper

	@author		:	siddharth deore
	@licence	:	MIT
*/

#define _CRT_SECURE_NO_WARNINGS
#include <iostream>
// reading a text file
#include <iostream>
#include <fstream>
#include <string>

#include <boost/array.hpp>
#include <boost/numeric/odeint.hpp>
#include "Satellite.h"
#include <ctime>
using namespace std;
using namespace boost::numeric::odeint;

int main(int argc, char** argv)
{
	system("cls");
	// Initial condition
	double IC[7] = { 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0 };
	// Read Configration file for initial condiion Each line is state, TODO:: some kind of parser!
	string line;
	ifstream myfile("conf.txt");
	int i = 0;
	if (myfile.is_open())
	{
		while (getline(myfile, line))
		{
			IC[i++] = std::stod(line);
			//cout << IC[i-1] <<std::endl;
		}
		myfile.close();
	}

	else cout << "Unable to open file";
	// Clean Results file

	std::ofstream outfile;

	outfile.open("results.csv"); // overwrite
	outfile << "time, q0, q1, q2, q3, wx, wy, wz" << endl;
	outfile.close();

	Satellite sat;
	Satellite::state_type x;
	time_t t1, t2;
	//double q0, q1, q2, q3, w0, w1, w2, w3;
	sat.setInnertia(1.0, 2.0, 3.0);
	sat.setControllerGains(0.005, 0.01); // (Kp,Kd)
	sat.setTargetQuaternion(0.0, 1.0, 0.0, 0.0); //(q0,q1,q2,q3)
	sat.setState(IC[0], IC[1], IC[2], IC[3], IC[4], IC[5], IC[6]);
	time(&t1);
	sat.step(10000.0, 0.01, x);
	time(&t2);
	std::cout << "Execution time " << t2 - t1 << std::endl;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2011 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to 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)

#ifndef BOOST_GEOMETRY_MULTI_ALGORITHMS_INTERSECTION_HPP
#define BOOST_GEOMETRY_MULTI_ALGORITHMS_INTERSECTION_HPP

#include <boost/geometry/multi/core/closure.hpp>
#include <boost/geometry/multi/core/geometry_id.hpp>
#include <boost/geometry/multi/core/is_areal.hpp>
#include <boost/geometry/multi/core/point_order.hpp>
#include <boost/geometry/multi/algorithms/envelope.hpp>
#include <boost/geometry/multi/algorithms/num_points.hpp>
#include <boost/geometry/multi/algorithms/detail/overlay/get_ring.hpp>
#include <boost/geometry/multi/algorithms/detail/overlay/get_turns.hpp>
#include <boost/geometry/multi/algorithms/detail/overlay/copy_segments.hpp>
#include <boost/geometry/multi/algorithms/detail/overlay/copy_segment_point.hpp>
#include <boost/geometry/multi/algorithms/detail/overlay/select_rings.hpp>
#include <boost/geometry/multi/algorithms/detail/sections/range_by_section.hpp>
#include <boost/geometry/multi/algorithms/detail/sections/sectionalize.hpp>

#include <boost/geometry/algorithms/intersection.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace intersection
{

template
<
    typename MultiLinestring1, typename MultiLinestring2,
    typename OutputIterator, typename PointOut,
    typename Strategy
>
struct intersection_multi_linestring_multi_linestring_point
{
    static inline OutputIterator apply(MultiLinestring1 const& ml1,
            MultiLinestring2 const& ml2, OutputIterator out,
            Strategy const& strategy)
    {
        // Note, this loop is quadratic w.r.t. number of linestrings per input.
        // Future Enhancement: first do the sections of each, then intersect.
        for (typename boost::range_iterator
                <
                    MultiLinestring1 const
                >::type it1 = boost::begin(ml1);
            it1 != boost::end(ml1);
            ++it1)
        {
            for (typename boost::range_iterator
                    <
                        MultiLinestring2 const
                    >::type it2 = boost::begin(ml2);
                it2 != boost::end(ml2);
                ++it2)
            {
                out = intersection_linestring_linestring_point
                    <
                        typename boost::range_value<MultiLinestring1>::type,
                        typename boost::range_value<MultiLinestring2>::type,
                        OutputIterator, PointOut, Strategy
                    >::apply(*it1, *it2, out, strategy);
            }
        }

        return out;
    }
};

template
<
    typename Linestring, typename MultiLinestring,
    typename OutputIterator, typename PointOut,
    typename Strategy
>
struct intersection_linestring_multi_linestring_point
{
    static inline OutputIterator apply(Linestring const& linestring,
            MultiLinestring const& ml, OutputIterator out,
            Strategy const& strategy)
    {
        for (typename boost::range_iterator
                <
                    MultiLinestring const
                >::type it = boost::begin(ml);
            it != boost::end(ml);
            ++it)
        {
            out = intersection_linestring_linestring_point
                <
                    Linestring,
                    typename boost::range_value<MultiLinestring>::type,
                    OutputIterator, PointOut, Strategy
                >::apply(linestring, *it, out, strategy);
        }

        return out;
    }
};

template
<
    typename MultiLinestring, typename Box,
    typename OutputIterator, typename LinestringOut,
    typename Strategy
>
struct clip_multi_linestring
{
    static inline OutputIterator apply(MultiLinestring const& multi_linestring,
            Box const& box, OutputIterator out, Strategy const& strategy)
    {
        typedef typename point_type<LinestringOut>::type point_type;
        strategy::intersection::liang_barsky<Box, point_type> lb_strategy;
        for (typename boost::range_iterator<MultiLinestring const>::type it
            = boost::begin(multi_linestring);
            it != boost::end(multi_linestring); ++it)
        {
            out = detail::intersection::clip_range_with_box
                <LinestringOut>(box, *it, out, lb_strategy);
        }
        return out;
    }
};

}} // namespace detail::intersection
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

// Linear
template
<
    typename MultiLinestring1, typename MultiLinestring2,
    bool Reverse1, bool Reverse2, bool ReverseOut,
    typename OutputIterator, typename GeometryOut,
    overlay_type OverlayType,
    typename Strategy
>
struct intersection_insert
    <
        multi_linestring_tag, multi_linestring_tag, point_tag,
        false, false, false,
        MultiLinestring1, MultiLinestring2,
        Reverse1, Reverse2, ReverseOut,
        OutputIterator, GeometryOut,
        OverlayType,
        Strategy
    > : detail::intersection::intersection_multi_linestring_multi_linestring_point
            <
                MultiLinestring1, MultiLinestring2,
                OutputIterator, GeometryOut,
                Strategy
            >
{};

template
<
    typename Linestring, typename MultiLinestring,
    typename OutputIterator, typename GeometryOut,
    bool Reverse1, bool Reverse2, bool ReverseOut,
    overlay_type OverlayType,
    typename Strategy
>
struct intersection_insert
    <
        linestring_tag, multi_linestring_tag, point_tag,
        false, false, false,
        Linestring, MultiLinestring,
        Reverse1, Reverse2, ReverseOut,
        OutputIterator, GeometryOut,
        OverlayType,
        Strategy
    > : detail::intersection::intersection_linestring_multi_linestring_point
            <
                Linestring, MultiLinestring,
                OutputIterator, GeometryOut,
                Strategy
            >
{};

template
<
    typename MultiLinestring, typename Box,
    bool Reverse1, bool Reverse2, bool ReverseOut,
    typename OutputIterator, typename GeometryOut,
    overlay_type OverlayType,
    typename Strategy
>
struct intersection_insert
    <
        multi_linestring_tag, box_tag, linestring_tag,
        false, true, false,
        MultiLinestring, Box,
        Reverse1, Reverse2, ReverseOut,
        OutputIterator, GeometryOut,
        OverlayType,
        Strategy
    > : detail::intersection::clip_multi_linestring
            <
                MultiLinestring, Box,
                OutputIterator, GeometryOut,
                Strategy
            >
{};

} // namespace dispatch
#endif

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_MULTI_ALGORITHMS_INTERSECTION_HPP


// Copyright (C) 2004-2006 The Trustees of Indiana University.

// Use, modification and distribution is subject to 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)

//  Authors: Douglas Gregor
//           Andrew Lumsdaine

/**
 * This header implements four distributed algorithms to compute
 * the minimum spanning tree (actually, minimum spanning forest) of a
 * graph. All of the algorithms were implemented as specified in the
 * paper by Dehne and Gotz:
 *
 *   Frank Dehne and Silvia Gotz. Practical Parallel Algorithms for Minimum
 *   Spanning Trees. In Symposium on Reliable Distributed Systems,
 *   pages 366--371, 1998.
 *
 * There are four algorithm variants implemented.
 */

#ifndef BOOST_DEHNE_GOTZ_MIN_SPANNING_TREE_HPP
#define BOOST_DEHNE_GOTZ_MIN_SPANNING_TREE_HPP

#ifndef BOOST_GRAPH_USE_MPI
#error "Parallel BGL files should not be included unless <boost/graph/use_mpi.hpp> has been included"
#endif

#include <boost/graph/graph_traits.hpp>
#include <boost/property_map/property_map.hpp>
#include <vector>
#include <boost/graph/parallel/algorithm.hpp>
#include <boost/limits.hpp>
#include <utility>
#include <boost/pending/disjoint_sets.hpp>
#include <boost/pending/indirect_cmp.hpp>
#include <boost/property_map/parallel/caching_property_map.hpp>
#include <boost/graph/vertex_and_edge_range.hpp>
#include <boost/graph/kruskal_min_spanning_tree.hpp>
#include <boost/iterator/counting_iterator.hpp>
#include <boost/iterator/transform_iterator.hpp>
#include <boost/graph/parallel/container_traits.hpp>
#include <boost/graph/parallel/detail/untracked_pair.hpp>
#include <cmath>

namespace boost { namespace graph { namespace distributed {

namespace detail {
  /**
   * Binary function object type that selects the (edge, weight) pair
   * with the minimum weight. Used within a Boruvka merge step to select
   * the candidate edges incident to each supervertex.
   */
  struct smaller_weighted_edge
  {
    template<typename Edge, typename Weight>
    std::pair<Edge, Weight>
    operator()(const std::pair<Edge, Weight>& x,
               const std::pair<Edge, Weight>& y) const
    { return x.second < y.second? x : y; }
  };

  /**
   * Unary predicate that determines if the source and target vertices
   * of the given edge have the same representative within a disjoint
   * sets data structure. Used to indicate when an edge is now a
   * self-loop because of supervertex merging in Boruvka's algorithm.
   */
  template<typename DisjointSets, typename Graph>
  class do_has_same_supervertex
  {
  public:
    typedef typename graph_traits<Graph>::edge_descriptor edge_descriptor;

    do_has_same_supervertex(DisjointSets& dset, const Graph& g)
      : dset(dset), g(g) { }

    bool operator()(edge_descriptor e)
    { return dset.find_set(source(e, g)) == dset.find_set(target(e, g));    }

  private:
    DisjointSets&  dset;
    const Graph&   g;
  };

  /**
   * Build a @ref do_has_same_supervertex object.
   */
  template<typename DisjointSets, typename Graph>
  inline do_has_same_supervertex<DisjointSets, Graph>
  has_same_supervertex(DisjointSets& dset, const Graph& g)
  { return do_has_same_supervertex<DisjointSets, Graph>(dset, g); }

  /** \brief A single distributed Boruvka merge step.
   *
   * A distributed Boruvka merge step involves computing (globally)
   * the minimum weight edges incident on each supervertex and then
   * merging supervertices along these edges. Once supervertices are
   * merged, self-loops are eliminated.
   *
   * The set of parameters passed to this algorithm is large, and
   * considering this algorithm in isolation there are several
   * redundancies. However, the more asymptotically efficient
   * distributed MSF algorithms require mixing Boruvka steps with the
   * merging of local MSFs (implemented in
   * merge_local_minimum_spanning_trees_step): the interaction of the
   * two algorithms mandates the addition of these parameters.
   *
   * \param pg The process group over which communication should be
   * performed. Within the distributed Boruvka algorithm, this will be
   * equivalent to \code process_group(g); however, in the context of
   * the mixed MSF algorithms, the process group @p pg will be a
   * (non-strict) process subgroup of \code process_group(g).
   *
   * \param g The underlying graph on which the MSF is being
   * computed. The type of @p g must model DistributedGraph, but there
   * are no other requirements because the edge and (super)vertex
   * lists are passed separately.
   *
   * \param weight_map Property map containing the weights of each
   * edge. The type of this property map must model
   * ReadablePropertyMap and must support caching.
   *
   * \param out An output iterator that will be written with the set
   * of edges selected to build the MSF. Every process within the
   * process group @p pg will receive all edges in the MSF.
   *
   * \param dset Disjoint sets data structure mapping from vertices in
   * the graph @p g to their representative supervertex.
   *
   * \param supervertex_map Mapping from supervertex descriptors to
   * indices.
   *
   * \param supervertices A vector containing all of the
   * supervertices. Will be modified to include only the remaining
   * supervertices after merging occurs.
   *
   * \param edge_list The list of edges that remain in the graph. This
   * list will be pruned to remove self-loops once MSF edges have been
   * found.
   */
  template<typename ProcessGroup, typename Graph, typename WeightMap,
           typename OutputIterator, typename RankMap, typename ParentMap,
           typename SupervertexMap, typename Vertex, typename EdgeList>
  OutputIterator
  boruvka_merge_step(ProcessGroup pg, const Graph& g, WeightMap weight_map,
                     OutputIterator out,
                     disjoint_sets<RankMap, ParentMap>& dset,
                     SupervertexMap supervertex_map,
                     std::vector<Vertex>& supervertices,
                     EdgeList& edge_list)
  {
    typedef typename graph_traits<Graph>::vertex_descriptor
                                                           vertex_descriptor;
    typedef typename graph_traits<Graph>::vertices_size_type
                                                           vertices_size_type;
    typedef typename graph_traits<Graph>::edge_descriptor  edge_descriptor;
    typedef typename EdgeList::iterator                    edge_iterator;
    typedef typename property_traits<WeightMap>::value_type
                                                           weight_type;
    typedef boost::parallel::detail::untracked_pair<edge_descriptor,
                                       weight_type>        w_edge;
    typedef typename property_traits<SupervertexMap>::value_type
                                                           supervertex_index;

    smaller_weighted_edge min_edge;
    weight_type inf = (std::numeric_limits<weight_type>::max)();

    // Renumber the supervertices
    for (std::size_t i = 0; i < supervertices.size(); ++i)
      put(supervertex_map, supervertices[i], i);

    // BSP-B1: Find local minimum-weight edges for each supervertex
    std::vector<w_edge> candidate_edges(supervertices.size(),
                                        w_edge(edge_descriptor(), inf));
    for (edge_iterator ei = edge_list.begin(); ei != edge_list.end(); ++ei) {
      weight_type w = get(weight_map, *ei);
      supervertex_index u =
        get(supervertex_map, dset.find_set(source(*ei, g)));
      supervertex_index v =
        get(supervertex_map, dset.find_set(target(*ei, g)));

      if (u != v) {
        candidate_edges[u] = min_edge(candidate_edges[u], w_edge(*ei, w));
        candidate_edges[v] = min_edge(candidate_edges[v], w_edge(*ei, w));
      }
    }

    // BSP-B2 (a): Compute global minimum edges for each supervertex
    all_reduce(pg,
               &candidate_edges[0],
               &candidate_edges[0] + candidate_edges.size(),
               &candidate_edges[0], min_edge);

    // BSP-B2 (b): Use the edges to compute sequentially the new
    // connected components and emit the edges.
    for (vertices_size_type i = 0; i < candidate_edges.size(); ++i) {
      if (candidate_edges[i].second != inf) {
        edge_descriptor e = candidate_edges[i].first;
        vertex_descriptor u = dset.find_set(source(e, g));
        vertex_descriptor v = dset.find_set(target(e, g));
        if (u != v) {
          // Emit the edge, but cache the weight so everyone knows it
          cache(weight_map, e, candidate_edges[i].second);
          *out++ = e;

          // Link the two supervertices
          dset.link(u, v);

          // Whichever vertex was reparented will be removed from the
          // list of supervertices.
          vertex_descriptor victim = u;
          if (dset.find_set(u) == u) victim = v;
          supervertices[get(supervertex_map, victim)] =
            graph_traits<Graph>::null_vertex();
        }
      }
    }

    // BSP-B3: Eliminate self-loops
    edge_list.erase(std::remove_if(edge_list.begin(), edge_list.end(),
                                   has_same_supervertex(dset, g)),
                    edge_list.end());

    // TBD: might also eliminate multiple edges between supervertices
    // when the edges do not have the best weight, but this is not
    // strictly necessary.

    // Eliminate supervertices that have been absorbed
    supervertices.erase(std::remove(supervertices.begin(),
                                    supervertices.end(),
                                    graph_traits<Graph>::null_vertex()),
                        supervertices.end());

    return out;
  }

  /**
   * An edge descriptor adaptor that reroutes the source and target
   * edges to different vertices, but retains the original edge
   * descriptor for, e.g., property maps. This is used when we want to
   * turn a set of edges in the overall graph into a set of edges
   * between supervertices.
   */
  template<typename Graph>
  struct supervertex_edge_descriptor
  {
    typedef supervertex_edge_descriptor self_type;
    typedef typename graph_traits<Graph>::vertex_descriptor Vertex;
    typedef typename graph_traits<Graph>::edge_descriptor Edge;

    Vertex source;
    Vertex target;
    Edge e;

    operator Edge() const { return e; }

    friend inline bool operator==(const self_type& x, const self_type& y)
    { return x.e == y.e; }

    friend inline bool operator!=(const self_type& x, const self_type& y)
    { return x.e != y.e; }
  };

  template<typename Graph>
  inline typename supervertex_edge_descriptor<Graph>::Vertex
  source(supervertex_edge_descriptor<Graph> se, const Graph&)
  { return se.source; }

  template<typename Graph>
  inline typename supervertex_edge_descriptor<Graph>::Vertex
  target(supervertex_edge_descriptor<Graph> se, const Graph&)
  { return se.target; }

  /**
   * Build a supervertex edge descriptor from a normal edge descriptor
   * using the given disjoint sets data structure to identify
   * supervertex representatives.
   */
  template<typename Graph, typename DisjointSets>
  struct build_supervertex_edge_descriptor
  {
    typedef typename graph_traits<Graph>::vertex_descriptor Vertex;
    typedef typename graph_traits<Graph>::edge_descriptor   Edge;

    typedef Edge argument_type;
    typedef supervertex_edge_descriptor<Graph> result_type;

    build_supervertex_edge_descriptor() : g(0), dsets(0) { }

    build_supervertex_edge_descriptor(const Graph& g, DisjointSets& dsets)
      : g(&g), dsets(&dsets) { }

    result_type operator()(argument_type e) const
    {
      result_type result;
      result.source = dsets->find_set(source(e, *g));
      result.target = dsets->find_set(target(e, *g));
      result.e = e;
      return result;
    }

  private:
    const Graph* g;
    DisjointSets* dsets;
  };

  template<typename Graph, typename DisjointSets>
  inline build_supervertex_edge_descriptor<Graph, DisjointSets>
  make_supervertex_edge_descriptor(const Graph& g, DisjointSets& dsets)
  { return build_supervertex_edge_descriptor<Graph, DisjointSets>(g, dsets); }

  template<typename T>
  struct identity_function
  {
    typedef T argument_type;
    typedef T result_type;

    result_type operator()(argument_type x) const { return x; }
  };

  template<typename Graph, typename DisjointSets, typename EdgeMapper>
  class is_not_msf_edge
  {
    typedef typename graph_traits<Graph>::vertex_descriptor Vertex;
    typedef typename graph_traits<Graph>::edge_descriptor Edge;

  public:
    is_not_msf_edge(const Graph& g, DisjointSets dset, EdgeMapper edge_mapper)
      : g(g), dset(dset), edge_mapper(edge_mapper) { }

    bool operator()(Edge e)
    {
      Vertex u = dset.find_set(source(edge_mapper(e), g));
      Vertex v = dset.find_set(target(edge_mapper(e), g));
      if (u == v) return true;
      else {
        dset.link(u, v);
        return false;
      }
    }

  private:
    const Graph& g;
    DisjointSets dset;
    EdgeMapper edge_mapper;
  };

  template<typename Graph, typename ForwardIterator, typename EdgeList,
           typename EdgeMapper, typename RankMap, typename ParentMap>
  void
  sorted_mutating_kruskal(const Graph& g,
                          ForwardIterator first_vertex,
                          ForwardIterator last_vertex,
                          EdgeList& edge_list, EdgeMapper edge_mapper,
                          RankMap rank_map, ParentMap parent_map)
  {
    typedef disjoint_sets<RankMap, ParentMap> DisjointSets;

    // Build and initialize disjoint-sets data structure
    DisjointSets dset(rank_map, parent_map);
    for (ForwardIterator v = first_vertex; v != last_vertex; ++v)
      dset.make_set(*v);

    is_not_msf_edge<Graph, DisjointSets, EdgeMapper>
      remove_non_msf_edges(g, dset, edge_mapper);
    edge_list.erase(std::remove_if(edge_list.begin(), edge_list.end(),
                                   remove_non_msf_edges),
                    edge_list.end());
  }

  /**
   * Merge local minimum spanning forests from p processes into
   * minimum spanning forests on p/D processes (where D is the tree
   * factor, currently fixed at 3), eliminating unnecessary edges in
   * the process.
   *
   * As with @ref boruvka_merge_step, this routine has many
   * parameters, not all of which make sense within the limited
   * context of this routine. The parameters are required for the
   * Boruvka and local MSF merging steps to interoperate.
   *
   * \param pg The process group on which local minimum spanning
   * forests should be merged. The top (D-1)p/D processes will be
   * eliminated, and a new process subgroup containing p/D processors
   * will be returned. The value D is a constant factor that is
   * currently fixed to 3.
   *
   * \param g The underlying graph whose MSF is being computed. It must model
   * the DistributedGraph concept.
   *
   * \param first_vertex Iterator to the first vertex in the graph
   * that should be considered. While the local MSF merging algorithm
   * typically operates on the entire vertex set, within the hybrid
   * distributed MSF algorithms this will refer to the first
   * supervertex.
   *
   * \param last_vertex The past-the-end iterator for the vertex list.
   *
   * \param edge_list The list of local edges that will be
   * considered. For the p/D processes that remain, this list will
   * contain edges in the MSF known to the vertex after other
   * processes' edge lists have been merged. The edge list must be
   * sorted in order of increasing weight.
   *
   * \param weight Property map containing the weights of each
   * edge. The type of this property map must model
   * ReadablePropertyMap and must support caching.
   *
   * \param global_index Mapping from vertex descriptors to a global
   * index. The type must model ReadablePropertyMap.
   *
   * \param edge_mapper A function object that can remap edge descriptors
   * in the edge list to any alternative edge descriptor. This
   * function object will be the identity function when a pure merging
   * of local MSFs is required, but may be a mapping to a supervertex
   * edge when the local MSF merging occurs on a supervertex
   * graph. This function object saves us the trouble of having to
   * build a supervertex graph adaptor.
   *
   * \param already_local_msf True when the edge list already
   * constitutes a local MSF. If false, Kruskal's algorithm will first
   * be applied to the local edge list to select MSF edges.
   *
   * \returns The process subgroup containing the remaining p/D
   * processes. If the size of this process group is greater than one,
   * the MSF edges contained in the edge list do not constitute an MSF
   * for the entire graph.
   */
  template<typename ProcessGroup, typename Graph, typename ForwardIterator,
           typename EdgeList, typename WeightMap, typename GlobalIndexMap,
           typename EdgeMapper>
  ProcessGroup
  merge_local_minimum_spanning_trees_step(ProcessGroup pg,
                                          const Graph& g,
                                          ForwardIterator first_vertex,
                                          ForwardIterator last_vertex,
                                          EdgeList& edge_list,
                                          WeightMap weight,
                                          GlobalIndexMap global_index,
                                          EdgeMapper edge_mapper,
                                          bool already_local_msf)
  {
    typedef typename ProcessGroup::process_id_type process_id_type;
    typedef typename EdgeList::value_type edge_descriptor;
    typedef typename property_traits<WeightMap>::value_type weight_type;
    typedef typename graph_traits<Graph>::vertex_descriptor vertex_descriptor;

    // The tree factor, often called "D"
    process_id_type const tree_factor = 3;
    process_id_type num_procs = num_processes(pg);
    process_id_type id = process_id(pg);
    process_id_type procs_left = (num_procs + tree_factor - 1) / tree_factor;
    std::size_t n = std::size_t(last_vertex - first_vertex);

    if (!already_local_msf) {
      // Compute local minimum spanning forest. We only care about the
      // edges in the MSF, because only edges in the local MSF can be in
      // the global MSF.
      std::vector<std::size_t> ranks(n);
      std::vector<vertex_descriptor> parents(n);
      detail::sorted_mutating_kruskal
        (g, first_vertex, last_vertex,
         edge_list, edge_mapper,
         make_iterator_property_map(ranks.begin(), global_index),
         make_iterator_property_map(parents.begin(), global_index));
    }

    typedef std::pair<edge_descriptor, weight_type> w_edge;

    // Order edges based on their weights.
    indirect_cmp<WeightMap, std::less<weight_type> > cmp_edge_weight(weight);

    if (id < procs_left) {
      // The p/D processes that remain will receive local MSF edges from
      // D-1 other processes.
      synchronize(pg);
      for (process_id_type from_id = procs_left + id; from_id < num_procs;
           from_id += procs_left) {
        std::size_t num_incoming_edges;
        receive(pg, from_id, 0, num_incoming_edges);
        if (num_incoming_edges > 0) {
          std::vector<w_edge> incoming_edges(num_incoming_edges);
          receive(pg, from_id, 1, &incoming_edges[0], num_incoming_edges);

          edge_list.reserve(edge_list.size() + num_incoming_edges);
          for (std::size_t i = 0; i < num_incoming_edges; ++i) {
            cache(weight, incoming_edges[i].first, incoming_edges[i].second);
            edge_list.push_back(incoming_edges[i].first);
          }
          std::inplace_merge(edge_list.begin(),
                             edge_list.end() - num_incoming_edges,
                             edge_list.end(),
                             cmp_edge_weight);
        }
      }

      // Compute the local MSF from union of the edges in the MSFs of
      // all children.
      std::vector<std::size_t> ranks(n);
      std::vector<vertex_descriptor> parents(n);
      detail::sorted_mutating_kruskal
        (g, first_vertex, last_vertex,
         edge_list, edge_mapper,
         make_iterator_property_map(ranks.begin(), global_index),
         make_iterator_property_map(parents.begin(), global_index));
    } else {
      // The (D-1)p/D processes that are dropping out of further
      // computations merely send their MSF edges to their parent
      // process in the process tree.
      send(pg, id % procs_left, 0, edge_list.size());
      if (edge_list.size() > 0) {
        std::vector<w_edge> outgoing_edges;
        outgoing_edges.reserve(edge_list.size());
        for (std::size_t i = 0; i < edge_list.size(); ++i) {
          outgoing_edges.push_back(std::make_pair(edge_list[i],
                                                  get(weight, edge_list[i])));
        }
        send(pg, id % procs_left, 1, &outgoing_edges[0],
             outgoing_edges.size());
      }
      synchronize(pg);
    }

    // Return a process subgroup containing the p/D parent processes
    return process_subgroup(pg,
                            make_counting_iterator(process_id_type(0)),
                            make_counting_iterator(procs_left));
  }
} // end namespace detail

// ---------------------------------------------------------------------
// Dense Boruvka MSF algorithm
// ---------------------------------------------------------------------
template<typename Graph, typename WeightMap, typename OutputIterator,
         typename VertexIndexMap, typename RankMap, typename ParentMap,
         typename SupervertexMap>
OutputIterator
dense_boruvka_minimum_spanning_tree(const Graph& g, WeightMap weight_map,
                                    OutputIterator out,
                                    VertexIndexMap index_map,
                                    RankMap rank_map, ParentMap parent_map,
                                    SupervertexMap supervertex_map)
{
  using boost::graph::parallel::process_group;

  typedef typename graph_traits<Graph>::traversal_category traversal_category;

  BOOST_STATIC_ASSERT((is_convertible<traversal_category*,
                                      vertex_list_graph_tag*>::value));

  typedef typename graph_traits<Graph>::vertices_size_type vertices_size_type;
  typedef typename graph_traits<Graph>::vertex_descriptor  vertex_descriptor;
  typedef typename graph_traits<Graph>::vertex_iterator    vertex_iterator;
  typedef typename graph_traits<Graph>::edge_descriptor    edge_descriptor;

  // Don't throw away cached edge weights
  weight_map.set_max_ghost_cells(0);

  // Initialize the disjoint sets structures
  disjoint_sets<RankMap, ParentMap> dset(rank_map, parent_map);
  vertex_iterator vi, vi_end;
  for (boost::tie(vi, vi_end) = vertices(g); vi != vi_end; ++vi)
    dset.make_set(*vi);

  std::vector<vertex_descriptor> supervertices;
  supervertices.assign(vertices(g).first, vertices(g).second);

  // Use Kruskal's algorithm to find the minimum spanning forest
  // considering only the local edges. The resulting edges are not
  // necessarily going to be in the final minimum spanning
  // forest. However, any edge not part of the local MSF cannot be a
  // part of the global MSF, so we should have eliminated some edges
  // from consideration.
  std::vector<edge_descriptor> edge_list;
  kruskal_minimum_spanning_tree
    (make_vertex_and_edge_range(g, vertices(g).first, vertices(g).second,
                                edges(g).first, edges(g).second),
     std::back_inserter(edge_list),
     boost::weight_map(weight_map).
     vertex_index_map(index_map));

  // While the number of supervertices is decreasing, keep executing
  // Boruvka steps to identify additional MSF edges. This loop will
  // execute log |V| times.
  vertices_size_type old_num_supervertices;
  do {
    old_num_supervertices = supervertices.size();
    out = detail::boruvka_merge_step(process_group(g), g,
                                     weight_map, out,
                                     dset, supervertex_map, supervertices,
                                     edge_list);
  } while (supervertices.size() < old_num_supervertices);

  return out;
}

template<typename Graph, typename WeightMap, typename OutputIterator,
         typename VertexIndex>
OutputIterator
dense_boruvka_minimum_spanning_tree(const Graph& g, WeightMap weight_map,
                                    OutputIterator out, VertexIndex i_map)
{
  typedef typename graph_traits<Graph>::vertex_descriptor vertex_descriptor;

  std::vector<std::size_t> ranks(num_vertices(g));
  std::vector<vertex_descriptor> parents(num_vertices(g));
  std::vector<std::size_t> supervertices(num_vertices(g));

  return dense_boruvka_minimum_spanning_tree
           (g, weight_map, out, i_map,
            make_iterator_property_map(ranks.begin(), i_map),
            make_iterator_property_map(parents.begin(), i_map),
            make_iterator_property_map(supervertices.begin(), i_map));
}

template<typename Graph, typename WeightMap, typename OutputIterator>
OutputIterator
dense_boruvka_minimum_spanning_tree(const Graph& g, WeightMap weight_map,
                                    OutputIterator out)
{
  return dense_boruvka_minimum_spanning_tree(g, weight_map, out,
                                             get(vertex_index, g));
}

// ---------------------------------------------------------------------
// Merge local MSFs MSF algorithm
// ---------------------------------------------------------------------
template<typename Graph, typename WeightMap, typename OutputIterator,
         typename GlobalIndexMap>
OutputIterator
merge_local_minimum_spanning_trees(const Graph& g, WeightMap weight,
                                   OutputIterator out,
                                   GlobalIndexMap global_index)
{
  using boost::graph::parallel::process_group_type;
  using boost::graph::parallel::process_group;

  typedef typename graph_traits<Graph>::traversal_category traversal_category;

  BOOST_STATIC_ASSERT((is_convertible<traversal_category*,
                                      vertex_list_graph_tag*>::value));

  typedef typename graph_traits<Graph>::edge_descriptor edge_descriptor;

  // Don't throw away cached edge weights
  weight.set_max_ghost_cells(0);

  // Compute the initial local minimum spanning forests
  std::vector<edge_descriptor> edge_list;
  kruskal_minimum_spanning_tree
    (make_vertex_and_edge_range(g, vertices(g).first, vertices(g).second,
                                edges(g).first, edges(g).second),
     std::back_inserter(edge_list),
     boost::weight_map(weight).vertex_index_map(global_index));

  // Merge the local MSFs from p processes into p/D processes,
  // reducing the number of processes in each step. Continue looping
  // until either (a) the current process drops out or (b) only one
  // process remains in the group. This loop will execute log_D p
  // times.
  typename process_group_type<Graph>::type pg = process_group(g);
  while (pg && num_processes(pg) > 1) {
    pg = detail::merge_local_minimum_spanning_trees_step
           (pg, g, vertices(g).first, vertices(g).second,
            edge_list, weight, global_index,
            detail::identity_function<edge_descriptor>(), true);
  }

  // Only process 0 has the entire edge list, so emit it to the output
  // iterator.
  if (pg && process_id(pg) == 0) {
    out = std::copy(edge_list.begin(), edge_list.end(), out);
  }

  synchronize(process_group(g));
  return out;
}

template<typename Graph, typename WeightMap, typename OutputIterator>
inline OutputIterator
merge_local_minimum_spanning_trees(const Graph& g, WeightMap weight,
                                   OutputIterator out)
{
  return merge_local_minimum_spanning_trees(g, weight, out,
                                            get(vertex_index, g));
}

// ---------------------------------------------------------------------
// Boruvka-then-merge MSF algorithm
// ---------------------------------------------------------------------
template<typename Graph, typename WeightMap, typename OutputIterator,
         typename GlobalIndexMap, typename RankMap, typename ParentMap,
         typename SupervertexMap>
OutputIterator
boruvka_then_merge(const Graph& g, WeightMap weight, OutputIterator out,
                   GlobalIndexMap index, RankMap rank_map,
                   ParentMap parent_map, SupervertexMap supervertex_map)
{
  using std::log;
  using boost::graph::parallel::process_group_type;
  using boost::graph::parallel::process_group;

  typedef typename graph_traits<Graph>::traversal_category traversal_category;

  BOOST_STATIC_ASSERT((is_convertible<traversal_category*,
                                      vertex_list_graph_tag*>::value));

  typedef typename graph_traits<Graph>::vertices_size_type vertices_size_type;
  typedef typename graph_traits<Graph>::vertex_descriptor  vertex_descriptor;
  typedef typename graph_traits<Graph>::vertex_iterator    vertex_iterator;
  typedef typename graph_traits<Graph>::edge_descriptor    edge_descriptor;

  // Don't throw away cached edge weights
  weight.set_max_ghost_cells(0);

  // Compute the initial local minimum spanning forests
  std::vector<edge_descriptor> edge_list;
  kruskal_minimum_spanning_tree
    (make_vertex_and_edge_range(g, vertices(g).first, vertices(g).second,
                                edges(g).first, edges(g).second),
     std::back_inserter(edge_list),
     boost::weight_map(weight).
     vertex_index_map(index));

  // Initialize the disjoint sets structures for Boruvka steps
  disjoint_sets<RankMap, ParentMap> dset(rank_map, parent_map);
  vertex_iterator vi, vi_end;
  for (boost::tie(vi, vi_end) = vertices(g); vi != vi_end; ++vi)
    dset.make_set(*vi);

  // Construct the initial set of supervertices (all vertices)
  std::vector<vertex_descriptor> supervertices;
  supervertices.assign(vertices(g).first, vertices(g).second);

  // Continue performing Boruvka merge steps until the number of
  // supervertices reaches |V| / (log_D p)^2.
  const std::size_t tree_factor = 3; // TBD: same as above! should be param
  double log_d_p = log((double)num_processes(process_group(g)))
                 / log((double)tree_factor);
  vertices_size_type target_supervertices =
    vertices_size_type(num_vertices(g) / (log_d_p * log_d_p));
  vertices_size_type old_num_supervertices;
  while (supervertices.size() > target_supervertices) {
    old_num_supervertices = supervertices.size();
    out = detail::boruvka_merge_step(process_group(g), g,
                                     weight, out, dset,
                                     supervertex_map, supervertices,
                                     edge_list);
    if (supervertices.size() == old_num_supervertices)
      return out;
  }

  // Renumber the supervertices
  for (std::size_t i = 0; i < supervertices.size(); ++i)
    put(supervertex_map, supervertices[i], i);

  // Merge local MSFs on the supervertices. (D-1)p/D processors drop
  // out each iteration, so this loop executes log_D p times.
  typename process_group_type<Graph>::type pg = process_group(g);
  bool have_msf = false;
  while (pg && num_processes(pg) > 1) {
    pg = detail::merge_local_minimum_spanning_trees_step
           (pg, g, supervertices.begin(), supervertices.end(),
            edge_list, weight, supervertex_map,
            detail::make_supervertex_edge_descriptor(g, dset),
            have_msf);
    have_msf = true;
  }

  // Only process 0 has the complete list of _supervertex_ MST edges,
  // so emit those to the output iterator. This is not the complete
  // list of edges in the MSF, however: the Boruvka steps in the
  // beginning of the algorithm emitted any edges used to merge
  // supervertices.
  if (pg && process_id(pg) == 0)
    out = std::copy(edge_list.begin(), edge_list.end(), out);

  synchronize(process_group(g));
  return out;
}

template<typename Graph, typename WeightMap, typename OutputIterator,
         typename GlobalIndexMap>
inline OutputIterator
boruvka_then_merge(const Graph& g, WeightMap weight, OutputIterator out,
                    GlobalIndexMap index)
{
  typedef typename graph_traits<Graph>::vertex_descriptor vertex_descriptor;
  typedef typename graph_traits<Graph>::vertices_size_type vertices_size_type;
  std::vector<vertices_size_type> ranks(num_vertices(g));
  std::vector<vertex_descriptor> parents(num_vertices(g));
  std::vector<vertices_size_type> supervertex_indices(num_vertices(g));

  return boruvka_then_merge
           (g, weight, out, index,
            make_iterator_property_map(ranks.begin(), index),
            make_iterator_property_map(parents.begin(), index),
            make_iterator_property_map(supervertex_indices.begin(), index));
}

template<typename Graph, typename WeightMap, typename OutputIterator>
inline OutputIterator
boruvka_then_merge(const Graph& g, WeightMap weight, OutputIterator out)
{ return boruvka_then_merge(g, weight, out, get(vertex_index, g)); }

// ---------------------------------------------------------------------
// Boruvka-mixed-merge MSF algorithm
// ---------------------------------------------------------------------
template<typename Graph, typename WeightMap, typename OutputIterator,
         typename GlobalIndexMap, typename RankMap, typename ParentMap,
         typename SupervertexMap>
OutputIterator
boruvka_mixed_merge(const Graph& g, WeightMap weight, OutputIterator out,
                    GlobalIndexMap index, RankMap rank_map,
                    ParentMap parent_map, SupervertexMap supervertex_map)
{
  using boost::graph::parallel::process_group_type;
  using boost::graph::parallel::process_group;

  typedef typename graph_traits<Graph>::traversal_category traversal_category;

  BOOST_STATIC_ASSERT((is_convertible<traversal_category*,
                                      vertex_list_graph_tag*>::value));

  typedef typename graph_traits<Graph>::vertices_size_type vertices_size_type;
  typedef typename graph_traits<Graph>::vertex_descriptor  vertex_descriptor;
  typedef typename graph_traits<Graph>::vertex_iterator    vertex_iterator;
  typedef typename graph_traits<Graph>::edge_descriptor    edge_descriptor;

  // Don't throw away cached edge weights
  weight.set_max_ghost_cells(0);

  // Initialize the disjoint sets structures for Boruvka steps
  disjoint_sets<RankMap, ParentMap> dset(rank_map, parent_map);
  vertex_iterator vi, vi_end;
  for (boost::tie(vi, vi_end) = vertices(g); vi != vi_end; ++vi)
    dset.make_set(*vi);

  // Construct the initial set of supervertices (all vertices)
  std::vector<vertex_descriptor> supervertices;
  supervertices.assign(vertices(g).first, vertices(g).second);

  // Compute the initial local minimum spanning forests
  std::vector<edge_descriptor> edge_list;
  kruskal_minimum_spanning_tree
    (make_vertex_and_edge_range(g, vertices(g).first, vertices(g).second,
                                edges(g).first, edges(g).second),
     std::back_inserter(edge_list),
     boost::weight_map(weight).
     vertex_index_map(index));

  if (num_processes(process_group(g)) == 1) {
    return std::copy(edge_list.begin(), edge_list.end(), out);
  }

  // Like the merging local MSFs algorithm and the Boruvka-then-merge
  // algorithm, each iteration of this loop reduces the number of
  // processes by a constant factor D, and therefore we require log_D
  // p iterations. Note also that the number of edges in the edge list
  // decreases geometrically, giving us an efficient distributed MSF
  // algorithm.
  typename process_group_type<Graph>::type pg = process_group(g);
  vertices_size_type old_num_supervertices;
  while (pg && num_processes(pg) > 1) {
    // A single Boruvka step. If this doesn't change anything, we're done
    old_num_supervertices = supervertices.size();
    out = detail::boruvka_merge_step(pg, g, weight, out, dset,
                                     supervertex_map, supervertices,
                                     edge_list);
    if (old_num_supervertices == supervertices.size()) {
      edge_list.clear();
      break;
    }

    // Renumber the supervertices
    for (std::size_t i = 0; i < supervertices.size(); ++i)
      put(supervertex_map, supervertices[i], i);

    // A single merging of local MSTs, which reduces the number of
    // processes we're using by a constant factor D.
    pg = detail::merge_local_minimum_spanning_trees_step
           (pg, g, supervertices.begin(), supervertices.end(),
            edge_list, weight, supervertex_map,
            detail::make_supervertex_edge_descriptor(g, dset),
            true);

  }

  // Only process 0 has the complete edge list, so emit it for the
  // user. Note that list edge list only contains the MSF edges in the
  // final supervertex graph: all of the other edges were used to
  // merge supervertices and have been emitted by the Boruvka steps,
  // although only process 0 has received the complete set.
  if (pg && process_id(pg) == 0)
    out = std::copy(edge_list.begin(), edge_list.end(), out);

  synchronize(process_group(g));
  return out;
}

template<typename Graph, typename WeightMap, typename OutputIterator,
         typename GlobalIndexMap>
inline OutputIterator
boruvka_mixed_merge(const Graph& g, WeightMap weight, OutputIterator out,
                    GlobalIndexMap index)
{
  typedef typename graph_traits<Graph>::vertex_descriptor vertex_descriptor;
  typedef typename graph_traits<Graph>::vertices_size_type vertices_size_type;
  std::vector<vertices_size_type> ranks(num_vertices(g));
  std::vector<vertex_descriptor> parents(num_vertices(g));
  std::vector<vertices_size_type> supervertex_indices(num_vertices(g));

  return boruvka_mixed_merge
           (g, weight, out, index,
            make_iterator_property_map(ranks.begin(), index),
            make_iterator_property_map(parents.begin(), index),
            make_iterator_property_map(supervertex_indices.begin(), index));
}

template<typename Graph, typename WeightMap, typename OutputIterator>
inline OutputIterator
boruvka_mixed_merge(const Graph& g, WeightMap weight, OutputIterator out)
{ return boruvka_mixed_merge(g, weight, out, get(vertex_index, g)); }

} // end namespace distributed

using distributed::dense_boruvka_minimum_spanning_tree;
using distributed::merge_local_minimum_spanning_trees;
using distributed::boruvka_then_merge;
using distributed::boruvka_mixed_merge;

} } // end namespace boost::graph

#endif // BOOST_DEHNE_GOTZ_MIN_SPANNING_TREE_HPP


// graph-tool -- a general graph modification and manipulation thingy
//
// Copyright (C) 2006-2015 Tiago de Paula Peixoto <tiago@skewed.de>
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 3
// of the License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.

#include "graph_filtering.hh"
#include "graph.hh"
#include "graph_properties.hh"

#include <boost/graph/boyer_myrvold_planar_test.hpp>

using namespace std;
using namespace boost;
using namespace graph_tool;

struct get_planar_embedding
{
    template <class EdgeMap>
    class edge_inserter
    {
    public:
        edge_inserter(EdgeMap edge_map): _edge_map(edge_map) {}

        edge_inserter& operator++() { return *this; }
        edge_inserter& operator++(int) { return *this; }
        edge_inserter& operator*() { return *this; }

        template <class Key>
        edge_inserter& operator=(const Key& e)
        {
            _edge_map[e] = 1;
            return *this;
        }

    private:
        EdgeMap _edge_map;
    };

    template <class Graph, class VertexIndex, class EdgeIndex, class EmbedMap,
              class KurMap>
    void operator()(Graph& g, VertexIndex vertex_index, EdgeIndex edge_index,
                    EmbedMap embed_map, KurMap kur_map, bool& is_planar) const
    {
        edge_inserter<KurMap> kur_insert(kur_map);
        unchecked_vector_property_map
            <vector<typename graph_traits<Graph>::edge_descriptor>, VertexIndex>
            embedding(vertex_index, num_vertices(g));
        is_planar = boyer_myrvold_planarity_test
            (boyer_myrvold_params::graph = g,
             boyer_myrvold_params::edge_index_map = edge_index,
             boyer_myrvold_params::embedding = embedding,
             boyer_myrvold_params::kuratowski_subgraph = kur_insert);

        int i, N = num_vertices(g);
        #pragma omp parallel for default(shared) private(i) schedule(runtime) if (N > 100)
        for (i = 0; i < N; ++i)
        {
            typename graph_traits<Graph>::vertex_descriptor v = vertex(i, g);
            if (v == graph_traits<Graph>::null_vertex())
                continue;
            embed_map[v].resize(embedding[v].size());
            for (size_t j = 0; j < embedding[v].size(); ++j)
                embed_map[v][j] = edge_index[embedding[v][j]];
        }
    }

    template <class Graph, class VertexIndex, class EdgeIndex, class KurMap>
    void operator()(Graph& g, VertexIndex, EdgeIndex edge_index,
                    dummy_property_map, KurMap kur_map,
                    bool& is_planar) const
    {
        edge_inserter<KurMap> kur_insert(kur_map);
        is_planar = boyer_myrvold_planarity_test
            (boyer_myrvold_params::graph = g,
             boyer_myrvold_params::edge_index_map = edge_index,
             boyer_myrvold_params::kuratowski_subgraph = kur_insert);
    }

};

bool is_planar(GraphInterface& gi, boost::any embed_map, boost::any kur_map)
{
    bool is_planar;

    if (embed_map.empty())
        embed_map = dummy_property_map();
    if (kur_map.empty())
        kur_map = dummy_property_map();

    typedef mpl::push_back<writable_edge_scalar_properties,
                           dummy_property_map>::type edge_map_types;
    typedef mpl::push_back<vertex_scalar_vector_properties,
                           dummy_property_map>::type vertex_map_types;

    run_action<graph_tool::detail::never_directed>()
        (gi, std::bind(get_planar_embedding(), placeholders::_1, gi.GetVertexIndex(),
                       gi.GetEdgeIndex(), placeholders::_2, placeholders::_3,
                       std::ref(is_planar)),
         vertex_map_types(), edge_map_types())
        (embed_map, kur_map);
    return is_planar;
}


/***
   HiCapTools.
   Copyright (c) 2017 Pelin Sahlén <pelin.akan@scilifelab.se>

	Permission is hereby granted, free of charge, to any person obtaining a
	copy of this software and associated documentation files (the "Software"),
	to deal in the Software with some restriction, including without limitation
	the rights to use, copy, modify, merge, publish, distribute the Software,
	and to permit persons to whom the Software is furnished to do so, subject to
	the following conditions:

	The above copyright notice and this permission notice shall be included in all
	copies or substantial portions of the Software. The Software shall not be used
	for commercial purposes.

	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
	INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
	PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
	HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF
	CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE
	OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
***/

//
//  BackgroundInteractionFrequency.cpp
//  HiCapTools
//
//  Created by Pelin Sahlen and Anandashankar Anil.
//

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/rolling_mean.hpp>
using namespace boost::accumulators;

#include "BackgroundInteractionFrequency.h"
#include "Global.h"
#include "linear.h"
#include <fstream>
#include <cmath>

///For Probe-Distal and Probe-Probe
void DetermineBackgroundLevels::CalculateMeanandStdRegress(std::string eName, int ExperimentNo, std::string DesignName, BG_signals& bglevelsloc, int binsize, std::string whichProx, int MinimumJunctionDistance, OutStream& log, int WindowSizeloc){

	expName= eName; //set experiment name

	std::map< int, int > nofentries_perBin; // required for calculating mean
	std::map< int, int > signal_square; // required for calculating stdev
    double sum_square, variance;
	int distance;
    std::string feature_id;

    std::map< int, Junction >::const_iterator iter;
	for(int i = 0; i < Design_NegCtrl[DesignName].Probes.size(); i++){
        feature_id = Design_NegCtrl[DesignName].Probes[i].feature_id;
        //////////Probe-distal
        if(whichProx=="ProbeDistal"){
			for (iter = Features[feature_id].proximities.junctions.begin(); iter != Features[feature_id].proximities.junctions.end(); ++iter){

				distance = iter->first - Design_NegCtrl[DesignName].Probes[i].end;

				int bin = abs(distance) / binsize;

				if(iter->second.paircount[ExperimentNo] > 0){

					if(bglevelsloc.mean.find(bin) == bglevelsloc.mean.end())
						bglevelsloc.mean[bin] = iter->second.paircount[ExperimentNo];
					else
						bglevelsloc.mean[bin] = bglevelsloc.mean[bin] + iter->second.paircount[ExperimentNo];

					if(nofentries_perBin.find(bin) == nofentries_perBin.end()){
						nofentries_perBin[bin] = 1;
						signal_square[bin] = (iter->second.paircount[ExperimentNo])*(iter->second.paircount[ExperimentNo]);
					}
					else{
						nofentries_perBin[bin] = nofentries_perBin[bin] + 1;
						signal_square[bin] = (signal_square[bin] + ((iter->second.paircount[ExperimentNo])*(iter->second.paircount[ExperimentNo])));
					}
				}
			}
		}
		///////////Probe-Probe
        else if(whichProx=="ProbeProbe"){
			for (auto iter = Features[feature_id].Inter_feature_ints.begin(); iter != Features[feature_id].Inter_feature_ints.end(); ++iter){

				distance = Features[feature_id].start - Features[(*iter).interacting_feature_id].start;

				if((Features[feature_id].FeatureType == 3 && Features[(*iter).interacting_feature_id].FeatureType == 3) && Features[feature_id].TranscriptName != Features[(*iter).interacting_feature_id].TranscriptName && (abs(distance) >= MinimumJunctionDistance)){

					int bin = abs(distance) / binsize;
					if((*iter).signal[ExperimentNo] > 0){
						if(bglevelsloc.mean.find(bin) == bglevelsloc.mean.end())
							bglevelsloc.mean[bin] = (*iter).signal[ExperimentNo];
						else
							bglevelsloc.mean[bin] = bglevelsloc.mean[bin] + (*iter).signal[ExperimentNo];

						if(nofentries_perBin.find(bin) == nofentries_perBin.end()){
							nofentries_perBin[bin] = 1;
							signal_square[bin] = ((*iter).signal[ExperimentNo])*((*iter).signal[ExperimentNo]);
						}
						else{
							nofentries_perBin[bin] = nofentries_perBin[bin] + 1;
							signal_square[bin] = (signal_square[bin] + (((*iter).signal[ExperimentNo])*((*iter).signal[ExperimentNo])));
						}
					}
				}
			}
		}
    }

// Calculate Mean and stdev
	std::map< int, double>::iterator it; // iterator for bin signals
	for (it = bglevelsloc.mean.begin(); it != bglevelsloc.mean.end(); ++it){
        bglevelsloc.samplesize[it->first] = nofentries_perBin[it->first];
        if(bglevelsloc.samplesize[it->first] == 0){
            it->second = 1; //Mean
        }
        else{
            it->second /= nofentries_perBin[it->first]; //Mean
        }
        if(bglevelsloc.samplesize[it->first] <= 1){
            bglevelsloc.stdev[it->first] = 0;
        }
        else{
            sum_square = ((it->second)*(it->second));
            sum_square /= (double(nofentries_perBin[it->first])); // Average Sum Squared
            variance = signal_square[it->first] - sum_square;
            variance /= (nofentries_perBin[it->first] -1);
            bglevelsloc.stdev[it->first] = sqrt(variance); //stdev
        }
	}

    int first25kb = ceil(25000/(double)binsize); // to exclude bins of first 25 kb from smoothing

    boost::accumulators::accumulator_set<double, stats<tag::rolling_mean> > acc(tag::rolling_window::window_size = (WindowSizeloc*2));
    boost::accumulators::accumulator_set<double, stats<tag::rolling_mean> > acc2(tag::rolling_window::window_size = (WindowSizeloc*2));

    int w = 0, z = 0, s = 0, a=0;
    std::deque< double > dm, ds, db, dwm, dws;

    // Put values into a queue
    for (it = bglevelsloc.mean.begin(); it != bglevelsloc.mean.end(); ++it){

        dm.push_back(it->second);
        ds.push_back(bglevelsloc.stdev[it->first]);
        db.push_back(it->first);

    }
    //Push the first WindowSize-1 elements into a queue
	if(dm.empty())
		log<<"No Element in dm Outloop"<<std::endl;

	bool firstpass=true;
    for(w = 0; w < (dm.size()); ++w){
		if(w < first25kb){
			bglevelsloc.smoothed[db[w]] = dm[w];
			bglevelsloc.smoothed_stdev[db[w]] = ds[w];
		}
		else{
			boost::accumulators::accumulator_set<double, stats<tag::rolling_mean> > acc(tag::rolling_window::window_size = (WindowSizeloc));
			boost::accumulators::accumulator_set<double, stats<tag::rolling_mean> > acc2(tag::rolling_window::window_size = (WindowSizeloc));

			if(w >= first25kb && w < first25kb + WindowSizeloc - 1){

				/**
				if(dwm.size()==WindowSizeloc){
					for(z = 0; z < WindowSizeloc;++z){
						std::cout<<dwm[z]<<std::endl;
					}
					dwm.pop_front();
				}
			**/
				//change to rolling mean if needed
				bglevelsloc.smoothed[db[w]] = dm[w];
				bglevelsloc.smoothed_stdev[db[w]] = ds[w];

			}
			else {
			    if(firstpass){
			        for(auto it=w-(WindowSizeloc/2); it<w+WindowSizeloc/2;++it){ //for the first bin that is to be smoothed
			            dwm.push_back(dm[it]);
			            dws.push_back(ds[it]);
			        }
			        firstpass=false;
			    }
				bool ifrolling=false;

				//To check that the window stops at the last bin
				if(w+WindowSizeloc/2 < dm.size()){
				    dwm.push_back(dm.at(w+WindowSizeloc/2));
				    dws.push_back(ds.at(w+WindowSizeloc/2));
				}

				if(dwm.size()==WindowSizeloc){
					ifrolling=true;
					for(z = 0; z < WindowSizeloc;++z){
						acc(dwm[z]);
						acc2(dws[z]);
					}
				}
				if(ifrolling){
					bglevelsloc.smoothed[db[w]] = rolling_mean(acc);
					bglevelsloc.smoothed_stdev[db[w]] = rolling_mean(acc2);

				}
				else{
					bglevelsloc.smoothed[db[w]] = dm[w];
					bglevelsloc.smoothed_stdev[db[w]] = ds[w];

				}

				dwm.pop_front();
				dws.pop_front();

			}
		}
	}
}

void DetermineBackgroundLevels::PrintBackgroundFrequency(int bSize, int bSizePP, PrDes::RENFileInfo& reInfo){

	std::string FileNamePE, FileNamePP;

	FileNamePE.append(expName);
	FileNamePE.append(".");
	FileNamePE.append(reInfo.genomeAssembly.substr(0, reInfo.genomeAssembly.find_first_of(',')));
	FileNamePE.append(".Probe_Distal.BackgroundLevels.");
	FileNamePE.append(reInfo.currTime);
	FileNamePE.append(".txt");

	FileNamePP.append(expName);
	FileNamePP.append(".");
	FileNamePP.append(reInfo.genomeAssembly.substr(0, reInfo.genomeAssembly.find_first_of(',')));
	FileNamePP.append(".Probe_Probe.BackgroundLevels.");
	FileNamePP.append(reInfo.currTime);
	FileNamePP.append(".txt");

	std::ofstream outfPE(FileNamePE.c_str());
	std::ofstream outfPP(FileNamePP.c_str());
    outfPE << "Distance Bin" << '\t' << "Interaction Distance" << '\t' << "Mean " << '\t' << "Stdev" << '\t' << "Sample Size" << '\t' << "Mean (Smoothed)" << '\t' << "StDev (Smoothed)" <<  std::endl;

	for (auto it = bglevels.smoothed.begin(); it != bglevels.smoothed.end(); ++it){
        outfPE << (it->first) << '\t'<< (it->first)*bSize << '\t' << bglevels.mean[it->first] << '\t' << bglevels.stdev[it->first] << '\t' << bglevels.samplesize[it->first] << '\t' << it->second << '\t' << bglevels.smoothed_stdev[it->first] << std::endl;
	}

	outfPP << "Distance Bin" << '\t' << "Interaction Distance" << '\t' << "Mean " << '\t' << "Stdev" << '\t' << "Sample Size" << '\t' << "Mean (Smoothed)" << '\t' << "StDev (Smoothed)" <<  std::endl;

	for (auto it = bglevelsProbeProbe.smoothed.begin(); it != bglevelsProbeProbe.smoothed.end(); ++it){
        outfPP << (it->first) << '\t' << (it->first)*bSizePP << '\t' << bglevelsProbeProbe.mean[it->first] << '\t' << bglevelsProbeProbe.stdev[it->first] << '\t' << bglevelsProbeProbe.samplesize[it->first] << '\t' << it->second << '\t' << bglevelsProbeProbe.smoothed_stdev[it->first] << std::endl;
	}

}