<<<<<<< HEAD
/*    Copyright (c) 2010-2018, Delft University of Technology
=======
/*    Copyright (c) 2010-2019, Delft University of Technology
>>>>>>> origin/master
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#include <boost/make_shared.hpp>

#if USE_CSPICE
#include "Tudat/External/SpiceInterface/spiceInterface.h"
#endif

#include "Tudat/Astrodynamics/Gravitation/timeDependentSphericalHarmonicsGravityField.h"
#include "Tudat/Astrodynamics/Gravitation/triAxialEllipsoidGravity.h"
#include "Tudat/SimulationSetup/EnvironmentSetup/createGravityField.h"
#include "Tudat/InputOutput/basicInputOutput.h"

namespace tudat
{

namespace simulation_setup
{

//! Get the path of the SH file for a SH model.
std::string getPathForSphericalHarmonicsModel( const SphericalHarmonicsModel sphericalHarmonicsModel )
{
    switch ( sphericalHarmonicsModel )
    {
    case egm96:
        return input_output::getGravityModelsPath( ) + "Earth/egm96.txt";
    case ggm02c:
        return input_output::getGravityModelsPath( ) + "Earth/ggm02c.txt";
    case ggm02s:
        return input_output::getGravityModelsPath( ) + "Earth/ggm02s.txt";
    case glgm3150:
        return input_output::getGravityModelsPath( ) + "Moon/glgm3150.txt";
    case lpe200:
        return input_output::getGravityModelsPath( ) + "Moon/lpe200.txt";
    case jgmro120d:
        return input_output::getGravityModelsPath( ) + "Mars/jgmro120d.txt";
    default:
        std::cerr << "No path known for Spherical Harmonics Model " << sphericalHarmonicsModel << std::endl;
        throw;
    }
}

//! Get the associated reference frame for a SH model.
std::string getReferenceFrameForSphericalHarmonicsModel( const SphericalHarmonicsModel sphericalHarmonicsModel )
{
    switch ( sphericalHarmonicsModel )
    {
    case egm96:
    case ggm02c:
    case ggm02s:
        return "IAU_Earth";
    case glgm3150:
    case lpe200:
        return "IAU_Moon";
    case jgmro120d:
        return "IAU_Mars";
    default:
        std::cerr << "No reference frame known for Spherical Harmonics Model " << sphericalHarmonicsModel << std::endl;
        throw;
    }
}

//! Constructor with custom model.
FromFileSphericalHarmonicsGravityFieldSettings::FromFileSphericalHarmonicsGravityFieldSettings(
        const std::string& filePath, const std::string& associatedReferenceFrame,
        const int maximumDegree, const int maximumOrder,
        const int gravitationalParameterIndex, const int referenceRadiusIndex,
        const double gravitationalParameter, const double referenceRadius ) :
    SphericalHarmonicsGravityFieldSettings( gravitationalParameter, referenceRadius, Eigen::MatrixXd( ),
                                            Eigen::MatrixXd( ), associatedReferenceFrame ),
    filePath_( filePath ),
    maximumDegree_( maximumDegree ),
    maximumOrder_( maximumOrder ),
    gravitationalParameterIndex_( gravitationalParameterIndex ),
    referenceRadiusIndex_( referenceRadiusIndex )
{
    std::pair< Eigen::MatrixXd, Eigen::MatrixXd > coefficients;
    std::pair< double, double > referenceData =
            readGravityFieldFile( filePath, maximumDegree, maximumOrder, coefficients,
                                  gravitationalParameterIndex, referenceRadiusIndex );
    gravitationalParameter_ = gravitationalParameterIndex >= 0 ? referenceData.first : gravitationalParameter;
    referenceRadius_ = referenceRadiusIndex >= 0 ? referenceData.second : referenceRadius;
    cosineCoefficients_ = coefficients.first;
    sineCoefficients_ = coefficients.second;
}

//! Constructor with model included in Tudat.
FromFileSphericalHarmonicsGravityFieldSettings::FromFileSphericalHarmonicsGravityFieldSettings(
        const SphericalHarmonicsModel sphericalHarmonicsModel ) :
    FromFileSphericalHarmonicsGravityFieldSettings( getPathForSphericalHarmonicsModel( sphericalHarmonicsModel ),
                                                    getReferenceFrameForSphericalHarmonicsModel( sphericalHarmonicsModel ),
                                                    50, 50, 0, 1 )
{
    sphericalHarmonicsModel_ = sphericalHarmonicsModel;
}

//! Function to read a gravity field file
std::pair< double, double  > readGravityFieldFile(
        const std::string& fileName, const int maximumDegree, const int maximumOrder,
        std::pair< Eigen::MatrixXd, Eigen::MatrixXd >& coefficients,
        const int gravitationalParameterIndex, const int referenceRadiusIndex )
{
    // Attempt to open gravity file.
    std::fstream stream( fileName.c_str( ), std::ios::in );
    if( stream.fail( ) )
    {
        throw std::runtime_error( "Pds gravity field data file could not be opened: " + fileName );
    }

    // Declare variables for reading file.
    std::vector< std::string > vectorOfIndividualStrings;
    vectorOfIndividualStrings.resize( 4 );
    std::string line;

    double gravitationalParameter = TUDAT_NAN;
    double referenceRadius = TUDAT_NAN;

    if( ( gravitationalParameterIndex >= 0 ) &&
            ( referenceRadiusIndex >= 0 ) )
    {
        // Get first line of file.
        std::getline( stream, line );

        // Get reference radius and gravitational parameter from first line of file.
        boost::algorithm::trim( line );
        boost::algorithm::split( vectorOfIndividualStrings,
                                 line,
                                 boost::algorithm::is_any_of( "\t, " ),
                                 boost::algorithm::token_compress_on );
        if( gravitationalParameterIndex >= static_cast< int >( vectorOfIndividualStrings.size( ) ) ||
                referenceRadiusIndex >= static_cast< int >( vectorOfIndividualStrings.size( ) ) )
        {
            throw std::runtime_error( "Error when reading gravity field file, requested header index exceeds file contents" );
        }

        gravitationalParameter = std::stod( vectorOfIndividualStrings[ gravitationalParameterIndex ] );
        referenceRadius = std::stod( vectorOfIndividualStrings[ referenceRadiusIndex ] );
    }
    else if( ( !( gravitationalParameterIndex >= 0 ) &&
               ( referenceRadiusIndex >= 0 ) ) ||
             ( ( gravitationalParameterIndex >= 0 ) &&
               !( referenceRadiusIndex >= 0 ) ) )
    {
        throw std::runtime_error( "Error when reading gravity field file, must retrieve either both or neither of Re and mu" );
    }

    // Declare variables for reading in cosine and sine coefficients.
    int currentDegree = 0, currentOrder = 0;
    Eigen::MatrixXd cosineCoefficients = Eigen::MatrixXd( maximumDegree + 1, maximumOrder + 1 );
    cosineCoefficients.setZero( );
    Eigen::MatrixXd sineCoefficients = Eigen::MatrixXd( maximumDegree + 1, maximumOrder + 1 );
    sineCoefficients.setZero( );

    // Read coefficients up to required maximum degree and order.
    while ( !stream.fail( ) && !stream.eof( ) &&
            ( currentDegree <= maximumDegree && currentOrder < maximumOrder )  )
    {
        // Read current line
        std::getline( stream, line );

        // Trim input string (removes all leading and trailing whitespaces).
        boost::algorithm::trim( line );

        // Split string into multiple strings, each containing one element from a line from the
        // data file.
        boost::algorithm::split( vectorOfIndividualStrings,
                                 line,
                                 boost::algorithm::is_any_of( ", " ),
                                 boost::algorithm::token_compress_on );

        // Check current line for consistency
        if( vectorOfIndividualStrings.size( ) != 0 )
        {
            if( vectorOfIndividualStrings.size( ) < 4 )
            {
                std::string errorMessage = "Error when reading pds gravity field file, number of fields is " +
                        std::to_string( vectorOfIndividualStrings.size( ) );
                throw std::runtime_error( errorMessage );
            }
            else
            {
                // Read current degree and orde from line.
                currentDegree = std::stoi( vectorOfIndividualStrings[ 0 ] );
                currentOrder = std::stoi( vectorOfIndividualStrings[ 1 ] );

                // Set cosine and sine coefficients for current degree and order.
                if( currentDegree <= maximumDegree && currentOrder <= maximumOrder )
                {
                    cosineCoefficients( currentDegree, currentOrder ) =
                            std::stod( vectorOfIndividualStrings[ 2 ] );
                    sineCoefficients( currentDegree, currentOrder ) =
                            std::stod( vectorOfIndividualStrings[ 3 ] );
                }
            }
        }
    }

    // Set cosine coefficient at (0,0) to 1.
    cosineCoefficients( 0, 0 ) = 1.0;
    coefficients = std::make_pair( cosineCoefficients, sineCoefficients );

    return std::make_pair( gravitationalParameter, referenceRadius );
}

//! Function to create a gravity field model.
std::shared_ptr< gravitation::GravityFieldModel > createGravityFieldModel(
        const std::shared_ptr< GravityFieldSettings > gravityFieldSettings,
        const std::string& body,
        const NamedBodyMap& bodyMap,
        const std::vector< std::shared_ptr< GravityFieldVariationSettings > >& gravityFieldVariationSettings )
{
    using namespace tudat::gravitation;

    // Declare return object.
    std::shared_ptr< GravityFieldModel > gravityFieldModel;

    // Check which type of gravity field model is to be created.
    switch( gravityFieldSettings->getGravityFieldType( ) )
    {
    case central:
    {
        // Check whether settings for point mass gravity field model are consistent with its type.
        std::shared_ptr< CentralGravityFieldSettings > centralFieldSettings =
                std::dynamic_pointer_cast< CentralGravityFieldSettings >( gravityFieldSettings );
        if( centralFieldSettings == nullptr )
        {
            throw std::runtime_error(
                        "Error, expected central field settings when making gravity field model for body " +
                        body);
        }
        else if( gravityFieldVariationSettings.size( ) != 0 )
        {
            throw std::runtime_error( "Error, requested central gravity field, but field variations settings are not empty." );
        }
        else
        {
            // Create and initialize point mass gravity field model.
            gravityFieldModel = std::make_shared< GravityFieldModel >(
                        centralFieldSettings->getGravitationalParameter( ) );
        }
        break;
    }
#if USE_CSPICE
    case central_spice:
    {
        if( gravityFieldVariationSettings.size( ) != 0 )
        {
            throw std::runtime_error( "Error, requested central gravity field, but field variations settings are not empty." );
        }
        else
        {
            // Create and initialize point mass gravity field model from Spice.
            gravityFieldModel = std::make_shared< GravityFieldModel >(
                        spice_interface::getBodyGravitationalParameter( body ) );
        }

        break;
    }
#endif
    case spherical_harmonic:
    {
        // Check whether settings for spherical harmonic gravity field model are consistent with
        // its type.
        std::shared_ptr< SphericalHarmonicsGravityFieldSettings > sphericalHarmonicFieldSettings =
                std::dynamic_pointer_cast< SphericalHarmonicsGravityFieldSettings >(
                    gravityFieldSettings );

        if( sphericalHarmonicFieldSettings == nullptr )
        {
            throw std::runtime_error(
                        "Error, expected spherical harmonic field settings when making gravity field model of "
                        + body );
        }
        else
        {
            std::function< void( ) > inertiaTensorUpdateFunction;
            if( bodyMap.count( body ) == 0 )
            {
                inertiaTensorUpdateFunction = std::function< void( ) >( );
            }
            else
            {
                inertiaTensorUpdateFunction =
                    std::bind( &Body::setBodyInertiaTensorFromGravityFieldAndExistingMeanMoment, bodyMap.at( body ), true );
            }

            // Check consistency of cosine and sine coefficients.
            if( ( sphericalHarmonicFieldSettings->getCosineCoefficients( ).rows( ) !=
                  sphericalHarmonicFieldSettings->getSineCoefficients( ).rows( ) ) ||
                    ( sphericalHarmonicFieldSettings->getCosineCoefficients( ).cols( ) !=
                      sphericalHarmonicFieldSettings->getSineCoefficients( ).cols( ) ) )
            {
                throw std::runtime_error(
                            std::string( "Error when making spherical harmonic field, sine and " ) +
                            std::string( "cosine matrix  sizes are not equal for body " ) + body );
            }
            else
            {

                if( gravityFieldVariationSettings.size( ) == 0 &&
                        sphericalHarmonicFieldSettings->getCreateTimeDependentField( ) == 0 )
                {
                    // Create and initialize spherical harmonic gravity field model.
                    gravityFieldModel = std::make_shared< SphericalHarmonicsGravityField >(
                                sphericalHarmonicFieldSettings->getGravitationalParameter( ),
                                sphericalHarmonicFieldSettings->getReferenceRadius( ),
                                sphericalHarmonicFieldSettings->getCosineCoefficients( ),
                                sphericalHarmonicFieldSettings->getSineCoefficients( ),
                                sphericalHarmonicFieldSettings->getAssociatedReferenceFrame( ),
                                inertiaTensorUpdateFunction );
                }
                else
                {
                    if( bodyMap.at( body )->getGravityFieldModel( ) != nullptr )
                    {
                        std::string errorMessage = "Warning when making time-dependent gravity field model for body " + body +
                                " existing gravity field is not empty but overwritten in Body! ";
                        throw std::runtime_error( errorMessage );
                    }

                    // Create preliminary TimeDependentSphericalHarmonicsGravityField, without actual variation settings.
                    gravityFieldModel = std::make_shared< TimeDependentSphericalHarmonicsGravityField >(
                                sphericalHarmonicFieldSettings->getGravitationalParameter( ),
                                sphericalHarmonicFieldSettings->getReferenceRadius( ),
                                sphericalHarmonicFieldSettings->getCosineCoefficients( ),
                                sphericalHarmonicFieldSettings->getSineCoefficients( ),
                                sphericalHarmonicFieldSettings->getAssociatedReferenceFrame( ),
                                inertiaTensorUpdateFunction );
                }

            }
        }
        break;
    }
    default:
        throw std::runtime_error(
                    "Error, did not recognize gravity field model settings type " +
                    std::to_string(
                        gravityFieldSettings->getGravityFieldType( ) ) );
    }

    return gravityFieldModel;
}

//! Function to create gravity field settings for a homogeneous triaxial ellipsoid
std::shared_ptr< SphericalHarmonicsGravityFieldSettings > createHomogeneousTriAxialEllipsoidGravitySettings(
        const double axisA, const double axisB, const double axisC, const double ellipsoidDensity,
        const int maximumDegree, const int maximumOrder,
        const std::string& associatedReferenceFrame  )
{
    // Compute reference quantities
    double ellipsoidGravitationalParameter = gravitation::calculateTriAxialEllipsoidVolume(
                axisA, axisB, axisC ) * ellipsoidDensity * physical_constants::GRAVITATIONAL_CONSTANT;
    double ellipsoidReferenceRadius = gravitation::calculateTriAxialEllipsoidVolume(
                axisA, axisB, axisC );

    // Compute coefficients
    std::pair< Eigen::MatrixXd, Eigen::MatrixXd > coefficients =
            gravitation::createTriAxialEllipsoidNormalizedSphericalHarmonicCoefficients(
                axisA, axisB, axisC, maximumDegree, maximumOrder );

    return std::make_shared< SphericalHarmonicsGravityFieldSettings >(
                ellipsoidGravitationalParameter, ellipsoidReferenceRadius, coefficients.first,
                coefficients.second, associatedReferenceFrame );
}

} // namespace simulation_setup

} // namespace tudat


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

#include <nt2/include/functor.hpp>
#include <boost/simd/constant/hierarchy.hpp>
#include <boost/simd/constant/register.hpp>
namespace nt2
{
  namespace tag
  {
   /*!
     @brief Pio2_1 generic tag

     Represents the Pio2_1 constant in generic contexts.

     @par Models:
        Hierarchy
   **/
    // 1.57079632673412561417e+00
    BOOST_SIMD_CONSTANT_REGISTER( Pio2_1, double
                                , 1, 0x3fc90f80
                                , 0x3FF921FB54400000ll
                                )
  }
  namespace ext
  {
   template<class Site, class... Ts>
   BOOST_FORCEINLINE generic_dispatcher<tag::Pio2_1, Site> dispatching_Pio2_1(adl_helper, boost::dispatch::meta::unknown_<Site>, boost::dispatch::meta::unknown_<Ts>...)
   {
     return generic_dispatcher<tag::Pio2_1, Site>();
   }
   template<class... Args>
   struct impl_Pio2_1;
  }
  /*!
    Constant used in modular computation involving \f$\pi\f$

    @par Semantic:

    For type T0:

    @code
    T0 r = Pio2_1<T0>();
    @endcode

    @return a value of type T0
  **/
  BOOST_SIMD_CONSTANT_IMPLEMENTATION(tag::Pio2_1, Pio2_1);
}

#endif


//
// Created by Joris on 09/07/2018.
//

// This code performs an iterative Monte Carlo procedure to obtain a Maximum Entropy
// model for the chromosome organization of C. crescentus

#include <iostream>
#include <Eigen/Dense>
#include <vector>
#include <random>
#include <chrono>
#include <fstream>
#include <unordered_map>
#include <thread>
#include "Initialize.h"
#include "Moves.h"
using namespace Eigen;

const int number_of_threads = 36;

const double diameter = 6.4;
const int length_cylinder = 21;
const int midway = int(ceil(length_cylinder/2));
const int cap_length = 2;

const int pol_length = 1620;

const long int mc_moves_start = 100000000;
long int mc_moves;
const int burn_in_time = 20000000;
const int update_steps = 30;
const double learning_rate = 0.5;

std::vector<std::vector<Vector3i>> polymer(number_of_threads);

bool boundary_cond = 1; //enforces boundary conditions if 1
bool orient = 1; //orients the cell such that the origin is always in the left half

std::vector< std::vector<double>> Interaction_E(pol_length, std::vector<double>(pol_length,0));

std::uniform_real_distribution<double> unif(0.0,1.0);
std::uniform_int_distribution<int> unimove(0,2);
std::uniform_int_distribution<int> unisite(0,pol_length-1);
std::vector<std::vector<std::vector<double>>> total_contacts(number_of_threads, std::vector< std::vector<double>>(pol_length, std::vector<double>(pol_length, 0)));
std::vector<std::vector<double>> final_contacts(pol_length, std::vector<double>(pol_length, 0));

void move(std::vector<Vector3i> &polymer,int thread_num, int m){ //performs a single Monte Carlo step
    int action;
    int site;

    action = unimove(gen);
    site = unisite(gen);
    if (action==0){
        kink_move(polymer,site, thread_num,m);
    }
    else if (action==1){
        crankshaft_move(polymer,site, thread_num,m);
    }
    else if (action==2){
       loop_move(polymer,site, thread_num,m);
    }
}

void run_burnin(int thread_num, int mc_moves) { //burns in the polymer configurations
    for (int m = 1; m < mc_moves; m++) {
        move(polymer[thread_num], thread_num, m);
    }
}

void run(int thread_num, int mc_moves) {
    for (int m = 1; m < mc_moves; m++) {    //performs a forward polymer simulation
        move(polymer[thread_num], thread_num, m);
    }
}

//after each forward run, the polymer interaction energies are updated according to the pairwise difference
// between model contact frequencies and experimental contact frequencies
void update_energies(std::vector< std::vector<double>> &total_contacts, std::vector< std::vector<double>> &reference_contacts, std::vector< std::vector<double>> &Interaction_E) {

    float checksum = 0;
    for (int i = 0; i < pol_length/4; i++) {
        for (int j = i+1; j < pol_length/4; j++) {
            if ((i != (j+1)%(pol_length/4)) && (j != (i+1)%(pol_length/4))) {
                Interaction_E[4 * i][4 * j] += learning_rate * sqrt(1/(std::max(reference_contacts[4*i][4*j],0.0001)))*(float(total_contacts[4 * i][4 * j])- reference_contacts[4 * i][4 * j]);
                checksum += float(total_contacts[4 * i][4 * j])*(pol_length/4);
                Interaction_E[4 * j][4 * i] = Interaction_E[4 * i][4 * j];
            }
        }
    }

    //calculates the shift of all energies, imposed to ensure a MaxEnt solution is found for the contact frequency scale
    float shift = 0;
    for (int i = 0; i < pol_length/4; i++) {
        for (int j = i + 2; j < pol_length / 4; j++) {
           shift += 2*(Interaction_E[4 * i][4 * j] * reference_contacts[4 * i][4 * j]) / (pol_length / 4);
        }
    }

    std::cout << "Shift: " << shift << std::endl;
    for (int i = 0; i < pol_length/4; i++) {
        for (int j = i + 2; j < pol_length / 4; j++) {
            //if ((i != (j + 1) % (pol_length / 2)) && (j != (i + 1) % (pol_length / 2)) && not(i >=0 && i<=30 && j>=150 && j<=220) && not(i >=150 && i <=230 && j>=370 && j<=410) ) {
            Interaction_E[4 * i][4 * j] -= shift;
            Interaction_E[4 * j][4 * i] = Interaction_E[4 * i][4 * j];
            //}
        }
    }
}

//Normalizes model contact frequencies to allow for comparison with experimental data
void normalize() {
    double sum = 0;
    for (int i = 0; i < pol_length; i++) {
        for (int j = 0; j < pol_length; j++) {
            if (i%4 == 0 && j%4 == 0 && i!=(j+4)%pol_length && j!=(i+4)%pol_length) {
                sum += final_contacts[i][j];
            }
        }
    }
    for (int i = 0; i < pol_length; i++) {
        for (int j = 0; j < pol_length; j++) {
            final_contacts[i][j] *= double(pol_length)/(8*sum);
        }
    }
}

int main() {
    auto start = std::chrono::high_resolution_clock::now();
    std::cout << "started! " << std::endl;

    for (int l = 0; l < number_of_threads; l++) {
        initialize(polymer[l], pol_length, l);
    }
    std::cout << "Initialized " << std::endl;

    // Read in starting interaction energies
    std::ifstream couplings;
    couplings.open("/media/joris/raid_data/Chromosome_Maxent/Dataprocess_check_review/Processed_new_r0/Inverse/Energies_Intermediate/energies_intermediate_49.txt");
    //couplings.open ("/media/joris/raid_data/Chromosome_Maxent/Saved_energies/energies_smallcell.txt");
    for (int i = 0; i < pol_length; i++) { //read in energies
        for (int j = 0; j < pol_length; j++) {
            couplings >> Interaction_E[i][j];
        }
    }
    couplings.close();

    // Read in reference contacts
    std::vector<std::vector<double>> reference_contacts(pol_length, std::vector<double>(pol_length));
    std::ifstream input_contacts;
    input_contacts.open("/media/joris/raid_data/Chromosome_Maxent/experimental_data_noisecancel_r1.txt");
    for (int i = 0; i < pol_length / 4; i++) { //read in contacts
        for (int j = 0; j < pol_length / 4; j++) {
            input_contacts >> reference_contacts[4 * i][4 * j];
            if (reference_contacts[4*i][4*j] == 0 && i != j && i != (j + 1) % (pol_length / 2) &&
                j != (i + 1) % (pol_length / 2)) {
                Interaction_E[4 * i][4 * j] = 10;
            }
        }
    }
    input_contacts.close();

    // burn in configurations
    std::vector<std::thread> threads(number_of_threads);
    for (auto l = 0; l < number_of_threads; l++) {
        threads[l] = std::thread(run_burnin, l, burn_in_time);
    }
    for (auto &&l : threads) {
        l.join();
    }

    std::cout << "Done with burn in " << std::endl;

    for (int n =0; n<update_steps;n++) { //do iterative update scheme
        mc_moves = mc_moves_start*sqrt(n+10)/sqrt(10); //number of MC moves grows with each iteration (implicitly converted to long int)

        for (int l = 0; l < number_of_threads; l++) {
            for (int i = 0; i < pol_length; i++) { //reset contact frequencies before starting new forward round
                for (int j = 0; j < pol_length; j++) {
                    total_contacts[l][i][j] = 0;
                    if (l==0){
                        final_contacts[i][j] =0;
                    }
                }

            }
            for (auto elem : contacts[l]) { //reset contacts
                contacts[l][elem.first] = 0;
            }
        }

        //run forward simulation
        for (auto l = 0; l < number_of_threads; l++) {
            threads[l] = std::thread(run, l, mc_moves);
        }
        for (auto &&l : threads) {
            l.join();
        }
        // read in remaining contacts at the end of forward simulation
        for (auto l = 0; l < number_of_threads; l++) {
            for (auto elem : contacts[l]) { //add the contacts remaining at the end of the simulation
                total_contacts[l][elem.first.first][elem.first.second] += mc_moves - elem.second;
                contacts[l][elem.first] = 0;
            }
        }

        //add up contacts from threads
        for (int i = 0; i < number_of_threads; i++) {
            for (int j = 0; j < pol_length; j++) {
                for (int k = 0; k < pol_length; k++) {
                    final_contacts[j][k] += total_contacts[i][j][k];
                }
            }
        }

        //normalize contact frequencies
        normalize();

        //update energies
        update_energies(final_contacts,reference_contacts,Interaction_E);

        //output intermediate results
        std::ofstream intermediate_energies;
        intermediate_energies.open ("/media/joris/raid_data/Chromosome_Maxent/Dataprocess_check_review/Processed_new_r0/Inverse/Energies_Intermediate/energies_intermediate_" + std::to_string(n) + ".txt");
        for(int i = 0; i < pol_length; i++){ //output contact frequencies
            for(int j = 0; j < pol_length; j++){
                intermediate_energies << Interaction_E[i][j] << std::endl;
            }
        }
        intermediate_energies.close();

        std::ofstream intermediate_contacts;
        intermediate_contacts.open ("/media/joris/raid_data/Chromosome_Maxent/Dataprocess_check_review/Processed_new_r0/Inverse/Contacts_Intermediate/contacts_intermediate_" + std::to_string(n) + ".txt");
        for(int i = 0; i < pol_length; i++){ //output contact frequencies
            for(int j = 0; j < pol_length; j++){
                double contact = double(final_contacts[i][j]);
                intermediate_contacts << contact << std::endl;
            }
        }
        intermediate_contacts.close();
    }

    std::cout << "Finished! "  << std::endl;

    //output final configurations for each thread
    int n_capture = 0;
    while (n_capture <number_of_threads) {
        for (int thread_num = 0; thread_num < number_of_threads; thread_num++) {
            if (polymer[thread_num][0][2] < 20) {
                std::ofstream final_configuration;
                final_configuration.open(
                        "/media/joris/raid_data/Chromosome_Maxent/Dataprocess_check_review/Processed_new_r0/Inverse/configuration_init_" +
                        std::to_string(n_capture) + ".txt");
                for (int i = 0; i < pol_length; i++) { //output contact frequencies
                    for (int j = 0; j < 3; j++) {
                        final_configuration << polymer[thread_num][i][j] << std::endl;
                    }
                }
                final_configuration.close();
                n_capture++;
            }
        }
    }

    auto finish = std::chrono::high_resolution_clock::now();
    std::chrono::duration<double> elapsed = finish - start;
    std::cout << "Elapsed time: " <<  elapsed.count() << " seconds\n";
    return 0;
}


#include <algorithm>
#include <cerrno>
#include <csignal>
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <fstream>
#include <iostream>
#include <iterator>
#include <limits>
#include <map>
#include <math.h>
#include <pthread.h>
#include <sstream>
#include <string>
#include <sys/ptrace.h>
#include <sys/time.h>
#include <sys/user.h>
#include <sys/wait.h>
#include <thread>
#include <unistd.h>
#include <vector>

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>

#include "papi.h"

#include "supereasyjson/json.h"
#include "papi-helpers.hpp"

#ifdef DEBUG
#define print(...) printf(__VA_ARGS__)
#else
#define print(...)
#endif

using namespace std;
using namespace boost::numeric;
using namespace json;
using namespace papi;

namespace{

/*
 * Constants
 */
const unsigned kProcStatIdx = 39;
const long kDefaultSamplePeriodUsecs = 1000;
const long kMicroToBase = 1e6;
const long kNanoToBase = 1e9;
const char* kDefaultPrefix = "eaudit";
const string kPackageEnergyName = "rapl:::PACKAGE_ENERGY:PACKAGE0";
const string kDRAMEnergyName = "rapl:::DRAM_ENERGY:PACKAGE0";
const string kCoreEnergyName = "rapl:::PP0_ENERGY:PACKAGE0";
const vector<string> kAllEnergyNames = {kCoreEnergyName, kPackageEnergyName, kDRAMEnergyName};
const char* kDefaultModelName = "default.model";
const int kTotalCoreAssignments = 5;

struct stats_t {
  long time; // in microseconds
  vector<long long> counters;
  stats_t& operator+=(const stats_t &rhs){
    time += rhs.time;
    counters.resize(rhs.counters.size());
    for(unsigned int i = 0; i < rhs.counters.size(); ++i){
      counters[i] += rhs.counters[i];
    }
    return *this;
  }
  // initialize time to 0 for new stats
  stats_t() : time{0} {}
};

struct result_stats_t {
  stats_t per_core_stats;
  long long estimated_energy;
  result_stats_t& operator+=(const result_stats_t &rhs){
    per_core_stats += rhs.per_core_stats;
    estimated_energy += rhs.estimated_energy;
    return *this;
  }
};

/*
 * Global variables
 */
volatile bool is_timer_done = false;
} // end unnamed namespace

struct Model {
  Model(const string& model_fname) : model_fname_{model_fname} {
    ifstream in{model_fname_};
    if(!in.is_open()){
      cerr << "Unable to open model file\n";
      exit(-1);
    }
    stringstream buffer;
    buffer << in.rdbuf();
    json::Value modelval = json::Deserialize(buffer.str());
    if(modelval.GetType() == json::NULLVal){
      cerr << "Unable to parse json from model file\n";
      exit(-1);
    }
    json::Object model = modelval.ToObject();

    for(const auto& name : model["metric_names"].ToArray()){
      input_metrics_.push_back(name.ToString());
    }

    const auto means = model["means"].ToArray();
    means_.resize(means.size());
    for(size_t i = 0; i < means.size(); ++i){
      means_[i] = means[i].ToDouble();
    }

    const auto std_devs = model["std_devs"].ToArray();
    std_deviations_.resize(std_devs.size());
    for(size_t i = 0; i < std_devs.size(); ++i){
      std_deviations_[i] = std_devs[i].ToDouble();
    }

    const auto rot_mat = model["rotation_matrix"].ToArray();
    const auto n_rot_mat_rows = rot_mat.size();
    if(n_rot_mat_rows > 0){
      const auto n_rot_mat_cols = rot_mat[0].ToArray().size();
      principal_components_.resize(n_rot_mat_rows, n_rot_mat_cols);
      for(size_t i = 0; i < n_rot_mat_rows; ++i){
        for(size_t j = 0; j < n_rot_mat_cols; ++j){
          principal_components_(i,j) = rot_mat[i][j].ToDouble();
        }
      }
    }

    const auto clusters = model["clusters"].ToArray();
    for(const auto& cluster_val : clusters){
      const auto& cluster = cluster_val.ToObject();
      model_t internalized_model;

      const auto center = cluster["center"].ToArray();
      internalized_model.centroid_.resize(center.size());
      for(size_t i = 0; i < center.size(); ++i){
        internalized_model.centroid_[i] = center[i].ToDouble();
      }

      const auto regressors = cluster["regressors"].ToArray();
      internalized_model.regressors_.resize(regressors.size());
      internalized_model.weights_.resize(regressors.size());
      for(size_t i = 0; i < regressors.size(); ++i){
        // turn function object into a lambda doing the right thing
        auto regressor = regressors[i].ToObject();
        auto regressor_name = regressor["function"].ToString();
        if(regressor_name == "identity"){
          internalized_model.regressors_[i] = function<double(const ublas::vector<double>&)>(
                                   [](ublas::vector<double>){ return 1.0; });
        } else if(regressor_name == "power"){
          auto idx = regressor["index"].ToInt();
          auto exp = regressor["exponent"].ToDouble();
          internalized_model.regressors_[i] = function<double(const ublas::vector<double>&)>(
            [=](const ublas::vector<double>& v) {
              return v(idx) != 0.0 ? pow(fabs(v(idx)), exp) : 1.0;
            });
        } else if(regressor_name == "product"){
          auto first_idx = regressor["first_idx"].ToInt();
          auto second_idx = regressor["second_idx"].ToInt();
          internalized_model.regressors_[i] = function<double(const ublas::vector<double>&)>(
            [=](const ublas::vector<double>& v) {
              return v(first_idx) * v(second_idx);
            });
        } else if(regressor_name == "sqrt"){
          auto idx = regressor["index"].ToInt();
          internalized_model.regressors_[i] = function<double(const ublas::vector<double>&)>(
            [=](const ublas::vector<double>& v) {
              return sqrt(fabs(v(idx)));
            });
        } else if(regressor_name == "log"){
          auto idx = regressor["index"].ToInt();
          internalized_model.regressors_[i] = function<double(const ublas::vector<double>&)>(
            [=](const ublas::vector<double>& v) {
              return idx == 0 ? 1.0 : log(fabs(v(idx)))/log(2);
            });
        } else {
          cerr << "Invalid function name '" << regressor["name"].ToString() << "\n";
          exit(-1);
        }
        internalized_model.weights_[i] = regressor["weight"].ToDouble();
      }
      models_.push_back(internalized_model);
    }
  }

  double poll(const vector<long long>& values, const vector<string>& names) const {
    ublas::vector<double> v = ublas::vector<double>(input_metrics_.size());
    //vector<string> input_metrics_;

    for(unsigned i = 0; i < v.size(); ++i){
      for(unsigned j = 0; j < names.size(); ++j){
        if(names[j] == input_metrics_[j]){
          v[i] = values[j];
          break;
        }
      }
    }

    ublas::vector<double> inputs;
    inputs = prod(v,principal_components_);
    ublas::vector<double> norm_inputs = element_div(inputs - means_, std_deviations_);

    int closest = 0;
    double min_distance = numeric_limits<double>::max();
    for(unsigned k = 0; k < models_.size(); ++k) {
      //get distance
      double distance = norm_2(norm_inputs - models_[k].centroid_);
      if(distance < min_distance) {
        closest = k;
        min_distance = distance;
      }
    }
    ublas::vector<double> function_vals =
      ublas::vector<double>(models_[closest].regressors_.size());
    int i = 0;
    for(auto it =
          begin(models_[closest].regressors_);
        it != end(models_[closest].regressors_); ++it, ++i) {
      function_vals(i) = (*it)(inputs);
    }
    double final_res = fabs(inner_prod(models_[closest].weights_,function_vals));
    return final_res;
  }

  string model_fname_;

  struct model_t {
    ublas::vector<double> centroid_;
    ublas::vector<double> weights_;
    vector<function<double(const ublas::vector<double>&)>> regressors_;
  };

  ublas::vector<double> means_;
  ublas::vector<double> std_deviations_;
  ublas::matrix<double> principal_components_;
  vector<string> input_metrics_;
  vector<model_t> models_;
};

void overflow(int signum, siginfo_t* info, void* context){
  (void)info;
  (void)context;
  if(signum == SIGALRM){
    is_timer_done = true;
  }
}

stats_t read_rapl(const event_info_t& eventsets, long period){
  stats_t res;
  res.counters.resize(eventsets.codes.size());
  int retval=PAPI_stop(eventsets.set, &res.counters[0]);
  if(retval != PAPI_OK){
    cerr << "Error: bad PAPI stop: ";
    PAPI_perror(NULL);
    terminate();
  }
  retval = PAPI_start(eventsets.set);
  if(retval != PAPI_OK){
    cerr << "Error: bad PAPI stop: ";
    PAPI_perror(NULL);
    terminate();
  }
  res.time = period;
  return res;
}

vector<long long> modelPerCoreEnergies(const Model& model,
                                       const vector<stats_t>& core_stats,
                                       const vector<string>& counter_names,
                                       long long total_energy) {
  vector<long long> results(core_stats.size());
  //double poll(map<string, double> params) const {
  vector<double> model_vals;
  model_vals.reserve(core_stats.size());
  for(const auto& core_stat : core_stats){
    model_vals.push_back(model.poll(core_stat.counters, counter_names));
  }
  double total = accumulate(begin(model_vals), end(model_vals), double{0});
  for(unsigned i = 0; i < results.size(); ++i){
    results[i] = model_vals[i] / total * total_energy;
  }
  return results;
}

struct ProfileValue{
  double processor_energy, uncore_energy, dram_energy;
  double time;
  double instructions;
  ProfileValue() : processor_energy{0}, uncore_energy(0), dram_energy(0), time{0}, instructions{0} {}
};

struct ProfileEntry{
  string name;
  double processor_energy, uncore_energy, dram_energy, time, instructions;
};

void do_profiling(int profilee_pid, const char* profilee_name,
                  const long period, const char* prefix,
                  const Model& proc_model, const Model& uncore_model, const Model& dram_model) {
  /*
   * Structures holding profiling data
   */
  vector<int> children_pids;
  vector<map<void*, ProfileValue>> core_profiles;
  stats_t global_stats;
  global_stats.counters.resize(3);
  vector<event_info_t> core_counters;
  // TODO: assumption here is that hyperthreading is turned on, and that there
  // are two hardware threads per physical core. We have to make sure we only
  // run the correct number of threads during auditing, since this assumption
  // is made.
  auto ncores = thread::hardware_concurrency() / 2;
  //auto ncores = 1u;
  core_profiles.resize(ncores);
  core_counters.reserve(ncores);

  /*
   * Initialize PAPI
   */
  print("Init PAPI\n");
  int retval;
  if ((retval = PAPI_library_init(PAPI_VER_CURRENT)) != PAPI_VER_CURRENT) {
    cerr << "Unable to init PAPI library - " << PAPI_strerror(retval) << endl;
    exit(-1);
  }

  /*
   * Initialize PAPI measurement of all cores
   */
  int inst_counter_idx = 0;
  // get all the names for all the input counters
  vector<string> counter_names = proc_model.input_metrics_;
  counter_names.insert(end(counter_names), begin(uncore_model.input_metrics_), end(uncore_model.input_metrics_));
  counter_names.insert(end(counter_names), begin(dram_model.input_metrics_), end(dram_model.input_metrics_));
  // remove all duplicates
  sort(begin(counter_names), end(counter_names));
  auto last_elem = unique(begin(counter_names), end(counter_names));
  counter_names.erase(last_elem, end(counter_names));
  auto inst_iter = find(begin(counter_names), end(counter_names), "PAPI_TOT_INS");
  if(inst_iter == end(counter_names)){
    inst_counter_idx = counter_names.size();
    counter_names.push_back("PAPI_TOT_INS");
  } else {
    inst_counter_idx = distance(begin(counter_names), inst_iter);
  }
  // setup all core counters
  for(unsigned int i = 0; i < ncores; ++i){
    print("Creating per-core counters on core %d\n", i);
    core_counters.emplace_back(init_papi_counters(counter_names));
    auto& counters = core_counters[i];
    attach_counters_to_core(counters, i);
    start_counters(counters);
  }
  print("Creating global counters.\n");
  auto global_counters = init_papi_counters(kAllEnergyNames);
  start_counters(global_counters);

  /*
   * Setup tracing of all profilee threads
   */
  children_pids.push_back(profilee_pid);

  /*
   * Set up timer
   */
  struct sigaction sa;
  sa.sa_sigaction = overflow;
  sa.sa_flags = SA_SIGINFO;
  if (sigaction(SIGALRM, &sa, nullptr) != 0) {
    cerr << "Unable to set up signal handler\n";
    exit(-1);
  }
  struct itimerval work_time;
  time_t sleep_secs = period / kMicroToBase;
  suseconds_t sleep_usecs = period % kMicroToBase;
  work_time.it_value.tv_sec = sleep_secs;
  work_time.it_value.tv_usec = sleep_usecs;
  work_time.it_interval.tv_sec = sleep_secs;
  work_time.it_interval.tv_usec = sleep_usecs;
  setitimer(ITIMER_REAL, &work_time, nullptr);

  /*
   * Let the profilee run, periodically interrupting to collect profile data.
   */
  ptrace(PTRACE_CONT, profilee_pid, nullptr, nullptr); // Allow child to fork
  int status;
  wait(&status); // wait for child to begin executing
  print("Start profiling.\n");
  /* Reassert that we want the profilee to stop when it clones */
  ptrace(PTRACE_SETOPTIONS, profilee_pid, nullptr,
         PTRACE_O_EXITKILL | PTRACE_O_TRACECLONE | PTRACE_O_TRACEEXIT);
  ptrace(PTRACE_CONT, profilee_pid, nullptr, nullptr); // Allow child to run!
  using core_id_t = int;
  using assignments_left_t = int;
  map<int, pair<core_id_t, assignments_left_t>> children_cores;
  // We only want to match child PIDs to processors infrequently, since it
  // requires a filesystem read. The assumption here is that threads are bound
  // to cores, and only get created at the beginning of the function
  // 50 is a magic number derived by running some apps a bunch of times.
  auto start_time = PAPI_get_real_usec();
  for (;;) {
    auto wait_res = waitpid(-1, &status, __WALL);
    if(wait_res == -1){ // bad wait!
      if(errno == EINTR && is_timer_done){ // timer expired, do profiling
        // halt timer
        work_time.it_value.tv_sec = 0;
        work_time.it_value.tv_usec = 0;
        setitimer(ITIMER_REAL, &work_time, nullptr);

        // kill all the children
        for(const auto& child : children_pids){
          kill(child, SIGSTOP);
        }

        // find last executing core ID for each child
        for(const auto& child : children_pids){
          auto core_iter = children_cores.find(child);
          if(core_iter == end(children_cores)){
            auto& elem = children_cores[child];
            elem.second = kTotalCoreAssignments;
            core_iter = children_cores.find(child);
          }
          if(core_iter->second.second > 0){
            core_iter->second.second--;
            stringstream proc_fname;
            proc_fname << "/proc/" << child << "/stat";
            ifstream procfile(proc_fname.str());
            if(!procfile.is_open()){
              cerr << "Error: couldn't open proc file!\n";
              exit(-1);
            }
            string line;
            // ignore the first kProcStatIdx lines
            for(unsigned int i = 0; i < kProcStatIdx; ++i){
              getline(procfile, line, ' ');
            }
            core_iter->second.first = stoi(line);
          }
        }

        // collect stats from cores
        print("EAUDIT collating stats\n");
        // read all rapl counters
        vector<stats_t> stats(ncores);
        for(unsigned int i = 0; i < ncores; ++i){
          stats[i] = read_rapl(core_counters[i], period);
        }
        auto cur_global_stats = read_rapl(global_counters, period);
        global_stats += cur_global_stats;
        cout << "p: " << cur_global_stats.counters[0] << endl;
        cout << "u: " << cur_global_stats.counters[1] << endl;
        cout << "m: " << cur_global_stats.counters[2] << endl;

        // global counter 0 is processor plane energy
        auto proc_energies = modelPerCoreEnergies(
          proc_model, stats, counter_names, cur_global_stats.counters[0]);

        // global counter 1 is package energy, including the processor plane
        // (which we have to remove to calcluate uncore energy
        auto uncore_energies = modelPerCoreEnergies(
          uncore_model, stats, counter_names, cur_global_stats.counters[1] - cur_global_stats.counters[0]);
        // global counter 2 is DRAM energy
        auto dram_energies = modelPerCoreEnergies(
          dram_model, stats, counter_names, cur_global_stats.counters[2]);

        // read all the children registers
        for(const auto& child : children_pids){
          struct user_regs_struct regs;
          ptrace(PTRACE_GETREGS, child, nullptr, &regs);
          void* rip = (void*)regs.rip;
          auto child_core = children_cores[child].first;
          // TODO This is a hack to avoid attempting to log threads that
          // miraculously end up on the second hardware thread of a core.
          // We try to make sure things are bound appropriately, but there's
          // no guarantee, so we just ignore things that happen where we don't
          // want them.
          if((unsigned)child_core >= ncores) { continue; }
          auto& profile = core_profiles[child_core][rip];
          profile.processor_energy += proc_energies[child_core];
          profile.uncore_energy += uncore_energies[child_core];
          profile.dram_energy += dram_energies[child_core];
          profile.time += stats[child_core].time;
          profile.instructions += stats[child_core].counters[inst_counter_idx];
        }

        // resume all children
        for(const auto& child : children_pids){
          ptrace(PTRACE_CONT, child, nullptr, nullptr);
        }
        is_timer_done = false;
        // resume timer
        work_time.it_value.tv_sec = sleep_secs;
        work_time.it_value.tv_usec = sleep_usecs;
        setitimer(ITIMER_REAL, &work_time, nullptr);
      } else {
        cerr << "Error: unexpected return from wait - " << strerror(errno) << "\n";
        exit(-1);
      }
    } else { // good wait, add new thread
      if(status>>8 == (SIGTRAP | (PTRACE_EVENT_CLONE<<8))) { // new thread created
        print("New thread created.\n");
        unsigned long new_pid;
        ptrace(PTRACE_GETEVENTMSG, wait_res, nullptr, &new_pid);
        auto pid_iter = find(begin(children_pids), end(children_pids), new_pid);
        if(pid_iter != end(children_pids)) {
          cerr << "Already have this newly cloned pid: " << new_pid << ".\n";
          exit(-1);
        }
        print("Thread ID %lu created from thread ID %d\n", new_pid, wait_res);
        children_pids.push_back(new_pid);
        ptrace(PTRACE_SETOPTIONS, new_pid, nullptr,
               PTRACE_O_EXITKILL | PTRACE_O_TRACECLONE | PTRACE_O_TRACEEXIT);
        ptrace(PTRACE_CONT, wait_res, nullptr, nullptr);
      } else {
        if(status>>8 == (SIGTRAP | (PTRACE_EVENT_EXIT<<8))){
          print("Deleting child %d\n", wait_res);
          auto pid_iter = find(begin(children_pids), end(children_pids), wait_res);
          if(pid_iter == end(children_pids)){
            cerr << "Error: Saw exit from pid " << wait_res << ". We haven't seen before!\n";
            exit(-1);
          }
          children_pids.erase(pid_iter);
          if(children_pids.size() == 0){ // All done, not tracking any more threads
            break;
          }
          print("%lu children left\n", children_pids.size());
        }
        // always let the stopped tracee continue
        ptrace(PTRACE_CONT, wait_res, nullptr, nullptr);
      }
    }
  }
  auto elapsed_time = PAPI_get_real_usec() - start_time;

  /*
   * Done profiling. Convert data to output file.
   */
  print("Finalize profile.\n");
  auto profile_start_time = PAPI_get_real_usec();
  for(unsigned int i = 0; i < ncores; ++i){
    vector<ProfileEntry> profile;
    // Convert stack IDs into function names.
    for (auto& core_profile : core_profiles[i]) {
      stringstream cmd;
      cmd << "addr2line -f -s -C -e " << profilee_name << " " << core_profile.first;
      auto pipe = popen(cmd.str().c_str(), "r");  // call command and read output
      if (!pipe) {
        cerr << "Unable to open pipe to call addr2line.\n";
        return;
      }
      char buffer[128];
      string result = "";
      while (!feof(pipe)) {
        if (fgets(buffer, 128, pipe) != nullptr) {
          result += buffer;
        }
      }
      pclose(pipe);
      stringstream resultstream{result};
      string func_name, file_name;
      getline(resultstream, func_name);
      getline(resultstream, file_name);
      // NOTE: remove the trailing function annotation that says that this
      // function has been used/called by different threads
      if(func_name.back() == ']'){
        auto last_open_bracket_pos = func_name.find_last_of('[');
        func_name.erase(last_open_bracket_pos - 1);
      }
      file_name.erase(file_name.find(':'));
      string entry_name = func_name + " at " + file_name;
      print("Reporting function %s\n", entry_name.c_str());

      auto profile_iter =
          find_if(begin(profile), end(profile),
                  [&](const ProfileEntry& e) { return e.name == entry_name; });
      if(profile_iter == end(profile)){
        ProfileEntry entry;
        entry.name = entry_name;
        profile.push_back(entry);
        profile_iter = end(profile) - 1;
      }
      profile_iter->processor_energy += core_profile.second.processor_energy;
      profile_iter->uncore_energy += core_profile.second.uncore_energy;
      profile_iter->dram_energy += core_profile.second.dram_energy;
      profile_iter->time += core_profile.second.time;
      profile_iter->instructions += core_profile.second.instructions;
    }
    sort(begin(profile), end(profile),
         [&](const ProfileEntry& a, const ProfileEntry& b) {
           auto a_energy = a.processor_energy + a.uncore_energy + a.dram_energy;
           auto b_energy = b.processor_energy + b.uncore_energy + b.dram_energy;
           return a_energy > b_energy;
         });

    /*
     * Write profile to file
     */
    stringstream namestream;
    namestream << prefix << "." << i << ".tsv";
    ofstream outfile{namestream.str()};
    outfile << "Name\tProcessor Energy\tUncore Energy\tDRAM Energy\tTime\tInstructions\n";
    for(const auto& elem : profile){
      outfile << elem.name << "\t"
              << elem.processor_energy / kNanoToBase << "\t"
              << elem.uncore_energy / kNanoToBase << "\t"
              << elem.dram_energy / kNanoToBase << "\t"
              << elem.time / kMicroToBase << "\t"
              << elem.instructions << "\n";
    }
  }

  cout << "Total Processor Energy:\t" << global_stats.counters[0] / (double)kNanoToBase << " joules\n"
       << "Total Uncore Energy:\t" << (global_stats.counters[1] - global_stats.counters[0]) / (double)kNanoToBase << " joules\n"
       << "Total DRAM Energy:\t" << global_stats.counters[2] / (double)kNanoToBase << " joules\n"
       << "Elapsed Time:\t" << elapsed_time / (double)kMicroToBase << " seconds\n";

  auto profile_elapsed = PAPI_get_real_usec() - profile_start_time;
  cout << "Profile creation time:\t" << profile_elapsed / (double) kMicroToBase << " seconds\n";
}

int main(int argc, char* argv[], char* envp[]) {
  /*
   * Check params
   */
  auto usage =
    "Usage:\n"
    " eaudit [options] executable\n"
    "\n"
    "Options:\n"
    " -h                  Show this help\n"
    " -p <microseconds>   Sample period in microseconds, default 1000\n"
    " -o <prefix>         Prefix to use when writing files, default eaudit\n"
    " -m <filename>       Model file name, default 'default.model'\n"
    "\n";

  auto period = kDefaultSamplePeriodUsecs;
  auto proc_model_fname = kDefaultModelName;
  auto uncore_model_fname = kDefaultModelName;
  auto dram_model_fname = kDefaultModelName;
  auto prefix = kDefaultPrefix;
  int param;
  while((param = getopt(argc, argv, "+hp:o:c:u:m:")) != -1){
    switch(param){
      case 'p':
        period = stol(optarg);
        break;
      case 'o':
        prefix = optarg;
        break;
      case 'c':
        {
          proc_model_fname = optarg;
        }
        break;
      case 'u':
        {
          uncore_model_fname = optarg;
        }
        break;
      case 'm':
        {
          dram_model_fname = optarg;
        }
        break;
      case 'h':
      case '?':
        cout << usage;
        exit(0);
        break;
      default:
        cerr << "Error: bad getopt parse of parameter.\n";
        exit(-1);
        break;
    }
  }

  /*
   * Make our models
   */
  Model proc_model{proc_model_fname};
  Model uncore_model{uncore_model_fname};
  Model dram_model{dram_model_fname};

  /*
   * Fork a process to run the profiled application
   */
  auto profilee = fork();
  if(profilee > 0){ /* parent */
    // Let's do this.
    ptrace(PTRACE_SETOPTIONS, profilee, nullptr,
           PTRACE_O_EXITKILL | PTRACE_O_TRACECLONE | PTRACE_O_TRACEEXIT);
    do_profiling(profilee, argv[optind], period, prefix, proc_model, uncore_model, dram_model);
  } else if(profilee == 0){ /* profilee */
    // prepare for tracing
    ptrace(PTRACE_TRACEME, 0, nullptr, nullptr);
    raise(SIGSTOP);
    // start up client program
    execve(argv[optind], &argv[optind], envp);
    cerr << "Error: profilee couldn't start its program!\n";
    perror(nullptr);
    exit(-1);
  } else { /* error */
    cerr << "Error: couldn't fork audited program.\n";
    return -1;
  }
}


//
// GMOM.cpp
//
// Copyright (c) 2017 Shion Hosoda
//
// This software is released under the MIT License.
// http://opensource.org/licenses/mit-license.php
//

//include{{{
#include <stdlib.h>
#include <boost/program_options.hpp>
#include <boost/mpi/environment.hpp>
#include <boost/mpi/communicator.hpp>
#include <boost/mpi/collectives.hpp>
#include "include/CsvFileParser.hpp"
#include "include/GibbsSamplerFromGMOM.hpp"
//}}}

using namespace std;

int main(int argc, char *argv[]){

    //Options{{{
    boost::program_options::options_description opt("Options");
    opt.add_options()
    ("help,h", "show help")
    ("otpt,o", boost::program_options::value<string>()->default_value("./"), "directory name for output")
    ("itnm,n", boost::program_options::value<unsigned int>()->default_value(1000), "the number of iteration")
    ("intr,i", boost::program_options::value<unsigned int>()->default_value(5), "sampling interval")
    ("bnin,b", boost::program_options::value<unsigned int>()->default_value(500), "burn-in term")
    ("k,k", boost::program_options::value<double>()->default_value(1.0), "k value")
    ("theta,t", boost::program_options::value<double>()->default_value(1.0), "theta value")
    ("A,A", boost::program_options::value<double>()->default_value(1000.0), "A value")
    ;

    boost::program_options::positional_options_description pd;
    pd.add("orthologfile", 1);
    pd.add("microbefile", 2);

    boost::program_options::options_description hidden("hidden");
    hidden.add_options()
        ("orthologfile", boost::program_options::value<string>(), "hidden")
        ("microbefile", boost::program_options::value<string>(), "hidden")
        ;
    boost::program_options::options_description cmdline_options;
    cmdline_options.add(opt).add(hidden);

    boost::program_options::variables_map vm;
    try{
        boost::program_options::store(boost::program_options::command_line_parser(argc, argv).options(cmdline_options).positional(pd).run(), vm);
    }catch(const boost::program_options::error_with_option_name& e){
        cout<<e.what()<<endl;
    }
    boost::program_options::notify(vm);
    string orthologFilename;
    string microbeFilename;
    double k, theta, A;
    unsigned int iterationNumber;
    unsigned int samplingInterval;
    unsigned int burnIn;
    string outputDirectory;
    string PFilename;
    string VFilename;
    string logLikelihoodFilename;
    if (vm.count("help") || !vm.count("orthologfile") || !vm.count("microbefile")){
        cout<<"Usage:\n GMOM [ortholog file] [microbe file] [-options] "<<endl;
        cout<<endl;
        cout<<opt<<endl;
        exit(1);
    }else{
        orthologFilename = vm["orthologfile"].as<std::string>();
        microbeFilename = vm["microbefile"].as<std::string>();
        if(vm.count("k"))k = vm["k"].as<double>();
        if(vm.count("theta"))theta = vm["theta"].as<double>();
        if(vm.count("A"))A = vm["A"].as<double>();
        if(vm.count("itnm"))iterationNumber = vm["itnm"].as<unsigned int>();
        if(vm.count("intr"))samplingInterval = vm["intr"].as<unsigned int>();
        if(vm.count("bnin"))burnIn = vm["bnin"].as<unsigned int>();
        if(vm.count("otpt"))outputDirectory = vm["otpt"].as<std::string>();
        if(outputDirectory[outputDirectory.size()-1] != '/')outputDirectory.push_back('/');
        PFilename = outputDirectory + "P.csv";
        VFilename = outputDirectory + "V.csv";
        logLikelihoodFilename = outputDirectory + "LogLikelihood.csv";
    }

    CsvFileParser<double> orthologFile(orthologFilename);
    CsvFileParser<double> microbeFile(microbeFilename);
    orthologFile.convertLog();
    //}}}

//estimation{{{
    GibbsSamplerFromGMOM *estimator;
    bmpi::environment env(argc, argv);
    bmpi::communicator world;
    estimator = new GibbsSamplerFromGMOM(orthologFile, microbeFile, A, k, theta, iterationNumber, burnIn, samplingInterval, world);
    estimator->runIteraions();
    if(world.rank() == 0){
        estimator->writeParameters(PFilename, VFilename);
        estimator->writeLogLikelihood(logLikelihoodFilename);
    }
    delete estimator;
//}}}
    return 0;
}


/** This is free and unencumbered software released into the public domain.
The authors of ISIS do not claim copyright on the contents of this file.
For more details about the LICENSE terms and the AUTHORS, you will
find files of those names at the top level of this repository. **/

/* SPDX-License-Identifier: CC0-1.0 */
#include "jama/jama_svd.h"
#include "jama/jama_qr.h"

#include <armadillo>

#include "LeastSquares.h"
#include "IException.h"
#include "IString.h"

namespace Isis {
  /**
   * Creates a LeastSquares Object.
   *
   * @param basis A BasisFunction. This parameter allows for the least squares
   *              fitting to be applied to arbitrary equations.
   */
  LeastSquares::LeastSquares(Isis::BasisFunction &basis, bool sparse,
                             int sparseRows, int sparseCols, bool jigsaw) {
    p_jigsaw = jigsaw;
    p_basis = &basis;
    p_solved = false;
    p_sparse = sparse;
    p_sigma0 = 0.;

    p_sparseRows = sparseRows;
    p_sparseCols = sparseCols;

    if (p_sparse) {

      //  make sure sparse nrows/ncols have been set
      if (sparseRows == 0  ||  sparseCols == 0) {
        QString msg = "If solving using sparse matrices, you must enter the "
                      "number of rows/columns";
        throw IException(IException::Programmer, msg, _FILEINFO_);
      }

      p_sparseA.set_size(sparseRows, sparseCols);
      p_normals.set_size(sparseCols, sparseCols);
      p_ATb.resize(sparseCols, 1);
      p_xSparse.resize(sparseCols);

      if( p_jigsaw ) {
        p_epsilonsSparse.resize(sparseCols);
        std::fill_n(p_epsilonsSparse.begin(), sparseCols, 0.0);

        p_parameterWeights.resize(sparseCols);
      }

    }
    p_currentFillRow = -1;
  }

  //! Destroys the LeastSquares object.
  LeastSquares::~LeastSquares() {
  }

  /**
   * Invoke this method for each set of knowns. Given our example in the
   * description, we have three knowns and expecteds. They are
   * @f[
   * (1,1) = 3
   * @f]
   * @f[
   * (-2,3) = 1
   * @f]
   * @f[
   * (2,-1) = 2
   * @f]
   *
   * @param data A vector of knowns.
   *
   * @param result The expected value for the knowns.
   *
   * @param weight (Default = 1.0) How strongly to weight this known. Weight less
   *               than 1 increases residual for this known, while weight greater
   *               than 1 decreases the residual for this known.
   *
   * @throws Isis::IException::Programmer - Number of elements in data does not
   *                                        match basis requirements
   *
   * @internal
   * @history 2008-04-22  Tracie Sucharski,  Fill sparse matrix.
   * @history 2009-12-21  Jeannie Walldren,  Modified code to add
   *          the square root of the weight to the vector
   *          p_sqrtweight.
   *
   */
  void LeastSquares::AddKnown(const std::vector<double> &data, double result,
                              double weight) {
    if((int) data.size() != p_basis->Variables()) {
      QString msg = "Number of elements in data does not match basis [" +
                        p_basis->Name() + "] requirements";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }

    p_expected.push_back(result);

    if (weight == 1) {
      p_sqrtWeight.push_back(weight);
    }
    else {
      p_sqrtWeight.push_back(sqrt(weight));
    }

    if(p_sparse) {
      FillSparseA(data);
    }
    else {
      p_input.push_back(data);
    }
  }

  /**
   *  Invoke this method for each set of knowns for sparse solutions.  The A
   *  sparse matrix must be filled as we go or we will quickly run out of memory
   *  for large solutions. So, expand the basis function, apply weights (which is
   *  done in the Solve method for the non-sparse case.
   *
   * @param data A vector of knowns.
   *
   * @internal
   * @history 2008-04-22  Tracie Sucharski - New method for sparse solutions.
   * @history 2009-12-21  Jeannie Walldren - Changed variable name
   *          from p_weight to p_sqrtweight.
   *
   */
  void LeastSquares::FillSparseA(const std::vector<double> &data) {

    p_basis->Expand(data);

    p_currentFillRow++;

    int ncolumns = (int)data.size();

    for(int c = 0;  c < ncolumns; c++) {
      p_sparseA(p_currentFillRow, c) = p_basis->Term(c) * p_sqrtWeight[p_currentFillRow];
    }
  }

  /**
   * This method returns the data at the given row.
   *
   * @param row
   *
   * @return std::vector<double>
   */
  std::vector<double> LeastSquares::GetInput(int row) const {
    if((row >= Rows()) || (row < 0)) {
      QString msg = "Index out of bounds [Given = " + toString(row) + "]";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
    return p_input[row];
  }

  /**
   * This method returns the expected value at the given row.
   *
   * @param row
   *
   * @return double
   */
  double LeastSquares::GetExpected(int row) const {
    if((row >= Rows()) || (row < 0)) {
      QString msg = "Index out of bounds [Given = " + toString(row) + "]";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
    return p_expected[row];
  }

  /**
   * This methods returns the number of rows in the matrix.
   *
   *
   * @return int
   */
  int LeastSquares::Rows() const {
    return (int)p_input.size();
  }

  /**
   * After all the data has been registered through AddKnown, invoke this
   * method to solve the system of equations. You can then use the Evaluate
   * and Residual methods freely.
   *
   * @internal
   * @history  2008-04-16 Debbie Cook / Tracie Sucharski, Added SolveSparse.
   * @history  2009-04-08 Tracie Sucharski - Added return value which will
   *                          pass on what is returned from SolveSparse which
   *                          is a column number of a column that contained
   *                          all zeros.
   * @history  2010-12-12 Debbie A. Cook  Fixed "no data" test for SPARSE
   *                          case
   */
  int LeastSquares::Solve(Isis::LeastSquares::SolveMethod method) {

    if((method == SPARSE  &&  p_sparseRows == 0)  ||
       (method != SPARSE  &&  Rows() == 0 )) {
      p_solved = false;
      QString msg = "No solution available because no input data was provided";
      throw IException(IException::Unknown, msg, _FILEINFO_);
    }

    if(method == SVD) {
      SolveSVD();
    }
    else if(method == QRD) {
      SolveQRD();
    }
    else if(method == SPARSE) {
      int column = SolveSparse();
      return column;
    }
    return 0;
  }

  /**
   * After all the data has been registered through AddKnown, invoke this
   * method to solve the system of equations. You can then use the Evaluate
   * and Residual methods freely.
   * @internal
   * @history 2009-12-21  Jeannie Walldren - Changed variable name
   *          from p_weight to p_sqrtweight.
   *
   */
  void LeastSquares::SolveSVD() {

    // We are solving Ax=b ... start by creating A
    TNT::Array2D<double> A(p_input.size(), p_basis->Coefficients());
    for(int r = 0; r < A.dim1(); r++) {
      p_basis->Expand(p_input[r]);
      for(int c = 0; c < A.dim2(); c++) {
        A[r][c] = p_basis->Term(c) * p_sqrtWeight[r];
      }
    }

    // Ok use singular value decomposition to solve for the coefficients
    // A = [U][S][V']  where [U] is MxN, [S] is NxN, [V'] is NxN transpose
    // of [V].  We are solving for [A]x=b and need inverse of [A] such
    // that x = [invA]b. Since inverse may not exist we use the
    // pseudo-inverse [A+] from SVD which is [A+] = [V][invS][U']
    // Our coefficents are then x = [A+]b where b is p_b.
    JAMA::SVD<double> svd(A);

    TNT::Array2D<double> V;
    svd.getV(V);

    // The inverse of S is the 1 over each diagonal element of S
    TNT::Array2D<double> invS;
    svd.getS(invS);

    for(int i = 0; i < invS.dim1(); i++) {
      if(invS[i][i] != 0.0) invS[i][i] = 1.0 / invS[i][i];
    }

    // Transpose U
    TNT::Array2D<double> U;
    svd.getU(U);
    TNT::Array2D<double> transU(U.dim2(), U.dim1());

    for(int r = 0; r < U.dim1(); r++) {
      for(int c = 0; c < U.dim2(); c++) {
        transU[c][r] = U[r][c];
      }
    }

    // Now multiply everything together to get [A+]
    TNT::Array2D<double> VinvS = TNT::matmult(V, invS);
    TNT::Array2D<double> Aplus = TNT::matmult(VinvS, transU);

    // Using Aplus and our b we can solve for the coefficients
    TNT::Array2D<double> b(p_expected.size(), 1);

    for(int r = 0; r < (int)p_expected.size(); r++) {
      b[r][0] = p_expected[r] * p_sqrtWeight[r];
    }

    TNT::Array2D<double> coefs = TNT::matmult(Aplus, b);

    // If the rank of the matrix is not large enough we don't
    // have enough coefficients for the solution
    if (coefs.dim1() < p_basis->Coefficients()) {
      QString msg = "Unable to solve least-squares using SVD method. No "
                    "solution available. Not enough knowns or knowns are "
                    "co-linear ... [Unknowns = "
                    + toString(p_basis->Coefficients()) + "] [Knowns = "
                    + toString(coefs.dim1()) + "]";
      throw IException(IException::Unknown, msg, _FILEINFO_);
    }

    // Set the coefficients in our basis equation
    std::vector<double> bcoefs;
    for (int i = 0; i < coefs.dim1(); i++) bcoefs.push_back(coefs[i][0]);

    p_basis->SetCoefficients(bcoefs);

    // Compute the errors
    for(int i = 0; i < (int)p_input.size(); i++) {
      double value = p_basis->Evaluate(p_input[i]);
      p_residuals.push_back(value - p_expected[i]);
      p_sigma0 += p_residuals[i]*p_residuals[i]*p_sqrtWeight[i]*p_sqrtWeight[i];
    }
    // calculate degrees of freedom (or redundancy)
    // DOF = # observations + # constrained parameters - # unknown parameters
    p_degreesOfFreedom = p_basis->Coefficients() - coefs.dim1();

    if( p_degreesOfFreedom > 0.0 )  {
      p_sigma0 = p_sigma0/(double)p_degreesOfFreedom;
    }

    // check for p_sigma0 < 0
    p_sigma0 = sqrt(p_sigma0);

    // All done
    p_solved = true;
  }

  /**
   * After all the data has been registered through AddKnown, invoke this
   * method to solve the system of equations with a QR
   * decomposition of A = QR. You can then use the Evaluate and
   * Residual methods freely. The QR decomposition is only slightly
   * less reliable than the SVD, but much faster.
   * @internal
   * @history 2009-12-21  Jeannie Walldren - Changed variable name
   *          from p_weight to p_sqrtweight.
   *
   */
  void LeastSquares::SolveQRD() {

    // We are solving Ax=b ... start by creating an MxN matrix, A
    TNT::Array2D<double> A(p_input.size(), p_basis->Coefficients());
    for(int r = 0; r < A.dim1(); r++) {
      p_basis->Expand(p_input[r]);
      for(int c = 0; c < A.dim2(); c++) {
        A[r][c] = p_basis->Term(c) * p_sqrtWeight[r];
      }
    }

    // Ok use  to solve for the coefficients
    // [A] = [Q][R]  where [Q] is MxN and orthogonal and  [R] is an NxN,
    // upper triangular matrix.  TNT provides the solve method that inverts
    // [Q] and backsolves [R] to get the coefficients in the vector x.
    // That is, we solve the system Rx = Q^T b
    JAMA::QR<double> qr(A);

    // Using A and our b we can solve for the coefficients
    TNT::Array1D<double> b(p_expected.size());
    for(int r = 0; r < (int)p_expected.size(); r++) {
      b[r] = p_expected[r] * p_sqrtWeight[r];
    }// by construction, we know the size of b is equal to M, so b is conformant

    // Check to make sure the matrix is full rank before solving
    // -- rectangular matrices must be full rank in order for the solve method
    //    to be successful
    int full = qr.isFullRank();
    if(full == 0) {
      QString msg = "Unable to solve-least squares using QR Decomposition. "
                    "The upper triangular R matrix is not full rank";
      throw IException(IException::Unknown, msg, _FILEINFO_);
    }

    TNT::Array1D<double> coefs = qr.solve(b);

    // Set the coefficients in our basis equation
    std::vector<double> bcoefs;
    for(int i = 0; i < coefs.dim1(); i++) {
      bcoefs.push_back(coefs[i]);
    }
    p_basis->SetCoefficients(bcoefs);

    // Compute the errors
    for(int i = 0; i < (int)p_input.size(); i++) {
      double value = p_basis->Evaluate(p_input[i]);
      p_residuals.push_back(value - p_expected[i]);
    }

    // All done
    p_solved = true;
  }

  /**
   * @brief  Solve using sparse class
   *
   * After all the data has been registered through AddKnown, invoke this
   * method to solve the system of equations Nx = b, where
   *
   * N = ATPA
   * b = ATPl, and
   *
   * N is the "normal equations matrix;
   * A is the so-called "design" matrix;
   * P is the observation weight matrix (typically diagonal);
   * l is the "computed - observed" column vector;
   *
   * The solution is achieved using a sparse matrix formulation of
   * the LU decomposition of the normal equations.
   *
   * You can then use the Evaluate and Residual methods freely.
   *
   * @internal
   * @history  2008-04-16 Debbie Cook / Tracie Sucharski, New method
   * @history  2008-04-23 Tracie Sucharski,  Fill sparse matrix as we go in
   *                          AddKnown method rather than in the solve method,
   *                          otherwise we run out of memory very quickly.
   * @history  2009-04-08 Tracie Sucharski - Added return value which is a
   *                          column number of a column that contained all zeros.
   * @history 2009-12-21  Jeannie Walldren - Changed variable name
   *                          from p_weight to p_sqrtweight.
   * @history 2010        Ken Edmundson
   * @history 2010-11-20  Debbie A. Cook Merged Ken Edmundson verion with system version
   * @history 2011-03-17  Ken Edmundson Corrected computation of residuals
   *
   */
  int LeastSquares::SolveSparse() {

    // form "normal equations" matrix by multiplying ATA
    p_normals = p_sparseA.t()*p_sparseA;

    // Create the right-hand-side column vector 'b'
    arma::mat b(p_sparseRows, 1);

    // multiply each element of 'b' by it's associated weight
    for ( int r = 0; r < p_sparseRows; r++ )
      b(r,0) = p_expected[r] * p_sqrtWeight[r];

    // form ATb
    p_ATb = p_sparseA.t()*b;

    // apply parameter weighting if Jigsaw (bundle adjustment)
    if ( p_jigsaw ) {
      for( int i = 0; i < p_sparseCols; i++) {
        double weight = p_parameterWeights[i];

        if( weight <= 0.0 )
          continue;

        p_normals(i, i) += weight;
        p_ATb(i, 0) -= p_epsilonsSparse[i]*weight;
      }
    }

    bool status = spsolve(p_xSparse, p_normals, p_ATb, "superlu");

    if (status == false) {
      QString msg = "Could not solve sparse least squares problem.";
      throw IException(IException::Unknown, msg, _FILEINFO_);
    }

    // Set the coefficients in our basis equation
    p_basis->SetCoefficients(arma::conv_to< std::vector<double> >::from(p_xSparse));

    // if Jigsaw (bundle adjustment)
    // add corrections into epsilon vector (keeping track of total corrections)
    if ( p_jigsaw ) {
      for( int i = 0; i < p_sparseCols; i++ )
        p_epsilonsSparse[i] += p_xSparse[i];
    }

    // Compute the image coordinate residuals and sum into Sigma0
    // (note this is exactly what was being done before, but with less overhead - I think)
    // ultimately, we should not be using the A matrix but forming the normals
    // directly. Then we'll have to compute the residuals by back projection

    p_residuals.resize(p_sparseRows);
    p_residuals = arma::conv_to< std::vector<double> >::from(p_sparseA*p_xSparse);
    p_sigma0 = 0.0;

    for ( int i = 0; i < p_sparseRows; i++ ) {
        p_residuals[i] = p_residuals[i]/p_sqrtWeight[i];
        p_residuals[i] -= p_expected[i];
        p_sigma0 += p_residuals[i]*p_residuals[i]*p_sqrtWeight[i]*p_sqrtWeight[i];
    }

    // if Jigsaw (bundle adjustment)
    // add contibution to Sigma0 from constrained parameters
    if ( p_jigsaw ) {
      double constrained_vTPv = 0.0;

      for ( int i = 0; i < p_sparseCols; i++ ) {
        double weight = p_parameterWeights[i];

        if ( weight <= 0.0 )
          continue;

        constrained_vTPv += p_epsilonsSparse[i]*p_epsilonsSparse[i]*weight;
      }
      p_sigma0 += constrained_vTPv;
    }
    // calculate degrees of freedom (or redundancy)
    // DOF = # observations + # constrained parameters - # unknown parameters
    p_degreesOfFreedom = p_sparseRows + p_constrainedParameters - p_sparseCols;

    if( p_degreesOfFreedom <= 0.0 ) {
      p_sigma0 = 1.0;
    }
    else {
      p_sigma0 = p_sigma0/(double)p_degreesOfFreedom;
    }

    // check for p_sigma0 < 0
    p_sigma0 = sqrt(p_sigma0);

    // All done
    p_solved = true;
    return 0;
  }

  void LeastSquares::Reset ()
  {
    if ( p_sparse ) {
      p_sparseA.zeros();
      p_ATb.zeros();
      p_normals.zeros();
      p_currentFillRow = -1;
    }
    else {
      p_input.clear();
    }
      p_sigma0 = 0.;
    p_residuals.clear();
    p_expected.clear();
    p_sqrtWeight.clear();
    p_solved = false;
  }

  /**
   * Invokes the BasisFunction Evaluate method.
   *
   * @param data The input variables to evaluate.
   *
   * @return The evaluation for the input variable.
   *
   * @throws Isis::IException::Programmer - Unable to evaluate until a
   *                                        solution has been computed
   */
  double LeastSquares::Evaluate(const std::vector<double> &data) {
    if(!p_solved) {
      QString msg = "Unable to evaluate until a solution has been computed";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
    return p_basis->Evaluate(data);
  }

  /**
   * Returns a vector of residuals (errors). That is, the difference between the
   * evaluation of a known with the solution against the expected value.
   *
   * @return The vector of residuals.
   *
   * @throws Isis::IException::Programmer - Unable to return residuals until a
   *                                        solution has been computed
   */
  std::vector<double> LeastSquares::Residuals() const {
    if(!p_solved) {
      QString msg = "Unable to return residuals until a solution has been computed";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
    return p_residuals;
  }

  /**
   * Returns the ith residual. That is, the difference between the evaluation of
   * a known with the solution against the expected value. There is one residual
   * for each time AddKnown was invoked.
   *
   * @param i The number of times AddKnown was invoked to be evaluated.
   *
   * @return The output value of the residual.
   *
   * @throws Isis::IException::Programmer - Unable to return residuals until a
   *                                        solution has been computed
   */
  double LeastSquares::Residual(int i) const {
    if(!p_solved) {
      QString msg = "Unable to return residuals until a solution has been computed";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
    return p_residuals[i];
  }

  /**
   * Reset the weight for the ith known. This weight will not be used unless
   * the system is resolved using the Solve method.
   *
   * @param index The position in the array to assign the given weight value
   * @param weight A weight factor to apply to the ith known. A weight less
   *               than one increase the residual for this known while a
   *               weight greater than one decrease the residual for this
   *               known.
   * @internal
   * @history 2009-12-21  Jeannie Walldren,  Modified code to add
   *          the square root of the weight to the vector
   *          p_sqrtweight.
   *
   */

  void LeastSquares::Weight(int index, double weight) {
    if(weight == 1) {
      p_sqrtWeight[index] = weight;
    }
    else {
      p_sqrtWeight[index] = sqrt(weight);
    }
  }

} // end namespace isis


//=======================================================================
// Copyright 2013 University of Warsaw.
// Authors: Piotr Wygocki
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================

#ifndef BOOST_GRAPH_AUGMENT_HPP
#define BOOST_GRAPH_AUGMENT_HPP

#include <boost/graph/filtered_graph.hpp>

namespace boost
{
namespace detail
{

    template < class Graph, class ResCapMap >
    filtered_graph< const Graph, is_residual_edge< ResCapMap > > residual_graph(
        const Graph& g, ResCapMap residual_capacity)
    {
        return filtered_graph< const Graph, is_residual_edge< ResCapMap > >(
            g, is_residual_edge< ResCapMap >(residual_capacity));
    }

    template < class Graph, class PredEdgeMap, class ResCapMap,
        class RevEdgeMap >
    inline void augment(const Graph& g,
        typename graph_traits< Graph >::vertex_descriptor src,
        typename graph_traits< Graph >::vertex_descriptor sink, PredEdgeMap p,
        ResCapMap residual_capacity, RevEdgeMap reverse_edge)
    {
        typename graph_traits< Graph >::edge_descriptor e;
        typename graph_traits< Graph >::vertex_descriptor u;
        typedef typename property_traits< ResCapMap >::value_type FlowValue;

        // find minimum residual capacity along the augmenting path
        FlowValue delta = (std::numeric_limits< FlowValue >::max)();
        e = get(p, sink);
        do
        {
            BOOST_USING_STD_MIN();
            delta = min BOOST_PREVENT_MACRO_SUBSTITUTION(
                delta, get(residual_capacity, e));
            u = source(e, g);
            e = get(p, u);
        } while (u != src);

        // push delta units of flow along the augmenting path
        e = get(p, sink);
        do
        {
            put(residual_capacity, e, get(residual_capacity, e) - delta);
            put(residual_capacity, get(reverse_edge, e),
                get(residual_capacity, get(reverse_edge, e)) + delta);
            u = source(e, g);
            e = get(p, u);
        } while (u != src);
    }

} // namespace detail
} // namespace boost

#endif /* BOOST_GRAPH_AUGMENT_HPP */


#include <iostream>
#include <fstream>
#include <ctime>
#include <boost/random/mersenne_twister.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/tokenizer.hpp>

#include <boost/random/uniform_int_distribution.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <sys/time.h>

#include <vector>
#include <string>
#include <algorithm>
#include <numeric>
#include <queue>

#include <math.h>
#include <matrix.h>
#include <mex.h>

boost::random::mt19937 gen;

using namespace std;

int collisions (int &dimx_xp, deque<int> &xist_gene, deque<int> &tsix_gene,
        int &sum_xp, int &sum_tp, boost::random::uniform_int_distribution<> &dist)
{
    int q;
    for (q=0; q<dimx_xp; q++)
            {
            if ((xist_gene[q] + tsix_gene[q])>1) {
                if (dist(gen)==0) {
                    xist_gene[q]=0;
                    sum_xp--;
                } else {
                    tsix_gene[q]=0;
                    sum_tp--;
                }
            }
        }
}

int elongation (int &dimx_xp, deque<int> &xist_gene, deque<int> &tsix_gene, int &sum_xp, int &sum_tp,
        bool &xpol_ini, bool &tpol_ini, int &xist_rna, int &tsix_rna,
        vector<int> &all_xist, int &temp_sum_xr, int &temp_sum_tr, int &temp_sum_xp, int &temp_sum_tp,
        boost::random::uniform_int_distribution<> &dist, int &step, double &p8)
{
    xist_gene.push_front(xpol_ini);
    sum_xp = sum_xp - xist_gene.back() + xist_gene.front();
    xist_rna = xist_rna + (int)xist_gene.back();
    xist_gene.pop_back();
    if (p8>0) {
        collisions (dimx_xp, xist_gene, tsix_gene, sum_xp, sum_tp, dist);
    }
    tsix_gene.push_back(tpol_ini);
    sum_tp = sum_tp - tsix_gene.front() + tsix_gene.back();
    tsix_rna = tsix_rna + (int)tsix_gene.front();
    tsix_gene.pop_front();
    if (p8>0) {
        collisions (dimx_xp, xist_gene, tsix_gene, sum_xp, sum_tp, dist);
    }
    all_xist[step] = xist_rna;

    temp_sum_xr = temp_sum_xr + xist_rna;
    temp_sum_tr = temp_sum_tr + tsix_rna;
    temp_sum_xp = temp_sum_xp + sum_xp;
    temp_sum_tp = temp_sum_tp + sum_tp;
}

void mexFunction(int nhls, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{

    //initiate random number generator
    struct timeval t1;
    gettimeofday(&t1, NULL);
    gen.seed(static_cast<unsigned int>(t1.tv_usec * t1.tv_sec));

	//declare variables // pointers that point at input and output variables
	mxArray *xist_pol, *tsix_pol, *xist_pol2, *tsix_pol2, *par, *const_par;
    mxArray *time_out, *xist_pol_out, *tsix_pol_out, *xist_rna_out, *tsix_rna_out,
            *xist_pol_out2, *tsix_pol_out2, *xist_rna_out2, *tsix_rna_out2, *test, *stable_switch_on_out;
	const mwSize *dims;
	double *xp, *tp, *xr, *tr, *xpr, *xp2, *tp2, *xr2, *tr2, *xpr2, *p, *cp,
            t, sil_threshold, t_diff, t_before;
    double *xpo, *tpo, *xro, *tro, *xpro, *xpo2, *tpo2, *xro2, *tro2, *xpro2,
            *time, v = 1.0/1440.0, out_step, *to, *stable_sw_on,
            k_adv_sil, k2_red_X1, k2_red_X2, k1_ind_X1, k1_ind_X2, tp_dox;
	int dimx_xp, dimx_tp, xist_rna, tsix_rna, xist_prom, xist_rna2, tsix_rna2, xist_prom2;
	int n_steps,n_steps_before, n_steps_dox, n_out_steps, n_small_steps, sec_chr, step, sel_rx;

	//associate inputs: first chromosome
    par = mxDuplicateArray(prhs[0]);
    const_par = mxDuplicateArray(prhs[1]);
	xist_pol = mxDuplicateArray(prhs[2]);
    tsix_pol = mxDuplicateArray(prhs[3]);
    xist_rna = mxGetScalar(prhs[4]);
    tsix_rna = mxGetScalar(prhs[5]);
    xist_prom = mxGetScalar(prhs[6]);

	//associate pointers
    xp = mxGetPr(xist_pol);
    tp = mxGetPr(tsix_pol);
    p = mxGetPr(par);
    cp = mxGetPr(const_par);
    t = cp[0];
    t_before =  cp[1];
    t_diff =  cp[2];
    sil_threshold = cp[3];
    out_step = cp[4];
    sec_chr = cp[5];
    k_adv_sil = cp[6];
    k2_red_X1 = cp[7];
    k2_red_X2 = cp[8];
    k1_ind_X1 = cp[9];
    k1_ind_X2 = cp[10];
    tp_dox = cp[11];

    //associate inputs: second chromosome
   if (sec_chr==1) {
        xist_pol2 = mxDuplicateArray(prhs[7]);
        tsix_pol2 = mxDuplicateArray(prhs[8]);
        xist_rna2 = mxGetScalar(prhs[9]);
        tsix_rna2 = mxGetScalar(prhs[10]);
        xist_prom2 = mxGetScalar(prhs[11]);

        xp2 = mxGetPr(xist_pol2);
        tp2 = mxGetPr(tsix_pol2);
   }

    n_out_steps = 1+ceil((t_diff+t_before)/out_step);
    n_small_steps = round(out_step/v);
    n_steps = (n_out_steps-1)*n_small_steps;
    n_steps_before = (ceil(t_before/out_step))*n_small_steps;
    n_steps_dox = (ceil(tp_dox/out_step))*n_small_steps;

	//figure out dimensions and associate output
	dims = mxGetDimensions(prhs[2]);
	dimx_xp = (int)dims[0];
    dims = mxGetDimensions(prhs[3]);
	dimx_tp = (int)dims[0];

    //output
    time_out = plhs[0] = mxCreateDoubleMatrix(n_out_steps, 1, mxREAL);
    xist_pol_out = plhs[1] = mxCreateDoubleMatrix(n_out_steps, 1, mxREAL);
    tsix_pol_out = plhs[2] = mxCreateDoubleMatrix(n_out_steps, 1, mxREAL);
    xist_rna_out = plhs[3] = mxCreateDoubleMatrix(n_out_steps, 1, mxREAL);
    tsix_rna_out = plhs[4] = mxCreateDoubleMatrix(n_out_steps, 1, mxREAL);
    xpo = mxGetPr(xist_pol_out);
    tpo= mxGetPr(tsix_pol_out);
    xro = mxGetPr(xist_rna_out);
    tro = mxGetPr(tsix_rna_out);
    time =   mxGetPr(time_out);

    xist_pol_out2 = plhs[5] = mxCreateDoubleMatrix(n_out_steps, 1, mxREAL);
    tsix_pol_out2 = plhs[6] = mxCreateDoubleMatrix(n_out_steps, 1, mxREAL);
    xist_rna_out2 = plhs[7] = mxCreateDoubleMatrix(n_out_steps, 1, mxREAL);
    tsix_rna_out2 = plhs[8] = mxCreateDoubleMatrix(n_out_steps, 1, mxREAL);
    test = plhs[9] = mxCreateDoubleMatrix(n_out_steps, 1, mxREAL);
    stable_switch_on_out = plhs[10] = mxCreateDoubleMatrix(1, 1, mxREAL);

    xpo2 = mxGetPr(xist_pol_out2);
    tpo2= mxGetPr(tsix_pol_out2);
    xro2 = mxGetPr(xist_rna_out2);
    tro2 = mxGetPr(tsix_rna_out2);
    to = mxGetPr(test);
    stable_sw_on = mxGetPr(stable_switch_on_out);
    // Initialize
    stable_sw_on[0] = t_diff+t_before;

    //write pol mxarray in deque and sum up polymerases
    deque<int> xist_gene, tsix_gene, xist_gene2, tsix_gene2;
    int q, sum_xp=0, sum_tp=0, sum_xp2 = 0, sum_tp2 = 0;

    for (q=0; q<dimx_xp; q++)
        {
            xist_gene.push_back (xp[q]);
            sum_xp = sum_xp + xp[q];
             if (sec_chr==1) {
                xist_gene2.push_back (xp2[q]);
                sum_xp2 = sum_xp2 + xp2[q];
             }
        }
    for (q=0; q<dimx_tp; q++)
        {
            tsix_gene.push_back (tp[q]);
            sum_tp = sum_tp + tp[q];
            if (sec_chr==1) {
                tsix_gene2.push_back (tp2[q]);
                 sum_tp2 = sum_tp2 + tp2[q];
            }
        }

    bool xpol_ini=0, tpol_ini=0, xpol_ini2=0, tpol_ini2=0;
    boost::random::uniform_int_distribution<> dist(0, 1);
    boost::random::uniform_real_distribution<> dist_real(0.0, 1.0);
    double t_next, r, sum_rx;
    vector<double> rx(16), cum_rx(16);
    vector<int> all_xist(n_steps), all_xist2(n_steps);

    int sil_xa = 0, sil_xa2 = 0, xa_sil_steps = round(p[11]/v),
            sil_tsix = 0, sil_tsix2 = 0,
	    sil_state_xa = 0, sil_state_xa2 = 0, sil_state_tsix = 0, sil_state_tsix2 = 0, stst = 0;
    double xa = 1, xa2 = 0;
    if (sec_chr==1) {xa2 = 1;};
    double temp_sum_xa = xa + xa2;
    int m = 0, b = 0, temp_sum_xr = 0, temp_sum_tr = 0, temp_sum_xp = 0, temp_sum_tp = 0,
            temp_sum_xr2 = 0, temp_sum_tr2 = 0, temp_sum_xp2 = 0, temp_sum_tp2 = 0;
    all_xist[0] = xist_rna;
    xro[b] = xist_rna;
    tro[b] = tsix_rna;
    xpo[b] = sum_xp;
    tpo[b] = sum_tp;
    to[b] = temp_sum_xa;
    if (sec_chr==1) {
        all_xist2[0] = xist_rna2;
        xro2[b] = xist_rna2;
        tro2[b] = tsix_rna2;
        xpo2[b] = sum_xp2;
        tpo2[b] = sum_tp2;
    }
    //while (act_time<t_max)
    for (step=0; step<n_steps; step++)
        {
			// Identify first timepoint at which stable state with 1 Xa and 1 Xi is reached
			if (xist_rna>=sil_threshold & xa ==0 & sil_tsix ==1 & xist_rna2<sil_threshold & xa2==1 & sil_tsix2==0 & stst==0) {stable_sw_on[0] = step*v; stst=1;}
			else { if (xist_rna2>=sil_threshold & xa2 ==0 & sil_tsix2 ==1 & xist_rna<sil_threshold & xa==1 & sil_tsix==0 & stst==0) {stable_sw_on[0] = step*v; stst=1;}
			}
			//Silencing
			//If Tsix silencing has reached final state, Tsix is silenced //p[12]=parameter that determines # of intermediate states until silencing of tsix occurs
			if (sil_state_tsix>=p[12]&xist_rna>=sil_threshold) {sil_tsix=1;}
			//If silencing has not reached the final state but Xist rna level is below threshold system falls back into initial tsix silencing state
			else {
			if (xist_rna<sil_threshold&sil_state_tsix<p[12]) {sil_state_tsix=0; sil_tsix=0;}
			}
			//If XA silencing has reached final state, XA is silenced //p[11]=parameter that determines # of intermediate states until silencing of xa occurs
			if (sil_state_xa>=p[11]&xist_rna>=sil_threshold) {xa=0;}
			//If silencing has not reached the final state but Xist rna level is below threshold system falls back into initial xa silencing state
			else {
			if (xist_rna<sil_threshold&sil_state_xa<p[11]) {sil_state_xa=0; xa=1;}
			}
			if (p[6]==0&tsix_gene.front()==1)
		        	{
		            	xist_prom = 2;
				}
		        if (sec_chr==1) {
		     	if (sil_state_tsix2>=p[12]&xist_rna2>=sil_threshold) {sil_tsix2=1;}
			else {
			if (xist_rna2<sil_threshold&sil_state_tsix2<p[12]) {sil_state_tsix2=0; sil_tsix2=0;}
			}
			if (sil_state_xa2>=p[11]&xist_rna2>=sil_threshold) {xa2=0;}
			else {
			if (xist_rna2<sil_threshold&sil_state_xa2<p[11]) {sil_state_xa2=0; xa2=1;}
			}
			if (p[6]==0&tsix_gene2.front()==1)
				{
		                	xist_prom2 = 2;
				}
		        }

			//Gillespie reactions
	        t_next = 0;

	        xpol_ini=0, tpol_ini=0, xpol_ini2=0, tpol_ini2=0;
	        while (t_next<v) {
				//Xist initiation
	            //before induction of diff k1 10 times reduced
	            if (step<n_steps_before & step<n_steps_dox) {rx[0] = p[0]*0*(xa+xa2)*(xist_prom==1)*(tsix_gene.front()==0);}
	            else {
					if (step<n_steps_dox) {rx[0] = p[0]*(xa+xa2)*(xist_prom==1)*(tsix_gene.front()==0);}
					// if Xist is induced with Dox (k1_ind_X1>0) the Xist initiation rate becomes independent of the XA concentration
					else {
						if (k1_ind_X1==0) {rx[0] = (p[0]*(step>n_steps_before))*(xa+xa2)*(xist_prom==1)*(tsix_gene.front()==0);}
						// If X1 is induced with dox, the Xist initiation rate becomes independent of xa+xa2
						else {rx[0] = (p[0]*(step>n_steps_before)+p[0]*k1_ind_X1)*(xist_prom==1)*(tsix_gene.front()==0);}
					}
				}
	            rx[1] = (p[1]-k2_red_X1*p[1])*(sil_tsix<1); //Tsix initiation
	            rx[2] = p[7]*(xist_prom==2)*(tsix_gene.front()==0); // turning on the Xist promoter
	            rx[3] = p[3]*xist_rna; //Xist degradation
	            rx[4] = p[4]*tsix_rna; //Tsix degradation

	            //second chr
	            //Xist initiation
	            if (step<n_steps_before & step<n_steps_dox) {rx[5] = (sec_chr==1)*p[0]*0*(xa+xa2)*(xist_prom2==1)*(tsix_gene2.front()==0);}
	            else {
					if (step<n_steps_dox) {rx[5] = (sec_chr==1)*p[0]*(xa+xa2)*(xist_prom2==1)*(tsix_gene2.front()==0);}
					else {
						if (k1_ind_X2==0) {rx[5] = (sec_chr==1)*(p[0]*(step>n_steps_before))*(xa+xa2)*(xist_prom2==1)*(tsix_gene2.front()==0);}
						// If X2 is induced with dox, the Xist initiation rate becomes independent of xa+xa2
						else {rx[5] = (sec_chr==1)*(p[0]*(step>n_steps_before)+p[0]*k1_ind_X2)*(xist_prom2==1)*(tsix_gene2.front()==0);}
					}
				}
	            rx[6] = (sec_chr==1)*(p[1]-k2_red_X2*p[1])*(sil_tsix2<1);   //Tsix initiation
	            rx[7] = (sec_chr==1)*(p[7]*(xist_prom2==2))*(tsix_gene2.front()==0); // turning on the Xist promoter
	            rx[8] = (sec_chr==1)*p[3]*xist_rna2; //Xist degradation
	            rx[9] = (sec_chr==1)*p[4]*tsix_rna2; //Tsix degradation

		    //transition between silencing states as Gillespie reaction
		    ////chromosome 1: now k_adv_sil= rate for transition between silencing states
		    rx[10] = k_adv_sil*(xist_rna>=sil_threshold)*(sil_state_xa<p[11]|sil_state_tsix<p[12]); //XA and Tsix silencing advance one state
		    ////if system is in last silencing state of the xa but xist rna fall below the threshold the silencing state can go back to the initial state with rate k[13]
		    rx[11] = p[13]*(xist_rna<sil_threshold)*(sil_state_xa>=p[11]);
		    //if system is in last silencing state of tsix but xist rna fall below the threshold the silencing state can go back to the initial state with rate k[14]
		    rx[12] = p[14]*(xist_rna<sil_threshold)*(sil_state_tsix>=p[12]);
		    ////second chromosome
		    rx[13] = (sec_chr==1)*k_adv_sil*(xist_rna2>=sil_threshold)*(sil_state_xa2<p[11]|sil_state_tsix2<p[12]);
		    rx[14] = (sec_chr==1)*p[13]*(xist_rna2<sil_threshold)*(sil_state_xa2>=p[11]);
		    rx[15] = (sec_chr==1)*p[14]*(xist_rna2<sil_threshold)*(sil_state_tsix2>=p[12]);

            sum_rx = 0, sel_rx = 100;
            for (q=0; q<rx.size(); q++) {
                sum_rx = sum_rx + rx[q];
                cum_rx[q] = sum_rx;
            }

            if (sum_rx>0) {
                t_next = t_next + (-log(dist_real(gen)))/(sum_rx);
            } else {t_next=1;}

            if (t_next<v){
                // decide which reaction occurs
                r = dist_real(gen);
                for (q=0; q<rx.size(); q++) {
                    if (r<(cum_rx[q]/sum_rx)){
                        sel_rx = q;
                        break;
                    }
                }
                //execute reaction

                switch (sel_rx){
                    case 0:
                        xpol_ini = 1; break;
                    case 1:
                        tpol_ini = 1; break;
                    case 2:
                        xist_prom = 1; break;
                    case 3:
                        xist_rna--;
                         break;
                    case 4:
                        tsix_rna--; break;

                    //second chr

                   case 5:
                        xpol_ini2 = 1; break;
                    case 6:
                        tpol_ini2 = 1; break;
                    case 7:
                        xist_prom2 = 1; break;
                    case 8:
                        xist_rna2--;
                        break;
                    case 9:
                        tsix_rna2--; break;
					case 10:
						if (sil_state_xa<p[11]) {sil_state_xa++;}
						if (sil_state_tsix<p[12]) {sil_state_tsix++;}
						break;
					case 11:
						sil_state_xa=0; xa=1; break;
					case 12:
						sil_state_tsix=0; sil_tsix=0; break;
					case 13:
						if (sil_state_xa2<p[11]) {sil_state_xa2++;}
						if (sil_state_tsix2<p[12]) {sil_state_tsix2++;}
						break;
					case 14:
						sil_state_xa2=0; xa2=1; break;
					case 15:
						sil_state_tsix2=0; sil_tsix2=0; break;
					default:
                        break;
                }
            }
        }
        temp_sum_xa = temp_sum_xa + xa + xa2;
        //elongation and collisions
        elongation (dimx_xp, xist_gene, tsix_gene, sum_xp, sum_tp,
            xpol_ini, tpol_ini, xist_rna, tsix_rna,
            all_xist, temp_sum_xr, temp_sum_tr, temp_sum_xp, temp_sum_tp, dist, step, p[8]);

        //second chromosome
        if (sec_chr==1) {
            elongation (dimx_xp, xist_gene2, tsix_gene2, sum_xp2, sum_tp2,
            xpol_ini2, tpol_ini2, xist_rna2, tsix_rna2,
            all_xist2, temp_sum_xr2, temp_sum_tr2, temp_sum_xp2, temp_sum_tp2, dist, step, p[8]);

        }

        //write out smoothed variable
        m++;
        if (m==n_small_steps) {
            b++;
            xro[b] = temp_sum_xr/n_small_steps;
            tro[b] = temp_sum_tr/n_small_steps;
            xpo[b] = temp_sum_xp/n_small_steps;
            tpo[b] = temp_sum_tp/n_small_steps;
            to[b] = temp_sum_xa/n_small_steps;
            time[b] = step*v;
            temp_sum_xr = 0;
            temp_sum_tr = 0;
            temp_sum_xp = 0;
            temp_sum_tp = 0;
            temp_sum_xa = 0;
            if (sec_chr==1) {
                xro2[b] = temp_sum_xr2/n_small_steps;
                tro2[b] = temp_sum_tr2/n_small_steps;
                xpo2[b] = temp_sum_xp2/n_small_steps;
                tpo2[b] = temp_sum_tp2/n_small_steps;
                temp_sum_xr2 = 0;
                temp_sum_tr2 = 0;
                temp_sum_xp2 = 0;
                temp_sum_tp2 = 0;
            }
            m = 0;
        }

        }
    if (m>0) {
            b++;
            xro[b] = temp_sum_xr/m;
            tro[b] = temp_sum_tr/m;
            xpo[b] = temp_sum_xp/m;
            tpo[b] = temp_sum_tp/m;
            to[b] = temp_sum_xa/n_small_steps;
            time[b] = step*v;
            if (sec_chr==1) {
                xro2[b] = temp_sum_xr2/m;
                tro2[b] = temp_sum_tr2/m;
                xpo2[b] = temp_sum_xp2/m;
                tpo2[b] = temp_sum_tp2/m;
            }
    }
   	}