/* ----------------------------------------------------------------------- *//**
 *
 * @file sparse_linear_systems.cpp
 *
 * @brief sparse linear systems
 *
 * We implement sparse linear systems using the direct.
 *
 *//* ----------------------------------------------------------------------- */
#include <limits>
#include <dbconnector/dbconnector.hpp>
#include <modules/shared/HandleTraits.hpp>
#include <modules/prob/boost.hpp>
#include "sparse_linear_systems.hpp"
#include <Eigen/Sparse>

// Common SQL functions used by all modules
//#include "sparse_linear_systems_states.hpp"

namespace madlib {

// Use Eigen
using namespace dbal::eigen_integration;
using namespace Eigen;

namespace modules {

// Import names from other MADlib modules
using dbal::NoSolutionFoundException;

namespace linear_systems{

// ---------------------------------------------------------------------------
//              Direct sparse Linear System States
// ---------------------------------------------------------------------------

// Function protofype of Internal functions
AnyType direct_sparse_stateToResult(
    const Allocator &inAllocator,
    const ColumnVector &x,
    const double residual);

/**
 * @brief States for linear systems
 *
 * TransitionState encapsualtes the transition state during the
 * logistic-regression aggregate function. To the database, the state is
 * exposed as a single DOUBLE PRECISION array, to the C++ code it is a proper
 * object containing scalars and vectors.
 *
 * Note: We assume that the DOUBLE PRECISION array is initialized by the
 * database with length at least 5, and all elemenets are 0.
 *
 */
template <class Handle>
class SparseDirectLinearSystemTransitionState {
    template <class OtherHandle>
    friend class SparseDirectLinearSystemTransitionState;

public:
    SparseDirectLinearSystemTransitionState(const AnyType &inArray)
        : mStorage(inArray.getAs<Handle>()) {

        rebind( static_cast<uint32_t>(mStorage[1]),
                static_cast<uint32_t>(mStorage[2]));
    }

    /**
     * @brief Convert to backend representation
     *
     * We define this function so that we can use State in the
     * argument list and as a return type.
     */
    inline operator AnyType() const {
        return mStorage;
    }

    /**
     * @brief Initialize the conjugate-gradient state.
     *
     * This function is only called for the first iteration, for the first row.
     */
    inline void initialize(const Allocator &inAllocator,
                          uint32_t innumVars,
                          uint32_t innumEquations,
                          uint32_t inNNZA
                          ) {
        // Array size does not depend in numVars
        mStorage = inAllocator.allocateArray<double, dbal::AggregateContext,
            dbal::DoZero, dbal::ThrowBadAlloc>(arraySize(innumEquations, inNNZA));
        rebind(innumEquations, inNNZA);
        numVars = innumVars;
        numEquations = innumEquations;
        NNZA = inNNZA;
    }

    /**
     * @brief We need to support assigning the previous state
     */
    template <class OtherHandle>
    SparseDirectLinearSystemTransitionState &operator=(
        const SparseDirectLinearSystemTransitionState<OtherHandle> &inOtherState) {

        for (size_t i = 0; i < mStorage.size(); i++)
            mStorage[i] = inOtherState.mStorage[i];
        return *this;
    }

    /**
     * @brief Merge with another State object by copying the intra-iteration
     *     fields
     */
    template <class OtherHandle>
    SparseDirectLinearSystemTransitionState &operator+=(
        const SparseDirectLinearSystemTransitionState<OtherHandle> &inOtherState) {

        if (mStorage.size() != inOtherState.mStorage.size() ||
            numVars != inOtherState.numVars ||
            NNZA != inOtherState.NNZA||
            numEquations != inOtherState.numEquations)
            throw std::logic_error("Internal error: Incompatible transition "
                "states");

        b += inOtherState.b;
        b_stored += inOtherState.b_stored;

        // Merge state for sparse loading is not an array add operation
        // but it is an array-append operation
        for (uint32_t i=0; i < inOtherState.nnz_processed; i++){
            r(nnz_processed + i) += inOtherState.r(i);
            c(nnz_processed + i) += inOtherState.c(i);
            v(nnz_processed + i) += inOtherState.v(i);
        }
        nnz_processed += inOtherState.nnz_processed;
        return *this;
    }

    /**
     * @brief Reset the inter-iteration fields.
     */
    inline void reset() {
        nnz_processed = 0;
        r.fill(0);
        c.fill(0);
        v.fill(0);
        b.fill(0);
        b_stored.fill(0);
    }

private:
    static inline size_t arraySize(const uint32_t innumEquations,
                                   const uint32_t inNNZA) {
        return 5 + 3 * inNNZA + 2 * innumEquations;
    }

    /**
     * @brief Rebind to a new storage array
     *
     * @param innumVars The number of independent variables.
     *
     * Array layout (iteration refers to one aggregate-function call):
     * Inter-iteration components (updated in final function):
     * - 0: numVars                (Total number of variables)
     * - 1: numEquations           (Total number of equations)
     * - 2: nnZ                    (Total number of non-zeros)
     * - 3: algorithm
     * - 4: nnz_processed          Number of non-zeros processed by a node
     * - 4: b (RHS vector)
     * - 4 + 1 * numEquations: r (LHS matrix row)
     * - 4 + 1 * numEquations + 1 * nnzA: c (LHS matrix column)
     * - 4 + 1 * numEquations + 2 * nnzA: v (LHS matrix value)
     */
    void rebind(uint32_t innumEquations, uint32_t inNNZA) {
        numVars.rebind(&mStorage[0]);
        numEquations.rebind(&mStorage[1]);
        NNZA.rebind(&mStorage[2]);
        algorithm.rebind(&mStorage[3]);
        nnz_processed.rebind(&mStorage[4]);
        b.rebind(&mStorage[5], innumEquations);
        b_stored.rebind(&mStorage[5 + innumEquations], innumEquations);
        r.rebind(&mStorage[5 + 2*innumEquations], inNNZA);
        c.rebind(&mStorage[5 + 2*innumEquations + inNNZA], inNNZA);
        v.rebind(&mStorage[5 + 2*innumEquations + 2 * inNNZA], inNNZA);
    }

    Handle mStorage;

public:
    typename HandleTraits<Handle>::ReferenceToUInt32 numVars;
    typename HandleTraits<Handle>::ReferenceToUInt32 numEquations;
    typename HandleTraits<Handle>::ReferenceToUInt32 NNZA;
    typename HandleTraits<Handle>::ReferenceToUInt32 nnz_processed;
    typename HandleTraits<Handle>::ReferenceToUInt32 algorithm;

    typename HandleTraits<Handle>::ColumnVectorTransparentHandleMap b_stored;
    typename HandleTraits<Handle>::ColumnVectorTransparentHandleMap b;
    typename HandleTraits<Handle>::ColumnVectorTransparentHandleMap r;
    typename HandleTraits<Handle>::ColumnVectorTransparentHandleMap c;
    typename HandleTraits<Handle>::ColumnVectorTransparentHandleMap v;
};

/**
 * @brief Perform the sparse linear system transition step
 */
AnyType
sparse_direct_linear_system_transition::run(AnyType &args) {

    SparseDirectLinearSystemTransitionState<MutableArrayHandle<double> >
                                                              state = args[0];
    int32_t row_id = args[1].getAs<int32_t>();
    int32_t col_id = args[2].getAs<int32_t>();
    double value = args[3].getAs<double>();
    double _b = args[4].getAs<double>();

    // When the segment recieves the first non-zero in the sparse matrix
    // we initialize the state
    if (state.nnz_processed == 0) {

        int32_t num_equations = args[5].getAs<int32_t>();
        int32_t num_vars = args[6].getAs<int32_t>();
        int32_t total_nnz = args[7].getAs<int32_t>();
        int algorithm = args[8].getAs<int>();
        state.initialize(*this,
                        num_vars,
                        num_equations,
                        total_nnz
                        );
        state.algorithm = algorithm;
        state.NNZA = total_nnz;
        state.numVars = num_vars;
        state.numEquations = num_equations;
        state.b_stored.setZero();
        state.b.setZero();

    }

	// Now do the transition step
	// First we create a block of zeros in memory
	// and then add the vector in the appropriate position
    ColumnVector bvec(static_cast<uint32_t>(state.numEquations));
    ColumnVector rvec(static_cast<uint32_t>(state.NNZA));
    ColumnVector cvec(static_cast<uint32_t>(state.NNZA));
    ColumnVector vvec(static_cast<uint32_t>(state.NNZA));

    bvec.setZero();
    rvec.setZero();
    cvec.setZero();
    vvec.setZero();

    rvec(state.nnz_processed) = row_id;
    cvec(state.nnz_processed) = col_id;
    vvec(state.nnz_processed) = value;

    if (state.b_stored(row_id) == 0) {
        bvec(row_id) = _b;
        state.b_stored(row_id) = 1;
        state.b += bvec;
    }

    // Build the vector & matrices based on row_id
    state.r += rvec;
    state.c += cvec;
    state.v += vvec;
    state.nnz_processed++;

    return state;
}

/**
 * @brief Perform the perliminary aggregation function: Merge transition states
 */
AnyType
sparse_direct_linear_system_merge_states::run(AnyType &args) {
    SparseDirectLinearSystemTransitionState<MutableArrayHandle<double> > stateLeft = args[0];
    SparseDirectLinearSystemTransitionState<ArrayHandle<double> > stateRight = args[1];

    // We first handle the trivial case where this function is called with one
    // of the states being the initial state
    if (stateLeft.numEquations == 0)
        return stateRight;
    else if (stateRight.numEquations == 0)
        return stateLeft;

    // Merge states together and return
    stateLeft += stateRight;
    return stateLeft;
}

/**
 * @brief Perform the linear system final step
 */
AnyType
sparse_direct_linear_system_final::run(AnyType &args) {
    // We request a mutable object. Depending on the backend, this might perform
    // a deep copy.
    SparseDirectLinearSystemTransitionState<MutableArrayHandle<double> > state = args[0];

    // Aggregates that haven't seen any data just return Null.
    if (state.numEquations == 0)
        return Null();

    // Eigen works better when you reserve the number of nnz
    // Note: When calling reserve(), it is not required that nnz is the
    //       exact number of nonzero elements in the final matrix. However, an
    //       exact estimation will avoid multiple reallocations during the
    //       insertion phase.
    SparseMatrix A(state.numEquations, state.numVars);
    A.reserve(state.NNZA);
    ColumnVector x;

    for (uint32_t i=0; i < state.NNZA; i++){
        A.insert(static_cast<uint32_t>(state.r(i)),
                 static_cast<uint32_t>(state.c(i))) = state.v(i);
    }

    // Switch case needs scoping in C++ if you want to declare inside it
    // Unfortunately, this means that I have to write the code to call the
    // solver in each switch case separtely
    switch (state.algorithm) {
      case 1:{
              Eigen::SimplicialLLT<SparseMatrix> solver;
              solver.compute(A);
              x = solver.solve(state.b);
              break;
      }
      case 2:{
              Eigen::SimplicialLDLT<SparseMatrix> solver;
              solver.compute(A);
              x = solver.solve(state.b);
              break;
      }
    }

    // It is unclear whether I should do the residual computation in-memory or
    // in-database (as done in the dense linear systems case)
    ColumnVector bvec = state.b;
    double residual = (A*x-bvec).norm() / bvec.norm();

    // Compute the residual
    return direct_sparse_stateToResult(*this, x, residual);
}

/**
 * @brief Helper function that computes the final statistics
 */

AnyType direct_sparse_stateToResult(
    const Allocator &inAllocator,
	const ColumnVector &x,
	const double residual) {

	MutableNativeColumnVector solution(
        inAllocator.allocateArray<double>(x.size()));

    for (Index i = 0; i < x.size(); ++i) {
        solution(i) = x(i);
    }

    // Return all coefficients, standard errors, etc. in a tuple
    AnyType tuple;
    tuple << solution
          << residual
          << Null();

    return tuple;
}

// ---------------------------------------------------------------------------
//              In-memory iterative sparse Linear System States
// ---------------------------------------------------------------------------

// Function protofype of Internal functions
AnyType inmem_iterative_sparse_stateToResult(
    const Allocator &inAllocator,
    const ColumnVector &x,
	const int iters,
	const double residual_norm);

/**
 * @brief States for linear systems
 *
 * TransitionState encapsualtes the transition state during the
 * logistic-regression aggregate function. To the database, the state is
 * exposed as a single DOUBLE PRECISION array, to the C++ code it is a proper
 * object containing scalars and vectors.
 *
 * Note: We assume that the DOUBLE PRECISION array is initialized by the
 * database with length at least 5, and all elemenets are 0.
 *
 */
template <class Handle>
class SparseInMemIterativeLinearSystemTransitionState {
    template <class OtherHandle>
    friend class SparseInMemIterativeLinearSystemTransitionState;

public:
    SparseInMemIterativeLinearSystemTransitionState(const AnyType &inArray)
        : mStorage(inArray.getAs<Handle>()) {

        rebind( static_cast<uint32_t>(mStorage[1]),
                static_cast<uint32_t>(mStorage[2]));
    }

    /**
     * @brief Convert to backend representation
     *
     * We define this function so that we can use State in the
     * argument list and as a return type.
     */
    inline operator AnyType() const {
        return mStorage;
    }

    /**
     * @brief Initialize the conjugate-gradient state.
     *
     * This function is only called for the first iteration, for the first row.
     */
    inline void initialize(const Allocator &inAllocator,
                          uint32_t innumVars,
                          uint32_t innumEquations,
                          uint32_t inNNZA
                          ) {
        // Array size does not depend in numVars
        mStorage = inAllocator.allocateArray<double, dbal::AggregateContext,
            dbal::DoZero, dbal::ThrowBadAlloc>(arraySize(innumEquations, inNNZA));
        rebind(innumEquations, inNNZA);
        numVars = innumVars;
        numEquations = innumEquations;
        NNZA = inNNZA;
    }

    /**
     * @brief We need to support assigning the previous state
     */
    template <class OtherHandle>
    SparseInMemIterativeLinearSystemTransitionState &operator=(
        const SparseInMemIterativeLinearSystemTransitionState<OtherHandle> &inOtherState) {

        for (size_t i = 0; i < mStorage.size(); i++)
            mStorage[i] = inOtherState.mStorage[i];
        return *this;
    }

    /**
     * @brief Merge with another State object by copying the intra-iteration
     *     fields
     */
    template <class OtherHandle>
    SparseInMemIterativeLinearSystemTransitionState &operator+=(
        const SparseInMemIterativeLinearSystemTransitionState<OtherHandle> &inOtherState) {

        if (mStorage.size() != inOtherState.mStorage.size() ||
            numVars != inOtherState.numVars ||
            NNZA != inOtherState.NNZA||
            numEquations != inOtherState.numEquations)
            throw std::logic_error("Internal error: Incompatible transition "
                "states");

        b += inOtherState.b;
        b_stored += inOtherState.b_stored;

        // Merge state for sparse loading is not an array add operation
        // but it is an array-append operation
        for (uint32_t i=0; i < inOtherState.nnz_processed; i++){
            r(nnz_processed + i) += inOtherState.r(i);
            c(nnz_processed + i) += inOtherState.c(i);
            v(nnz_processed + i) += inOtherState.v(i);
        }
        nnz_processed += inOtherState.nnz_processed;
        return *this;
    }

    /**
     * @brief Reset the inter-iteration fields.
     */
    inline void reset() {
        nnz_processed = 0;
        r.fill(0);
        c.fill(0);
        v.fill(0);
        b.fill(0);
        b_stored.fill(0);
    }

private:
    static inline size_t arraySize(const uint32_t innumEquations,
                                   const uint32_t inNNZA) {
        return 7 + 3 * inNNZA + 2 * innumEquations;
    }

    /**
     * @brief Rebind to a new storage array
     *
     * @param innumVars The number of independent variables.
     *
     * Array layout (iteration refers to one aggregate-function call):
     * Inter-iteration components (updated in final function):
     * - 0: numVars                (Total number of variables)
     * - 1: numEquations           (Total number of equations)
     * - 2: nnZ                    (Total number of non-zeros)
     * - 3: algorithm
     * - 4: maxIter
     * - 5: termToler
     * - 6: nnz_processed          Number of non-zeros processed by a node
     * - 7: b                      (RHS vector)
     * - 7: b_stored               (Boolean: Has the b_vector already been loaded?)
     * - 7 + 2 * numEquations: r (LHS matrix row)
     * - 7 + 2 * numEquations + 1 * nnzA: c (LHS matrix column)
     * - 7 + 2 * numEquations + 2 * nnzA: v (LHS matrix value)
     */
    void rebind(uint32_t innumEquations, uint32_t inNNZA) {
        numVars.rebind(&mStorage[0]);
        numEquations.rebind(&mStorage[1]);
        NNZA.rebind(&mStorage[2]);
        algorithm.rebind(&mStorage[3]);
        nnz_processed.rebind(&mStorage[4]);
        maxIter.rebind(&mStorage[5]);
        termToler.rebind(&mStorage[6]);
        b.rebind(&mStorage[7], innumEquations);
        b_stored.rebind(&mStorage[7 + innumEquations], innumEquations);
        r.rebind(&mStorage[7 + 2*innumEquations], inNNZA);
        c.rebind(&mStorage[7 + 2*innumEquations + inNNZA], inNNZA);
        v.rebind(&mStorage[7 + 2*innumEquations + 2 * inNNZA], inNNZA);
    }

    Handle mStorage;

public:
    typename HandleTraits<Handle>::ReferenceToUInt32 numVars;
    typename HandleTraits<Handle>::ReferenceToUInt32 numEquations;
    typename HandleTraits<Handle>::ReferenceToUInt32 NNZA;
    typename HandleTraits<Handle>::ReferenceToUInt32 nnz_processed;
    typename HandleTraits<Handle>::ReferenceToUInt32 algorithm;
    typename HandleTraits<Handle>::ReferenceToUInt32 maxIter;
    typename HandleTraits<Handle>::ReferenceToDouble termToler;

    typename HandleTraits<Handle>::ColumnVectorTransparentHandleMap b_stored;
    typename HandleTraits<Handle>::ColumnVectorTransparentHandleMap b;
    typename HandleTraits<Handle>::ColumnVectorTransparentHandleMap r;
    typename HandleTraits<Handle>::ColumnVectorTransparentHandleMap c;
    typename HandleTraits<Handle>::ColumnVectorTransparentHandleMap v;
};

/**
 * @brief Perform the sparse linear system transition step
 */
AnyType
sparse_inmem_iterative_linear_system_transition::run(AnyType &args) {

    SparseInMemIterativeLinearSystemTransitionState<MutableArrayHandle<double> >
                                                              state = args[0];
    int32_t row_id = args[1].getAs<int32_t>();
    int32_t col_id = args[2].getAs<int32_t>();
    double value = args[3].getAs<double>();
    double _b = args[4].getAs<double>();

    // When the segment recieves the first non-zero in the sparse matrix
    // we initialize the state
    if (state.nnz_processed == 0) {

        int32_t num_equations = args[5].getAs<int32_t>();
        int32_t num_vars = args[6].getAs<int32_t>();
        int32_t total_nnz = args[7].getAs<int32_t>();
        int algorithm = args[8].getAs<int>();
        int32_t maxIter = args[9].getAs<int>();
        double termToler = args[10].getAs<double>();

        state.initialize(*this,
                        num_vars,
                        num_equations,
                        total_nnz
                        );

        state.algorithm = algorithm;
        state.NNZA = total_nnz;
        state.numVars = num_vars;
        state.numEquations = num_equations;
        state.maxIter = maxIter;
        state.termToler = termToler;
        state.b_stored.setZero();
        state.b.setZero();

    }

	// Now do the transition step
	// First we create a block of zeros in memory
	// and then add the vector in the appropriate position
    ColumnVector bvec(static_cast<uint32_t>(state.numEquations));
    ColumnVector rvec(static_cast<uint32_t>(state.NNZA));
    ColumnVector cvec(static_cast<uint32_t>(state.NNZA));
    ColumnVector vvec(static_cast<uint32_t>(state.NNZA));

    bvec.setZero();
    rvec.setZero();
    cvec.setZero();
    vvec.setZero();

    rvec(state.nnz_processed) = row_id;
    cvec(state.nnz_processed) = col_id;
    vvec(state.nnz_processed) = value;

    if (state.b_stored(row_id) == 0) {
        bvec(row_id) = _b;
        state.b_stored(row_id) = 1;
        state.b += bvec;
    }

    // Build the vector & matrices based on row_id
    state.r += rvec;
    state.c += cvec;
    state.v += vvec;
    state.nnz_processed++;

    return state;
}

/**
 * @brief Perform the perliminary aggregation function: Merge transition states
 */
AnyType
sparse_inmem_iterative_linear_system_merge_states::run(AnyType &args) {
    SparseInMemIterativeLinearSystemTransitionState<MutableArrayHandle<double> > stateLeft = args[0];
    SparseInMemIterativeLinearSystemTransitionState<ArrayHandle<double> > stateRight = args[1];

    // We first handle the trivial case where this function is called with one
    // of the states being the initial state
    if (stateLeft.numEquations == 0)
        return stateRight;
    else if (stateRight.numEquations == 0)
        return stateLeft;

    // Merge states together and return
    stateLeft += stateRight;
    return stateLeft;
}

/**
 * @brief Perform the linear system final step
 */
AnyType
sparse_inmem_iterative_linear_system_final::run(AnyType &args) {
    // We request a mutable object. Depending on the backend, this might perform
    // a deep copy.
    SparseInMemIterativeLinearSystemTransitionState<MutableArrayHandle<double> > state = args[0];

    // Aggregates that haven't seen any data just return Null.
    if (state.numEquations == 0)
        return Null();

    // Eigen works better when you reserve the number of nnz
    // Note: When calling reserve(), it is not required that nnz is the
    //       exact number of nonzero elements in the final matrix. However, an
    //       exact estimation will avoid multiple reallocations during the
    //       insertion phase.
    SparseMatrix A(state.numEquations, state.numVars);
    A.reserve(state.NNZA);
    ColumnVector x;

    for (uint32_t i=0; i < state.NNZA; i++){
        A.insert(static_cast<uint32_t>(state.r(i)),
                 static_cast<uint32_t>(state.c(i))) = state.v(i);
    }

    int iters = 0;
    double error = 0.;

    // Switch case needs scoping in C++ if you want to declare inside it
    // Unfortunately, this means that I have to write the code to call the
    // solver in each switch case separtely
    // Note: CG uses a diagonal preconditioner by default
    switch (state.algorithm) {
      case 1:{
              Eigen::ConjugateGradient<SparseMatrix> solver;
              solver.setTolerance(state.termToler);
              solver.setMaxIterations(static_cast<int>(state.maxIter));
              x = solver.compute(A).solve(state.b);
              iters = solver.iterations();
              error = solver.error();
              break;
      }
      case 2:{
              Eigen::BiCGSTAB<SparseMatrix> solver;
              solver.setTolerance(state.termToler);
              solver.setMaxIterations(static_cast<int>(state.maxIter));
              x = solver.compute(A).solve(state.b);
              iters = solver.iterations();
              error = solver.error();
              break;
      }
      // Preconditioned CG: Uses an incomplete LUT preconditioner
      // For the incomplete LUT preconditioner. You might want to see Eigen docs
      // for factors like fill-in which will make the algorithm more suitable
      // for handling tougher linear systgems
      case 3:{
              // 3 Arguments in template
              //  ConjugateGradient< _MatrixType, _UpLo, _Preconditioner >:
              Eigen::ConjugateGradient<SparseMatrix, 1,
                                        Eigen::IncompleteLUT<double> > solver;
              solver.setTolerance(state.termToler);
              solver.setMaxIterations(static_cast<int>(state.maxIter));
              x = solver.compute(A).solve(state.b);
              iters = solver.iterations();
              error = solver.error();
              break;
      }
      case 4:{
              // 2 Arguments in template:
              //  ConjugateGradient< _MatrixType, _UpLo, _Preconditioner >:
              // NOTE: BiCGSTAB is a bi-conjugate gradient that does not
              // require a lower or upper triangular option
              Eigen::BiCGSTAB<SparseMatrix, Eigen::IncompleteLUT<double> > solver;
              solver.setTolerance(state.termToler);
              solver.setMaxIterations(static_cast<int>(state.maxIter));
              x = solver.compute(A).solve(state.b);
              iters = solver.iterations();
              error = solver.error();
              break;
      }
    }

    // Compute the residual
    return inmem_iterative_sparse_stateToResult(*this, x, iters, error);
}

/**
 * @brief Helper function that computes the final statistics
 */

AnyType inmem_iterative_sparse_stateToResult(
    const Allocator &inAllocator,
	const ColumnVector &x,
	const int iters,
	const double residual_norm) {

	MutableNativeColumnVector solution(
        inAllocator.allocateArray<double>(x.size()));

    for (Index i = 0; i < x.size(); ++i) {
        solution(i) = x(i);
    }

    // Return all coefficients, standard errors, etc. in a tuple
    AnyType tuple;
    tuple << solution
          << residual_norm
          << iters;

    return tuple;
}

} // namespace linear_systems

} // namespace modules

} // namespace madlib


// SPDX-License-Identifier: Apache-2.0
//
// Copyright 2015 Conrad Sanderson (http://conradsanderson.id.au)
// Copyright 2015 National ICT Australia (NICTA)
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// ------------------------------------------------------------------------

#include <armadillo>
#include "catch.hpp"

using namespace arma;

TEST_CASE("fn_conv_to_1")
  {
  typedef std::vector<double> stdvec;

  stdvec x(3);
  x[0] = 10.0; x[1] = 20.0;  x[2] = 30.0;

  colvec y = conv_to< colvec >::from(x);
  stdvec z = conv_to< stdvec >::from(y);

  REQUIRE( z[0] == Approx(10.0) );
  REQUIRE( z[1] == Approx(20.0) );
  REQUIRE( z[2] == Approx(30.0) );
  }

TEST_CASE("fn_conv_to2")
  {
  mat A(5,6); A.fill(0.1);

  umat uA = conv_to<umat>::from(A);
  imat iA = conv_to<imat>::from(A);

  REQUIRE( (uA.n_rows - A.n_rows) == 0 );
  REQUIRE( (iA.n_rows - A.n_rows) == 0 );

  REQUIRE( (uA.n_cols - A.n_cols) == 0 );
  REQUIRE( (iA.n_cols - A.n_cols) == 0 );

  REQUIRE( any(vectorise(uA)) == false);
  REQUIRE( any(vectorise(iA)) == false);
  }

TEST_CASE("fn_conv_to3")
  {
  mat A(5,6); A.fill(1.0);

  umat uA = conv_to<umat>::from(A);
  imat iA = conv_to<imat>::from(A);

  REQUIRE( all(vectorise(uA)) == true);
  REQUIRE( all(vectorise(iA)) == true);
  }

TEST_CASE("fn_conv_to4")
  {
  mat A =   linspace<rowvec>(1,5,6);
  mat B = 2*linspace<colvec>(1,5,6);
  mat C = randu<mat>(5,6);

  REQUIRE( as_scalar( conv_to<rowvec>::from(A) * conv_to<colvec>::from(B) ) == Approx(130.40) );

  REQUIRE( conv_to<double>::from(A * B) == Approx(130.40) );

  REQUIRE_THROWS( conv_to<colvec>::from(C) );
  }


/*
    Copyright (c) 2017 Mobile Robots Laboratory at Poznan University of Technology:
    -Jan Wietrzykowski name.surname [at] put.poznan.pl

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

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

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

#ifndef INCLUDE_MATCHING_HPP_
#define INCLUDE_MATCHING_HPP_

#include <vector>

#include <opencv2/opencv.hpp>

#include <Eigen/Eigen>

#include <g2o/types/slam3d/se3quat.h>

#include "Types.hpp"
#include "ObjInstance.hpp"

class Matching {
public:
	enum class MatchType{
		Ok,
		Unknown
	};

    struct PotMatch{
        PotMatch() {}

        PotMatch(int plane1,
                 const std::vector<int> &lineSegs1,
                 int plane2,
                 const std::vector<int> &lineSegs2)
                : plane1(plane1),
                  lineSegs1(lineSegs1),
                  plane2(plane2),
                  lineSegs2(lineSegs2) {}

        PotMatch(int plane1,
                 const std::vector<int> &lineSegs1,
                 int plane2,
                 const std::vector<int> &lineSegs2,
                 double planeAppDiff,
                 const std::vector<double> &lineSegAppDiffs)
                : plane1(plane1),
                  lineSegs1(lineSegs1),
                  plane2(plane2),
                  lineSegs2(lineSegs2),
                  planeAppDiff(planeAppDiff),
                  lineSegAppDiffs(lineSegAppDiffs) {}

        int plane1;

        std::vector<int> lineSegs1;

        int plane2;

        std::vector<int> lineSegs2;

        double planeAppDiff;

        std::vector<double> lineSegAppDiffs;
    };

    struct ValidTransform{
        ValidTransform()
        {}

        ValidTransform(const Vector7d& itransform,
                       const std::vector<Matching::PotMatch> &imatchSet,
                       const std::vector<double>& iintAreaPlanes,
                       const std::vector<std::vector<double>>& iintLenLines)
                : transform(itransform),
                  matchSet(imatchSet),
                  intAreaPlanes(iintAreaPlanes),
                  intLenLines(iintLenLines),
                  score(0.0)

        {}
        Vector7d transform;
        double score;
        std::vector<Matching::PotMatch> matchSet;
        std::vector<double> intAreaPlanes;
        std::vector<std::vector<double> > intLenLines;
    };

	static MatchType matchFrameToMap(const cv::FileStorage &fs,
									 const vectorObjInstance &frameObjInstances,
									 const vectorObjInstance &mapObjInstances,
									 vectorVector7d &bestTrans,
									 std::vector<double> &bestTransProbs,
									 std::vector<double> &bestTransFits,
									 std::vector<int> &bestTransDistinct,
									 std::vector<Matching::ValidTransform> &retTransforms,
									 pcl::visualization::PCLVisualizer::Ptr viewer = nullptr,
									 int viewPort1 = -1,
									 int viewPort2 = -1);

    static double planeEqDiffLogMap(const ObjInstance &obj1,
                                    const ObjInstance &obj2,
                                    const Vector7d &transform);

    static double lineSegEqDiff(const LineSeg &lineSeg1,
                                const LineSeg &lineSeg2,
                                const Vector7d &transform);

    static double checkConvexHullIntersection(const ObjInstance& obj1,
                                              const ObjInstance& obj2,
                                              const Vector7d& transform,
                                              double& intArea,
                                              pcl::visualization::PCLVisualizer::Ptr viewer = nullptr,
                                              int viewPort1 = -1,
                                              int viewPort2 = -1);

    static double checkLineSegIntersection(const LineSeg &lineSeg1,
                                           const LineSeg &lineSeg2,
                                           const Vector7d &transform,
                                           double &intLen);

    static Vector7d bestTransformPointsDirsDists(const vectorVector3d &points1,
												 const vectorVector3d &points2,
												 const std::vector<double> &pointsW,
												 const vectorVector3d &virtPoints1,
												 const vectorVector3d &virtPoints2,
												 const std::vector<double> &virtPointsW,
												 const vectorVector3d &dirs1,
												 const vectorVector3d &dirs2,
												 const std::vector<double> &dirsW,
												 const std::vector<double> &dists1,
												 const std::vector<double> &dists2,
												 const vectorVector3d &distDirs1,
												 const std::vector<double> &distsW,
												 const vectorVector3d &distPts1,
												 const vectorVector3d &distPts2,
												 const vectorVector3d &distPtsDirs1,
												 const std::vector<double> &distsPtsW,
												 double sinValsThresh,
												 bool &fullConstrRot,
												 bool &fullConstrTrans);

    static void convertToPointsDirsDists(const vectorVector3d &points,
                                        const std::vector<Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d>> &planes,
                                        const std::vector<Vector6d> &lines,
                                        vectorVector3d &retPoints,
                                        vectorVector3d &retVirtPoints,
                                        vectorVector3d &retDirs,
                                        std::vector<double> &retDists,
                                        vectorVector3d &retDistDirs,
                                        vectorVector3d &retDistPts,
                                        vectorVector3d &retDistPtsDirs);

private:

	class ProbDistKernel{
	public:
		ProbDistKernel(Vector7d ikPt,
						Eigen::Matrix<double, 6, 6> iinfMat,
						double iweight);

		double eval(Vector7d pt) const;

		EIGEN_MAKE_ALIGNED_OPERATOR_NEW
	private:
		Vector7d kPt;

		g2o::SE3Quat kPtSE3Quat;

		Eigen::Matrix<double, 6, 6> infMat;

		double weight;
	};

    typedef std::vector<Matching::ProbDistKernel, Eigen::aligned_allocator<Matching::ProbDistKernel> > vectorProbDistKernel;

    static double compAngleDiffBetweenNormals(const Eigen::Vector3d &nf1,
											  const Eigen::Vector3d &ns1,
											  const Eigen::Vector3d &nf2,
											  const Eigen::Vector3d &ns2);

    static bool checkLineToLineAng(const vectorLineSeg &lineSegs1,
                                   const vectorLineSeg &lineSegs2,
                                   double lineToLineAngThresh);

    static bool checkPlaneToPlaneAng(const vectorVector4d &planes1,
                                     const vectorVector4d &planes2,
                                     double planeToPlaneAngThresh);

    static bool checkPlaneToLineAng(const vectorVector4d &planes1,
                                     const vectorLineSeg &lineSegs1,
                                     const vectorVector4d &planes2,
                                     const vectorLineSeg &lineSegs2,
                                     double planeToLineAngThresh);

    static std::vector<PotMatch> findPotMatches(const vectorObjInstance &mapObjInstances,
												const vectorObjInstance &frameObjInstances,
                                                double planeAppThresh,
                                                double lineAppThresh,
                                                double lineToLineAngThresh,
                                                pcl::visualization::PCLVisualizer::Ptr viewer = nullptr,
                                                int viewPort1 = -1,
                                                int viewPort2 = -1);

    static std::vector<std::vector<PotMatch> > findPotSets(const std::vector<PotMatch> &potMatches,
                                                           const vectorObjInstance &mapObjInstances,
                                                           const vectorObjInstance &frameObjInstances,
                                                           double planeDistThresh,
                                                           double lineToLineAngThresh,
                                                           double planeToPlaneAngThresh,
                                                           double planeToLineAngThresh,
                                                           pcl::visualization::PCLVisualizer::Ptr viewer = nullptr,
                                                           int viewPort1 = -1,
                                                           int viewPort2 = -1);

	static void compObjFeatures(const vectorObjInstance& objInstances,
								std::vector<cv::Mat>& objFeats);

    static void compObjDistances(const vectorObjInstance& objInstances,
                                std::vector<std::vector<double>>& objDistances);

	static void comp3DTransform(const vectorVector4d& planes1,
								const vectorVector4d& planes2,
								const std::vector<std::pair<int, int>>& triplet,
								Vector7d& transform,
                                double sinValsThresh,
								bool& fullConstr);

	static Vector7d bestTransformPlanes(const vectorVector4d& planes1,
                                        const vectorVector4d& planes2,
                                        double sinValsThresh,
                                        bool &fullConstr);

	static Vector7d bestTransformPointsAndDirs(const vectorVector3d& points1,
                                               const vectorVector3d& points2,
                                               const std::vector<double>& pointsW,
                                               const vectorVector3d& dirs1,
                                               const vectorVector3d& dirs2,
                                               const std::vector<double>& dirsW,
                                               double sinValsThresh,
                                               bool compTrans,
                                               bool &fullConstrRot,
                                               bool &fullConstrTrans);

	static double scoreTransformByProjection(const Vector7d& transform,
								const std::vector<std::pair<int, int>> triplet,
								const vectorObjInstance& objInstances1,
								const vectorObjInstance& objInstances2,
								std::vector<double>& intAreaPair,
                                double planeEqDiffThresh,
                                double intAreaThresh,
								pcl::visualization::PCLVisualizer::Ptr viewer = nullptr,
								int viewPort1 = -1,
								int viewPort2 = -1);

    static double scoreTransformByProjection(const Vector7d &transform,
                                             const std::vector<PotMatch> curSet,
                                             const vectorObjInstance &objInstances1,
                                             const vectorObjInstance &objInstances2,
                                             std::vector<double> &intAreaPlanes,
                                             std::vector<std::vector<double> > &intLenLines,
                                             double planeEqDiffThresh,
                                             double lineEqDiffThresh,
                                             double intAreaThresh,
                                             double intLenThresh,
                                             pcl::visualization::PCLVisualizer::Ptr viewer = nullptr,
											 int viewPort1 = -1,
											 int viewPort2 = -1);

	static double evalPoint(const Vector7d &pt,
							const vectorProbDistKernel &dist);

    static int countDifferent(const std::set<int> &setIdxs,
                              const vectorObjInstance &objs);
};

#endif /* INCLUDE_MATCHING_HPP_ */


#ifndef UNITS_HPP
#define UNITS_HPP

#include "utils/point.hpp"

#include <boost/units/quantity.hpp>
#include <boost/units/systems/si.hpp>

namespace utils
{
	namespace units
	{
		using acceleration = boost::units::quantity<boost::units::si::acceleration>;
		using force = boost::units::quantity<boost::units::si::force>;
		using frequency = boost::units::quantity<boost::units::si::frequency>;
		using length = boost::units::quantity<boost::units::si::length>;
		using mass = boost::units::quantity<boost::units::si::mass>;
		using area = boost::units::quantity<boost::units::si::area>;
		using time = boost::units::quantity<boost::units::si::time>;
		using velocity = boost::units::quantity<boost::units::si::velocity>;

		using acceleration_vector = utils::point3d<acceleration>;
		using velocity_vector = utils::point3d<velocity>;
		using position_vector = utils::point3d<length>;
	}
}

#endif // UNITS_HPP


/**
 * $Id$
 *
 * Copyright (C)
 * 2015 - $Date$
 *     Martin Wolf <ndhist@martin-wolf.org>
 *
 * This file is distributed under the BSD 2-Clause Open Source License
 * (See LICENSE file).
 *
 */
#include <string>

#include <boost/shared_ptr.hpp>
#include <boost/python.hpp>

#include <boost/numpy/ndarray.hpp>

#include <ndhist/axis.hpp>
#include <ndhist/axes/log10_axis.hpp>

namespace bp = boost::python;
namespace bn = boost::numpy;

namespace ndhist {
namespace axes {

void register_log10_axis()
{
    bp::class_<py::log10_axis, bp::bases<Axis>, boost::shared_ptr<py::log10_axis> >(
          "log10_axis"
        , "The log10_axis class provides an axis class for logarithmic scaled "
          "axes with constant bin widths in its logarithmic base 10 space."
        , bp::init<
            bn::ndarray const &
          , std::string const &
          , std::string const &
          , bool
          , bool
          , bool
          , intptr_t
          , intptr_t
          >(
          ( bp::arg("self")
          , bp::arg("edges")
          , bp::arg("label")=std::string("")
          , bp::arg("name")=std::string("")
          , bp::arg("has_underflow_bin")=true
          , bp::arg("has_overflow_bin")=true
          , bp::arg("is_extendable")=false
          , bp::arg("extension_max_fcap")=0
          , bp::arg("extension_max_bcap")=0
          )
          )
        )
        .def(axis_pyinterface<py::log10_axis>())
    ;
}

}//namespace axes
}//namespace ndhist


/**
 *  Copyright (C) 2012
 *    Ekaterina Potapova, Andreas Richtsfeld, Johann Prankl, Thomas Mörwald, Michael Zillich
 *    Automation and Control Institute
 *    Vienna University of Technology
 *    Gusshausstraße 25-29
 *    1170 Vienna, Austria
 *    ari(at)acin.tuwien.ac.at
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see http://www.gnu.org/licenses/
 */

/**
 * @file trainSVM.cpp
 * @author Andreas Richtsfeld, Ekaterina Potapova
 * @date January 2014
 * @version 0.1
 * @brief Trains svm based on scaled features.
 */

#include <stdio.h>  /* printf, scanf, puts, NULL */
#include <stdlib.h> /* srand, rand */
#include <time.h>   /* time */
#include <fstream>

#include <glog/logging.h>

#include <pcl/io/pcd_io.h>

#include <boost/filesystem.hpp>
#include <boost/filesystem/fstream.hpp>
#include <boost/random.hpp>
#include <random>

#include "v4r/attention_segmentation/SVMTrainModel.h"

namespace {
template <typename N>
N getRandom(N min, N max, std::mt19937 &seed) {
  std::uniform_real_distribution<> dist(min, max);
  return dist(seed);
}

void trainFeatures(double gamma, double C, int kernel, int n_folds, double &RecRate, std::vector<int> &ConfusionTable,
                   std::string &train_ST_file_name_scaled, std::string &model_file_name) {
  svm::SVMTrainModel svmTrainModel;
  svmTrainModel.setInputFileName(train_ST_file_name_scaled);
  svmTrainModel.setModelFileName(model_file_name);
  svmTrainModel.setKernelType(kernel);
  svmTrainModel.setSVMType(svm::C_SVC);
  svmTrainModel.setGamma(gamma);
  svmTrainModel.setC(C);
  svmTrainModel.setProbability(1);
  svmTrainModel.setCrossValidation(n_folds);
  svmTrainModel.setNoPrint(true);
  svmTrainModel.train(RecRate, ConfusionTable);
}

void trainSVM(std::string &train_ST_file_name_scaled, std::string &model_file_name) {
  std::vector<std::string> kernel_names = {"rbf", "linear", "polynomial", "sigmoid"};
  int kernels[4] = {svm::RBF, svm::LINEAR, svm::POLY, svm::SIGMOID};

  printf("Classifier: LibSVM\nParameters (LibSVM): KernelType,C,Gamma,RecRate,ConfusionTable\n--\n");

  double RecRateBest = 0;
  int kernelBest;
  std::vector<int> ConfusionTableBest;
  ConfusionTableBest.resize(4);
  int gammaBest = 0;
  int CBest = 0;

  for (int k = 0; k < 1; ++k) {
    for (int C = -5; C <= 15; C = C + 2) {
      for (int gamma = -15; gamma <= 3; gamma = gamma + 2) {
        double RecRate;
        std::vector<int> ConfusionTable;
        ConfusionTable.resize(4);
        trainFeatures(pow(2.0, gamma), pow(2.0, C), kernels[k], 2, RecRate, ConfusionTable, train_ST_file_name_scaled,
                      model_file_name);

        printf("%s;%8.6f;%8.6f;%8.6f;%3.1f/%3.1f/%3.1f/%3.1f/\n", kernel_names.at(k).c_str(), pow(2.0, C),
               pow(2.0, gamma), RecRate, (double)(ConfusionTable.at(0)), (double)(ConfusionTable.at(1)),
               (double)(ConfusionTable.at(2)), (double)(ConfusionTable.at(3)));

        if (RecRate > RecRateBest) {
          RecRateBest = RecRate;
          kernelBest = k;
          gammaBest = gamma;
          CBest = C;
          ConfusionTableBest = ConfusionTable;
        }
      }
    }
  }

  printf("--\nGlobalBest (LibSVM):\n");
  printf("%s;%8.6f;%8.6f;%8.6f;%3.1f/%3.1f/%3.1f/%3.1f/\n", kernel_names.at(kernelBest).c_str(), pow(2.0, CBest),
         pow(2.0, gammaBest), RecRateBest, (double)(ConfusionTableBest.at(0)), (double)(ConfusionTableBest.at(1)),
         (double)(ConfusionTableBest.at(2)), (double)(ConfusionTableBest.at(3)));

  printf("Training final classifier...\n");
  trainFeatures(pow(2.0, gammaBest), pow(2.0, CBest), kernels[kernelBest], 0, RecRateBest, ConfusionTableBest,
                train_ST_file_name_scaled, model_file_name);
  printf("%s;%8.6f;%8.6f;%8.6f;%3.1f/%3.1f/%3.1f/%3.1f/\n", kernel_names.at(kernelBest).c_str(), pow(2.0, CBest),
         pow(2.0, gammaBest), RecRateBest, (double)(ConfusionTableBest.at(0)), (double)(ConfusionTableBest.at(1)),
         (double)(ConfusionTableBest.at(2)), (double)(ConfusionTableBest.at(3)));
}

void trainSVMrand(std::string &train_ST_file_name_scaled, std::string &model_file_name) {
  int kernels[4] = {svm::RBF, svm::LINEAR, svm::POLY, svm::SIGMOID};
  std::vector<std::string> kernel_names = {"rbf", "linear", "polynomial", "sigmoid"};

  printf("Classifier: LibSVM\nParameters (LibSVM): KernelType,C,Gamma,RecRate,ConfusionTable\n--\n");

  //  double RecRateBest = 0;
  //  int kernelBest;
  //  std::vector<int> ConfusionTableBest;
  //  ConfusionTableBest.resize(4);
  //  int gammaBest = 0;
  //  int CBest = 0;

  double RecRateBest = 0;
  int kernelBest;
  std::vector<int> ConfusionTableBest;
  ConfusionTableBest.resize(4);
  double gammaBest = 0;
  double CBest = 0;

  // double start = 0.000011;
  // double end   = 32768.000000;
  double start = 0.000001;
  double gamma_end = 3.0;
  double gamma = 0;

  std::random_device rd;
  std::mt19937 seed(rd());

  // for(int k = 0; k < 1; ++k)
  for (int k = 0; k < 1; ++k) {
    // for(int C = -5; C <= 15; C = C+2)
    for (double C = -15; C <= 3; C = C + 2) {
      // for(int gamma = -15; gamma <= 3; gamma = gamma + 2)
      for (int i = 0; i <= 100; i++) {
        // random informed search
        gamma = getRandom<double>(start, gamma_end, seed);

        double RecRate;
        std::vector<int> ConfusionTable;
        ConfusionTable.resize(4);
        trainFeatures(gamma, pow(2.0, C), kernels[k], 10, RecRate, ConfusionTable, train_ST_file_name_scaled,
                      model_file_name);

        printf("%s;%8.6f;%8.6f;%8.6f;%3.1f/%3.1f/%3.1f/%3.1f/\n", kernel_names.at(k).c_str(), pow(2.0, C), gamma,
               RecRate, (double)(ConfusionTable.at(0)), (double)(ConfusionTable.at(1)), (double)(ConfusionTable.at(2)),
               (double)(ConfusionTable.at(3)));

        if (RecRate > RecRateBest) {
          RecRateBest = RecRate;
          kernelBest = k;
          gammaBest = gamma;
          CBest = C;
          ConfusionTableBest = ConfusionTable;
        }
      }
    }
  }

  printf("--\nGlobalBest (LibSVM):\n");
  printf("%s;%8.6f;%8.6f;%8.6f;%3.1f/%3.1f/%3.1f/%3.1f/\n", kernel_names.at(kernelBest).c_str(), pow(2.0, CBest),
         gammaBest, RecRateBest, (double)(ConfusionTableBest.at(0)), (double)(ConfusionTableBest.at(1)),
         (double)(ConfusionTableBest.at(2)), (double)(ConfusionTableBest.at(3)));

  printf("Training final classifier...\n");
  // trainFeatures(gammaBest,CBest,kernels[kernelBest],0,RecRateBest,ConfusionTableBest,train_ST_file_name_scaled,model_file_name);
  trainFeatures(pow(2.0, gammaBest), pow(2.0, CBest), kernels[kernelBest], 0, RecRateBest, ConfusionTableBest,
                train_ST_file_name_scaled, model_file_name);
  printf("%s;%8.6f;%8.6f;%8.6f;%3.1f/%3.1f/%3.1f/%3.1f/\n", kernel_names.at(kernelBest).c_str(), pow(2.0, CBest),
         gammaBest, RecRateBest, (double)(ConfusionTableBest.at(0)), (double)(ConfusionTableBest.at(1)),
         (double)(ConfusionTableBest.at(2)), (double)(ConfusionTableBest.at(3)));
}

void printUsage(char *av) {
  printf(
      "Usage: %s training_data.txt.scaled model.txt\n"
      " Options:\n"
      "   [-h] ... show this help.\n"
      "   training_data.txt.scaled ... filename with scaled training samples\n"
      "   model.txt                ... output model\n"
      "   [-s] ... structured search training\n",
      av);
  std::cout << " Example: " << av << " training_data.txt.scaled model.txt" << std::endl;
}
}  // namespace

int main(int argc, char *argv[]) {
  if (argc > 4 || argc < 3) {
    printUsage(argv[0]);
    exit(0);
  }

  // init logging
  google::InitGoogleLogging(argv[0]);

  std::string train_ST_file_name_scaled = argv[1];
  std::string model_file_name = argv[2];

  if (argc != 4) {
    trainSVMrand(train_ST_file_name_scaled, model_file_name);
  } else {
    trainSVM(train_ST_file_name_scaled, model_file_name);
  }

  return (0);
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   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_LINALG_FUNCTIONS_GENERAL_MPOWER_HPP_INCLUDED
#define NT2_LINALG_FUNCTIONS_GENERAL_MPOWER_HPP_INCLUDED

#include <nt2/linalg/functions/mpower.hpp>
#include <nt2/core/container/table/table.hpp>
#include <nt2/include/functions/sqrt.hpp>
#include <nt2/include/functions/is_flint.hpp>
#include <nt2/include/functions/is_ltz.hpp>
#include <nt2/include/functions/is_even.hpp>
#include <nt2/include/functions/logm.hpp>
#include <nt2/include/functions/expm.hpp>
#include <nt2/include/functions/log.hpp>
#include <nt2/include/functions/pow.hpp>
#include <nt2/include/functions/modf.hpp>
#include <nt2/include/functions/diag_of.hpp>
#include <nt2/include/functions/from_diag.hpp>
#include <nt2/include/functions/isdiagonal.hpp>
#include <nt2/include/functions/is_real.hpp>
#include <nt2/include/functions/schur.hpp>
#include <nt2/include/functions/mtimes.hpp>
#include <nt2/include/functions/power.hpp>
#include <nt2/include/functions/trunc.hpp>
#include <nt2/include/functions/inv.hpp>
#include <nt2/include/functions/sum.hpp>
#include <nt2/include/functions/sqrtm.hpp>
#include <nt2/include/functions/zeros.hpp>
#include <nt2/include/functions/eye.hpp>
#include <nt2/include/functions/length.hpp>
#include <nt2/include/functions/transpose.hpp>
#include <nt2/include/functions/conj.hpp>
#include <nt2/include/functions/size.hpp>
#include <nt2/include/functions/isscalar.hpp>
#include <nt2/include/functions/issquare.hpp>
#include <nt2/include/functions/real.hpp>
#include <nt2/include/functions/imag.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/core/container/dsl/size.hpp>
#include <nt2/core/utility/max_extent.hpp>
#include <nt2/sdk/complex/meta/is_complex.hpp>
#include <boost/mpl/bool.hpp>
#include <boost/assert.hpp>

namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( mpower_, tag::cpu_
                            , (A0)(A1)
                            , (scalar_<unspecified_<A0> >)
                              (scalar_<unspecified_<A1> >)
                            )
  {
    typedef A0 result_type;
    NT2_FUNCTOR_CALL(2)
    {
      return nt2::pow(a0, a1);
    }
  };

  BOOST_DISPATCH_IMPLEMENT  ( mpower_, tag::cpu_
                            , (A0)(A1)
                            , (scalar_<unspecified_<A0> >)
                              ((ast_<A1, nt2::container::domain>))
                            )
  {
    BOOST_DISPATCH_RETURNS(2, (A0 const& a0, A1 const& a1),
      nt2::expm(nt2::log(a0)*a1)
    )
  };

  // mpower tag only used for matrix/scalar
  template<class Domain, int N, class Expr>
  struct size_of<tag::mpower_,Domain,N,Expr>
  {
    typedef typename boost::proto::result_of::child_c<Expr&,0>::value_type  c0_t;
    typedef typename boost::proto::result_of::child_c<Expr&,1>::value_type  c1_t;
    typedef typename utility::result_of::max_extent<c0_t, c1_t, _2D>::type       result_type;

    BOOST_FORCEINLINE result_type operator()(Expr& e) const
    {
      BOOST_ASSERT_MSG( isscalar(boost::proto::child_c<1>(e)) && issquare(boost::proto::child_c<0>(e)),
                       "mpower needs a square matrix and a scalar or a scalar and a square matrix");

      return nt2::utility::max_extent(boost::proto::child_c<0>(e), boost::proto::child_c<1>(e), _2D());
    }
  };

  BOOST_DISPATCH_IMPLEMENT  ( run_assign_, tag::cpu_
                            , (A0)(A1)
                            , ((ast_<A0, nt2::container::domain>))
                              ((node_<A1, nt2::tag::mpower_, boost::mpl::long_<2>, nt2::container::domain>))
                            )
  {
    typedef void                                                    result_type;
    typedef typename A1::value_type                                  value_type;
    typedef typename meta::as_real<value_type>::type                     r_type;
    typedef std::complex<r_type>                                      cplx_type;

    BOOST_FORCEINLINE result_type operator()(A0& a0, A1& a1) const
    {
      typedef typename meta::is_complex<value_type>::type is_cplx;
      compute_power(boost::proto::child_c<0>(a1), value_type(boost::proto::child_c<1>(a1)), a0, is_cplx());
    }

  private:
    template < class T >
    BOOST_FORCEINLINE static void compute_power(const T& a, r_type b, A0& r, const boost::mpl::false_&)
    {
      r.resize(extent(a));
      typedef typename A1::index_type        index_type;
      typedef table<value_type, index_type> result_type;
      typedef table<cplx_type>                  ct_type;
      bool is_ltz_b = is_ltz(b);
      if(is_ltz_b) b = -b;
      r_type m = nt2::trunc(b);
      r_type f = b-m;
      ct_type q, t;
      nt2::tie(q, t) = schur(a, nt2::cmplx_);
      if(isdiagonal(t))
      {
        t = nt2::from_diag(nt2::pow(diag_of(t), m));
        if(is_ltz_b) t = nt2::inv(t);
        BOOST_AUTO_TPL(z, nt2::mtimes(q, nt2::mtimes(t, nt2::trans(nt2::conj(q)))));
        transtype(r, z, typename nt2::meta::is_complex<value_type>::type());
        return;
      }
      else
      { //use iterative method
        r = nt2::eye(nt2::size(a), meta::as_<r_type>());
        result_type rf = r;
        if(m)
        {
          result_type a00 = a;
          while (m >= nt2::One<r_type>())
          {
            if (nt2::is_odd(m))
            {
              r =  nt2::mtimes(a00, r);
            }
            a00 =  nt2::mtimes(a00, a00);
            m =  nt2::trunc(m/2);
          }
        }
        if(!f)
        {
          if(is_ltz_b) r = nt2::inv(r);
          return;
        }
        else
        {
          result_type a00 = nt2::sqrtm(a);
          r_type thresh = nt2::Half<r_type>();
          while(f > Zero<r_type>())
          {
            if(f >= thresh)
            {
              rf = nt2::mtimes(rf, a00);
              f -= thresh;
            }
            thresh *= nt2::Half<r_type>();
            a00 =  nt2::sqrtm(a00);
          }
        }
        r= nt2::mtimes(r, rf);
        if(is_ltz_b) r = nt2::inv(r);
      }
    }
    template < class T >
    BOOST_FORCEINLINE static void compute_power(const T& a, value_type b, A0& r, const boost::mpl::true_& )
    {
      if(is_real(b))
        compute_power(a, real(b), r, boost::mpl::false_());
      else
        r = nt2::expm(b*nt2::logm(a));
    }

    template < class T1, class T2>
    BOOST_FORCEINLINE static void transtype(T1& r, T2& z, boost::mpl::true_ const &)
    {
      r = z;
    }
    template < class T1, class T2 >
    BOOST_FORCEINLINE static void transtype(T1& r, T2& z, boost::mpl::false_ const &)
    {
      r = real(z);
    }

  };

} }

#endif


#include "BAData.hpp"
#include <cmath>
#include <boost/bind.hpp>
#include "PIPSLogging.hpp"
using boost::bind; // change to std::bind with C++11
using namespace std;

namespace {

template<typename T1, typename T2> void mergeColAndRow(T1& v, const T2 &col, const T2 &row) {
	for (unsigned i = 0; i < col.size(); i++) {
		v[i] = col[i];
	}
	for (unsigned i = 0; i < row.size(); i++) {
		v[i+col.size()] = row[i];
	}

}

template<typename BAVec, typename T1, typename Col2, typename Row2>
	void formBAVector(BAVec &v, const T1 &c1, Col2 c2, const T1 &r1, Row2 r2, const BAContext& ctx) {

	const vector<int> localScen = ctx.localScenarios();
	mergeColAndRow(v.getFirstStageVec(), c1, r1);

	for (unsigned i = 1; i < localScen.size(); i++) {
		int scen = localScen[i];
		mergeColAndRow(v.getSecondStageVec(scen), c2(scen), r2(scen));
	}

}

// use a lambda for this when we can use C++11
class emptyRowVector {
public:
	emptyRowVector(stochasticInput &i) : i(i) {}
	vector<double> operator()(int s) { return vector<double>(i.nSecondStageCons(s),0.0); }

private:
	stochasticInput &i;
};

void checkConstraintType(const denseVector &L, const denseVector &U, denseFlagVector<constraintType> &T) {
	int n = L.length();
	assert(U.length() == n && T.length() == n);

	for (int i = 0; i < n; i++) {
		if (L[i] < -1e20) {
			if (U[i] > 1e20) {
				T[i] = Free;
			} else {
				T[i] = UB;
			}
		} else {
			if (U[i] > 1e20) {
				T[i] = LB;
			} else {
				T[i] = Range;
			}
		}
		if (T[i] == Range && abs(L[i] - U[i]) < 1e-8) {
			T[i] = Fixed;
		}
	}

}

}

BAData::BAData(stochasticInput &input, BAContext &ctx) : ctx(ctx) {
	int nscen = input.nScenarios();
	ctx.initializeAssignment(nscen); // must do this first
	dims = BADimensions(input,ctx);

	const vector<int> &localScen = ctx.localScenarios();

	l.allocate(dims, ctx, PrimalVector);
	u.allocate(dims, ctx, PrimalVector);
	c.allocate(dims, ctx, PrimalVector);
	vartype.allocate(dims, ctx, PrimalVector);
	names.allocate(dims, ctx, PrimalVector);

	formBAVector(l, input.getFirstStageColLB(),
			bind(&stochasticInput::getSecondStageColLB,&input,_1),
			input.getFirstStageRowLB(),
			bind(&stochasticInput::getSecondStageRowLB,&input,_1),ctx);

	formBAVector(u, input.getFirstStageColUB(),
			bind(&stochasticInput::getSecondStageColUB,&input,_1),
			input.getFirstStageRowUB(),
			bind(&stochasticInput::getSecondStageRowUB,&input,_1),ctx);

	formBAVector(c, input.getFirstStageObj(),
			bind(&stochasticInput::getSecondStageObj,&input,_1),
			vector<double>(input.nFirstStageCons(),0.0),
			emptyRowVector(input), ctx);

	formBAVector(names, input.getFirstStageColNames(),
			bind(&stochasticInput::getSecondStageColNames,&input,_1),
			input.getFirstStageRowNames(),
			bind(&stochasticInput::getSecondStageRowNames,&input,_1),ctx);

	for (unsigned i = 0; i < localScen.size(); i++) {
		int scen = localScen[i];
		checkConstraintType(l.getVec(scen),u.getVec(scen),vartype.getVec(scen));
	}

	Acol.reset(new CoinPackedMatrix(input.getFirstStageConstraints()));
	Arow.reset(new CoinPackedMatrix());
	Arow->reverseOrderedCopyOf(*Acol);

	Tcol.resize(nscen); Trow.resize(nscen);
	Wcol.resize(nscen); Wrow.resize(nscen);

	/*
	We can save memory by not duplicating the constraint matrices, but
	as soon as we add individual scenario cuts these need to be duplicated anyway.
	One could think about adding identical cuts (with different RHSs)
	to each scenario to restore this optimization.

	onlyBoundsVary = input.onlyBoundsVary();
	if (onlyBoundsVary) {
		Tcol[0].reset(new CoinPackedMatrix(input.getLinkingConstraints(0)));
		Trow[0].reset(new CoinPackedMatrix());
		Trow[0]->reverseOrderedCopyOf(*Tcol[0]);

		Wcol[0].reset(new CoinPackedMatrix(input.getSecondStageConstraints(0)));
		Wrow[0].reset(new CoinPackedMatrix());
		Wrow[0]->reverseOrderedCopyOf(*Wcol[0]);

		for (int i = 1; i < nscen; i++) {
			Tcol[i] = Tcol[0];
			Trow[i] = Trow[0];
			Wcol[i] = Wcol[0];
			Wrow[i] = Wrow[0];
		}
	} else {*/
		for (int i = 0; i < nscen; i++) {
			if (!ctx.assignedScenario(i)) continue;
			Tcol[i].reset(new CoinPackedMatrix(input.getLinkingConstraints(i)));
			Trow[i].reset(new CoinPackedMatrix());
			Trow[i]->reverseOrderedCopyOf(*Tcol[i]);

			Wcol[i].reset(new CoinPackedMatrix(input.getSecondStageConstraints(i)));
			Wrow[i].reset(new CoinPackedMatrix());
			Wrow[i]->reverseOrderedCopyOf(*Wcol[i]);
		}
	/*
	}*/

	out1Send.reserve(dims.numFirstStageVars());

}

BAData::BAData(const BAData &d) : dims(d.dims.inner), ctx(d.ctx) {
	l.allocate(dims, ctx, PrimalVector);
	u.allocate(dims, ctx, PrimalVector);
	c.allocate(dims, ctx, PrimalVector);
	vartype.allocate(dims, ctx, PrimalVector);
	names.allocate(dims, ctx, PrimalVector);

	onlyBoundsVary = d.onlyBoundsVary;

	Acol = d.Acol;
	Arow = d.Arow;
	Tcol = d.Tcol;
	Trow = d.Trow;
	Wcol = d.Wcol;
	Wrow = d.Wrow;

	l.copyFrom(d.l);
	u.copyFrom(d.u);
	c.copyFrom(d.c);
	vartype.copyFrom(d.vartype);
	names.copyFrom(d.names);

	out1Send.reserve(dims.numFirstStageVars());

}

BAData::~BAData() {
	// shared_ptr handles the deletes
}

// assume v is cleared
void BAData::getCol(sparseBAVector &v, BAIndex idx) const {
	int scen = idx.scen;
	int col = idx.idx;
	const vector<int> &localScen = v.localScenarios();

	if (scen == -1) { // first stage
		CoinIndexedVector &v1 = v.getFirstStageVec().v;
		double *v1Elts = v1.denseVector();
		int *v1Idx = v1.getIndices();
		int nvarreal = dims.inner.numFirstStageVars();
		if (col < nvarreal) {
			int nnz = 0;
			const double *AcolElts = Acol->getElements();
			const int *AcolIdx = Acol->getIndices();
			CoinBigIndex start = Acol->getVectorFirst(col);
			CoinBigIndex end = Acol->getVectorLast(col);
			for (CoinBigIndex q = start; q < end; q++) {
				int row = AcolIdx[q];
				v1Elts[row] = AcolElts[q];
				v1Idx[nnz++] = row;
			}
			v1.setNumElements(nnz);

			for (unsigned j = 1; j < localScen.size(); j++) {
				int s = localScen[j];
				const double *TcolElts = Tcol[s]->getElements();
				const int *TcolIdx = Tcol[s]->getIndices();
				CoinIndexedVector &v2 = v.getSecondStageVec(s).v;
				double *v2Elts = v2.denseVector();
				int *v2Idx = v2.getIndices();
				nnz = 0;
				start = Tcol[s]->getVectorFirst(col);
				end = Tcol[s]->getVectorLast(col);
				for (CoinBigIndex q = start; q < end; q++) {
					int row = TcolIdx[q];
					v2Elts[row] = TcolElts[q];
					v2Idx[nnz++] = row;
				}
				v2.setNumElements(nnz);
			}
		} else {
			int k = col - nvarreal;
			v1Elts[k] = -1.0;
			v1Idx[0] = k;
			v1.setNumElements(1);
		}

	} else {
		if (!ctx.assignedScenario(scen)) return;
		CoinIndexedVector &v2 = v.getSecondStageVec(scen).v;
		double *v2Elts = v2.denseVector();
		int *v2Idx = v2.getIndices();
		int nvarreal = dims.inner.numSecondStageVars(scen);
		if (col < nvarreal) {
			const double *WcolElts = Wcol[scen]->getElements();
			const int *WcolIdx = Wcol[scen]->getIndices();
			int nnz = 0;
			CoinBigIndex start = Wcol[scen]->getVectorFirst(col);
			CoinBigIndex end = Wcol[scen]->getVectorLast(col);
			for (CoinBigIndex q = start; q < end; q++) {
				int row = WcolIdx[q];
				v2Elts[row] = WcolElts[q];
				v2Idx[nnz++] = row;
			}
			v2.setNumElements(nnz);
		} else {
			int k = col - nvarreal;
			v2Elts[k] = -1.0;
			v2Idx[0] = k;
			v2.setNumElements(1);
		}
	}
}

void BAData::addColToVec(sparseBAVector &v, BAIndex idx, double mult) const {
	assert(0); // need to fix handling of slacks
	int scen = idx.scen;
	int col = idx.idx;
	const vector<int> &localScen = v.localScenarios();

	if (scen == -1) { // first stage
		CoinIndexedVector &v1 = v.getFirstStageVec().v;
		const double *AcolElts = Acol->getElements();
		const int *AcolIdx = Acol->getIndices();
		CoinBigIndex start = Acol->getVectorFirst(col);
		CoinBigIndex end = Acol->getVectorLast(col);
		for (CoinBigIndex q = start; q < end; q++) {
			int row = AcolIdx[q];
			v1.quickAdd(row,mult*AcolElts[q]);
		}

		for (unsigned j = 1; j < localScen.size(); j++) {
			int s = localScen[j];
			const double *TcolElts = Tcol[s]->getElements();
			const int *TcolIdx = Tcol[s]->getIndices();
			CoinIndexedVector &v2 = v.getSecondStageVec(s).v;
			start = Tcol[s]->getVectorFirst(col);
			end = Tcol[s]->getVectorLast(col);
			for (CoinBigIndex q = start; q < end; q++) {
				int row = TcolIdx[q];
				v2.quickAdd(row,mult*TcolElts[q]);
			}
		}

	} else {
		if (!ctx.assignedScenario(scen)) return;
		CoinIndexedVector &v2 = v.getSecondStageVec(scen).v;
		const double *WcolElts = Wcol[scen]->getElements();
		const int *WcolIdx = Wcol[scen]->getIndices();
		CoinBigIndex start = Wcol[scen]->getVectorFirst(col);
		CoinBigIndex end = Wcol[scen]->getVectorLast(col);
		for (CoinBigIndex q = start; q < end; q++) {
			int row = WcolIdx[q];
			v2.quickAdd(row,mult*WcolElts[q]);
		}
	}

}

// multiplies "in" vector by constraint matrix
// set elements corresponding to basic columns to zero if you want to multiply with nonbasic columns
// uses dot product with rows approach, since "in" won't be hyper-sparse
void BAData::multiply(const denseBAVector &in, sparseBAVector &out, const BAFlagVector<variableState>& s) const {

	const denseVector &in1 = in.getFirstStageVec();
	CoinIndexedVector &out1 = out.getFirstStageVec().v;
	double *out1Elts = out1.denseVector();
	int *out1Idx = out1.getIndices();
	int nvar1Real = dims.inner.numFirstStageVars();
	int ncons1 = dims.numFirstStageCons();

	const vector<int> &localScen = in.localScenarios();
	for (unsigned j = 1; j < localScen.size(); j++) {
		int scen = localScen[j];

		const denseVector &in2 = in.getSecondStageVec(scen);
		CoinIndexedVector &out2 = out.getSecondStageVec(scen).v;
		double *out2Elts = out2.denseVector();
		int *out2Idx = out2.getIndices();

		int nvar2Real = dims.inner.numSecondStageVars(scen);
		int ncons2 = dims.numSecondStageCons(scen);
		int nnz2 = 0;

		const double *WrowElts = Wrow[scen]->getElements();
		const int *WrowIdx = Wrow[scen]->getIndices();
		const double *TrowElts = Trow[scen]->getElements();
		const int *TrowIdx = Trow[scen]->getIndices();

		for (int i = 0; i < ncons2; i++) {
			double work = 0.0;
			// W part
			CoinBigIndex start = Wrow[scen]->getVectorFirst(i);
			CoinBigIndex end = Wrow[scen]->getVectorLast(i);
			for (CoinBigIndex q = start; q < end; q++) {
				int col = WrowIdx[q];
				work += WrowElts[q]*in2[col];
			}
			int slackIdx = nvar2Real + i;
			work -= in2[slackIdx];
			// T part
			start = Trow[scen]->getVectorFirst(i);
			end = Trow[scen]->getVectorLast(i);
			for (CoinBigIndex q = start; q < end; q++) {
				int col = TrowIdx[q];
				work += TrowElts[q]*in1[col];
			}
			if (abs(work) > 1e-13) {
				out2Elts[i] = work;
				out2Idx[nnz2++] = i;
			} else {
				out2Elts[i] = 0.0;
			}
		}

		out2.setNumElements(nnz2);

	}

	const double *ArowElts = Arow->getElements();
	const int *ArowIdx = Arow->getIndices();
	int nnz1 = 0;

	// A block
	for (int i = 0; i < ncons1; i++) {
		CoinBigIndex start = Arow->getVectorFirst(i);
		CoinBigIndex end = Arow->getVectorLast(i);
		for (CoinBigIndex q = start; q < end; q++) {
			int col = ArowIdx[q];
			out1Elts[i] += ArowElts[q]*in1[col];
		}
		int slackIdx = nvar1Real + i;
		out1Elts[i] -= in1[slackIdx];
		if (abs(out1Elts[i]) > 1e-13) {
			out1Idx[nnz1++] = i;
		} else {
			out1Elts[i] = 0.0;
		}
	}
	out1.setNumElements(nnz1);

}

//#define PIPSPROF

void BAData::multiplyT(const sparseBAVector &in, sparseBAVector &out) const {
	// assume out is cleared

	const CoinIndexedVector &in1 = in.getFirstStageVec().v;
	CoinIndexedVector &out1 = out.getFirstStageVec().v;
	const double *in1Elts = in1.denseVector();
	const int* in1Idx = in1.getIndices();
	int nvarReal1 = dims.inner.numFirstStageVars();
	int nnzIn1 = in1.getNumElements();

	const vector<int> &localScen = in.localScenarios();

#ifdef PIPSPROF
	MPI_Barrier(ctx.comm());
	double local_t = MPI_Wtime();
#endif

	for (unsigned i = 1; i < localScen.size(); i++) {
		int scen = localScen[i];

		const CoinIndexedVector &in2 = in.getSecondStageVec(scen).v;
		CoinIndexedVector &out2 = out.getSecondStageVec(scen).v;
		const double *in2Elts = in2.denseVector();
		const int* in2Idx = in2.getIndices();

		int nvarReal2 = dims.inner.numSecondStageVars(scen);
		int nnzIn2 = in2.getNumElements();

		const double *WrowElts = Wrow[scen]->getElements();
		const int *WrowIdx = Wrow[scen]->getIndices();
		const double *TrowElts = Trow[scen]->getElements();
		const int *TrowIdx = Trow[scen]->getIndices();

		for (int j = 0; j < nnzIn2; j++) {
			int row = in2Idx[j];
			double mult = in2Elts[row];
			// W block -- add mult*(row) to out2
			CoinBigIndex start = Wrow[scen]->getVectorFirst(row);
			CoinBigIndex end = Wrow[scen]->getVectorLast(row);
			for (CoinBigIndex q = start; q < end; q++) {
				int col = WrowIdx[q];
				out2.quickAdd(col,mult*WrowElts[q]);
			}
			// slack, guaranteed to be zero in "out" until now,
			int slackIdx = nvarReal2 + row;
			out2.quickInsert(slackIdx,-mult);

			// T block -- goes into out1
			start = Trow[scen]->getVectorFirst(row);
			end = Trow[scen]->getVectorLast(row);
			for (CoinBigIndex q = start; q < end; q++) {
				int col = TrowIdx[q];
				out1Send.quickAdd(col,mult*TrowElts[q]);
			}
		}
	}
#ifdef PIPSPROF
	local_t = MPI_Wtime() - local_t;
	MPI_Barrier(ctx.comm());
	double comm_t = MPI_Wtime();
#endif

	MPI_Allreduce(out1Send.denseVector(), out1.denseVector(), dims.numFirstStageVars(), MPI_DOUBLE, MPI_SUM, ctx.comm());
#ifdef PIPSPROF
	comm_t = MPI_Wtime() - comm_t;
	double first_t = MPI_Wtime();
#endif

	out1Send.clear();

	// more efficient way to do it if T blocks are all the same

	// A block
	const double *ArowElts = Arow->getElements();
	const int *ArowIdx = Arow->getIndices();
	double *out1Elts = out1.denseVector();
	for (int j = 0; j < nnzIn1; j++) {
		int row = in1Idx[j];
		double mult = in1Elts[row];
		CoinBigIndex start = Arow->getVectorFirst(row);
		CoinBigIndex end = Arow->getVectorLast(row);
		for (CoinBigIndex q = start; q < end; q++) {
			int col = ArowIdx[q];
			out1Elts[col] += mult*ArowElts[q];
		}

		// slack
		int slackIdx = nvarReal1 + row;
		out1Elts[slackIdx] = -mult;

	}
	out1.scan(1e-13);

#ifdef PIPSPROF
	first_t = MPI_Wtime() - first_t;
	double totallocaltime;
	double maxlocaltime;
	MPI_Reduce(&local_t,&totallocaltime,1,MPI_DOUBLE,MPI_SUM,0,ctx.comm());
	MPI_Reduce(&local_t,&maxlocaltime,1,MPI_DOUBLE,MPI_MAX,0,ctx.comm());
	int nproc = ctx.nprocs();
	double idealtotal = totallocaltime/nproc + first_t;
	if (ctx.mype() == 0) {
		printf("PRICE ideal total: %g actual total: %g load imbalance: %f comm. cost: %f 1st stage cost: %f\n",
			idealtotal,maxlocaltime+comm_t + first_t,
			100.*(maxlocaltime-totallocaltime/nproc)/idealtotal,
			100.*(comm_t)/idealtotal,
			100.*(first_t)/idealtotal);
	}

#endif
}

void BAData::addSecondStageRow(const CoinPackedVectorBase& elts1, const CoinPackedVectorBase &elts2, int scen, double lb, double ub) {

	assert(scen >= 0 && scen < dims.numScenarios());
	if (ctx.assignedScenario(scen)) {
		Trow[scen]->appendRow(elts1);
		Tcol[scen]->reverseOrderedCopyOf(*Trow[scen]);
		Wrow[scen]->appendRow(elts2);
		Wcol[scen]->reverseOrderedCopyOf(*Wrow[scen]);

		int nvar2 = dims.inner.numSecondStageVars(scen);
		int ncons2 = dims.numSecondStageCons(scen);
		denseVector newL(nvar2+ncons2+1);
		denseVector &oldL = l.getSecondStageVec(scen);
		newL.copyBeginning(&oldL[0],nvar2+ncons2);
		newL[nvar2+ncons2] = lb;
		oldL.swap(newL);

		denseVector newU(nvar2+ncons2+1);
		denseVector &oldU = u.getSecondStageVec(scen);
		newU.copyBeginning(&oldU[0],nvar2+ncons2);
		newU[nvar2+ncons2] = ub;
		oldU.swap(newU);

		denseVector newC(nvar2+ncons2+1);
		denseVector &oldC = c.getSecondStageVec(scen);
		newC.copyBeginning(&oldC[0],nvar2+ncons2);
		newC[nvar2+ncons2] = 0.0;
		oldC.swap(newC);

		dims.inner.addSecondStageRow(scen);

		assert(oldU.length() == nvar2+ncons2+1);
		assert(oldL.length() == nvar2+ncons2+1);

	}

	vartype.deallocate();
	names.deallocate();

	vartype.allocate(dims, ctx, PrimalVector);

	// we're dropping the names here by not copying them
	// TODO: fix this
	names.allocate(dims, ctx, PrimalVector);

	const vector<int> &localScen = ctx.localScenarios();
	for (unsigned i = 0; i < localScen.size(); i++) {
		int scen = localScen[i];
		checkConstraintType(l.getVec(scen),u.getVec(scen),vartype.getVec(scen));
	}

}

	const CoinShallowPackedVector BAData::retrieveARow(int index)const{
		return Arow->getVector(index);

	}

	const CoinShallowPackedVector BAData::retrieveWRow(int index,int scen)const {
		return Wrow[scen]->getVector(index);

	}

	const CoinShallowPackedVector BAData::retrieveTRow(int index,int scen)const{
		return Trow[scen]->getVector(index);

	}

	const CoinShallowPackedVector BAData::retrieveACol(int index)const{
		return Acol->getVector(index);
	}

	const CoinShallowPackedVector BAData::retrieveWCol(int index,int scen)const{
		return Wcol[scen]->getVector(index);

	}

	const CoinShallowPackedVector BAData::retrieveTCol (int index,int scen)const{
		return Tcol[scen]->getVector(index);

	}

	void BAData::addSecondStageConsecutiveRows(const std::vector<CoinPackedVector*> &v1, const std::vector<CoinPackedVector*> &v2, int scenario, std::vector<double> &lb, std::vector<double> &ub, int nRows){

	CoinPackedVectorBase * const * elts1= (CoinPackedVectorBase * const *) &v1[0];
	CoinPackedVectorBase * const * elts2= (CoinPackedVectorBase * const *) &v2[0];
	assert(scenario >= 0 && scenario < dims.numScenarios());
	if (ctx.assignedScenario(scenario)) {
		Trow[scenario]->appendRows(nRows,elts1);
		Tcol[scenario]->reverseOrderedCopyOf(*Trow[scenario]);
		Wrow[scenario]->appendRows(nRows,elts2);
		Wcol[scenario]->reverseOrderedCopyOf(*Wrow[scenario]);

		int nvar2 = dims.inner.numSecondStageVars(scenario);
		int ncons2 = dims.numSecondStageCons(scenario);
		denseVector newL(nvar2+ncons2+nRows);
		denseVector &oldL = l.getSecondStageVec(scenario);
		newL.copyBeginning(&oldL[0],nvar2+ncons2);
		for(int i=0; i< nRows; i++)	newL[nvar2+ncons2+i] = lb[i];
		oldL.swap(newL);

		denseVector newU(nvar2+ncons2+nRows);
		denseVector &oldU = u.getSecondStageVec(scenario);
		newU.copyBeginning(&oldU[0],nvar2+ncons2);
		for(int i=0; i< nRows; i++) newU[nvar2+ncons2+i] = ub[i];
		oldU.swap(newU);

		denseVector newC(nvar2+ncons2+nRows);
		denseVector &oldC = c.getSecondStageVec(scenario);
		newC.copyBeginning(&oldC[0],nvar2+ncons2);
		for(int i=0; i< nRows; i++) newC[nvar2+ncons2+i] = 0.0;
		oldC.swap(newC);

		for(int i=0; i< nRows; i++) dims.inner.addSecondStageRow(scenario);

		assert(oldU.length() == nvar2+ncons2+nRows);
		assert(oldL.length() == nvar2+ncons2+nRows);

	}
	vartype.deallocate();
	names.deallocate();

	vartype.allocate(dims, ctx, PrimalVector);

	// we're dropping the names here by not copying them
	// TODO: fix this
	names.allocate(dims, ctx, PrimalVector);

	const vector<int> &localScen = ctx.localScenarios();
	for (unsigned i = 0; i < localScen.size(); i++) {
		int scen = localScen[i];
		checkConstraintType(l.getVec(scen),u.getVec(scen),vartype.getVec(scen));
	}
}

void BAData::addFirstStageRow(const CoinPackedVectorBase& elts1, double lb, double ub) {

	assert(lb<=ub);
	Arow->appendRow(elts1);
	Acol->reverseOrderedCopyOf(*Arow);
	int nvar = dims.inner.numFirstStageVars();
	int ncons = dims.numFirstStageCons();
	denseVector newL(nvar+ncons+1);
	denseVector &oldL = l.getFirstStageVec();
	newL.copyBeginning(&oldL[0],nvar+ncons);
	newL[nvar+ncons] = lb;
	oldL.swap(newL);
	denseVector newU(nvar+ncons+1);
	denseVector &oldU = u.getFirstStageVec();
	newU.copyBeginning(&oldU[0],nvar+ncons);
	newU[nvar+ncons] = ub;
	oldU.swap(newU);
	denseVector newC(nvar+ncons+1);
	denseVector &oldC = c.getFirstStageVec();
	newC.copyBeginning(&oldC[0],nvar+ncons);
	newC[nvar+ncons] = 0.0;
	oldC.swap(newC);
	dims.inner.addFirstStageRow();
	assert(oldU.length() == nvar+ncons+1);
	assert(oldL.length() == nvar+ncons+1);
	vartype.deallocate();
	names.deallocate();
	vartype.allocate(dims, ctx, PrimalVector);

	// we're dropping the names here by not copying them
	// TODO: fix this
	names.allocate(dims, ctx, PrimalVector);
	const vector<int> &localScen = ctx.localScenarios();
	for (unsigned i = 0; i < localScen.size(); i++) {
		int scen = localScen[i];
		checkConstraintType(l.getVec(scen),u.getVec(scen),vartype.getVec(scen));
	}
	out1Send.reserve(dims.numFirstStageVars());

}

void BAData::addFirstStageRows(const std::vector<CoinPackedVector*> &v1, std::vector<double> &lb, std::vector<double> &ub, int nRows) {

	CoinPackedVectorBase * const * elts1= (CoinPackedVectorBase * const *) &v1[0];
	assert(lb.size()==ub.size() && lb.size()==nRows && nRows>0);
	Arow->appendRows(nRows,elts1);
	Acol->reverseOrderedCopyOf(*Arow);

	int nvar2 = dims.inner.numFirstStageVars();
	int ncons2 = dims.numFirstStageCons();
	denseVector newL(nvar2+ncons2+nRows);
	denseVector &oldL = l.getFirstStageVec();
	newL.copyBeginning(&oldL[0],nvar2+ncons2);
	for(int i=0; i< nRows; i++) newL[nvar2+ncons2+i] = lb[i];
	oldL.swap(newL);
	denseVector newU(nvar2+ncons2+nRows);
	denseVector &oldU = u.getFirstStageVec();
	newU.copyBeginning(&oldU[0],nvar2+ncons2);
	for(int i=0; i< nRows; i++)newU[nvar2+ncons2+i] = ub[i];
	oldU.swap(newU);
	denseVector newC(nvar2+ncons2+nRows);
	denseVector &oldC = c.getFirstStageVec();
	newC.copyBeginning(&oldC[0],nvar2+ncons2);
	for(int i=0; i< nRows; i++)newC[nvar2+ncons2+i] = 0.0;
	oldC.swap(newC);

	for(int i=0; i< nRows; i++)dims.inner.addFirstStageRow();

	assert(oldU.length() == nvar2+ncons2+nRows);
	assert(oldL.length() == nvar2+ncons2+nRows);
	vartype.deallocate();
	names.deallocate();
	vartype.allocate(dims, ctx, PrimalVector);

	// we're dropping the names here by not copying them
	// TODO: fix this
	names.allocate(dims, ctx, PrimalVector);
	const vector<int> &localScen = ctx.localScenarios();
	for (unsigned i = 0; i < localScen.size(); i++) {
		int scen = localScen[i];
		checkConstraintType(l.getVec(scen),u.getVec(scen),vartype.getVec(scen));
	}
	out1Send.reserve(dims.numFirstStageVars());

}

int BAData::addFirstStageColumn( double lb, double ub, double cobj){

	vector<double> elems(Acol->getMinorDim(),0);
	CoinPackedVector elts;
	elts.setFullNonZero(elems.size(),&elems[0]);

	//Assertions
	assert(lb<=ub);

	Acol->appendCol(elts);
	Arow->reverseOrderedCopyOf(*Acol);

	for (int scen=0; scen < Tcol.size(); scen++){
		if (!ctx.assignedScenario(scen)) continue;
		Tcol[scen]->appendCol(elts);
		Trow[scen]->reverseOrderedCopyOf(*Tcol[scen]);
	}

	//increase the size of l, u, c,
	int nvar2 = dims.inner.numFirstStageVars();
	int ncons2 = dims.numFirstStageCons();
	denseVector newL(nvar2+ncons2+1);
	denseVector &oldL = l.getFirstStageVec();
	newL.copyBeginning(&oldL[0],nvar2);
	newL.copyToPosition(&oldL[nvar2],nvar2+1,ncons2);
	newL[nvar2] = lb;
	oldL.swap(newL);

	denseVector newU(nvar2+ncons2+1);
	denseVector &oldU = u.getFirstStageVec();
	newU.copyBeginning(&oldU[0],nvar2);
	newU.copyToPosition(&oldU[nvar2],nvar2+1,ncons2);
	newU[nvar2] = ub;
	oldU.swap(newU);

	denseVector newC(nvar2+ncons2+1);
	denseVector &oldC = c.getFirstStageVec();

	newC.copyBeginning(&oldC[0],nvar2);
	newC.copyToPosition(&oldC[nvar2],nvar2+1,ncons2);
	newC[nvar2] = cobj;
	oldC.swap(newC);

	assert(oldU.length() == nvar2+ncons2+1);
	assert(oldL.length() == nvar2+ncons2+1);

	//Update dims
	dims.inner.addFirstStageVar();

	//check constraint type?
	vartype.deallocate();
	names.deallocate();

	vartype.allocate(dims, ctx, PrimalVector);
	// we're dropping the names here by not copying them
	// TODO: fix this
	names.allocate(dims, ctx, PrimalVector);

	assert(l.getFirstStageVec().length()==u.getFirstStageVec().length());

	const vector<int> &localScen = ctx.localScenarios();

	for (unsigned i = 0; i < localScen.size(); i++) {
		int scen = localScen[i];
		checkConstraintType(l.getVec(scen),u.getVec(scen),vartype.getVec(scen));
	}
	out1Send.reserve(dims.numFirstStageVars());

	return nvar2;
}

int BAData::addSecondStageColumn(int scen,double lb, double ub, double cobj){

	assert(scen >= 0 && scen < dims.numScenarios());
	int returnIndex= -1;
	if (ctx.assignedScenario(scen)) {

		vector<double> elems(Wcol[scen]->getMinorDim(),0);
		CoinPackedVector elts;
		elts.setFullNonZero(elems.size(),&elems[0]);

		//Assertions
		assert(lb<=ub);

		Wcol[scen]->appendCol(elts);
		Wrow[scen]->reverseOrderedCopyOf(*Wcol[scen]);

		int nvar2 = dims.inner.numSecondStageVars(scen);
		int ncons2 = dims.numSecondStageCons(scen);
		denseVector newL(nvar2+ncons2+1);
		denseVector &oldL = l.getSecondStageVec(scen);
		newL.copyBeginning(&oldL[0],nvar2);
		newL.copyToPosition(&oldL[nvar2],nvar2+1,ncons2);
		newL[nvar2] = lb;
		oldL.swap(newL);

		denseVector newU(nvar2+ncons2+1);
		denseVector &oldU = u.getSecondStageVec(scen);
		newU.copyBeginning(&oldU[0],nvar2);
		newU.copyToPosition(&oldU[nvar2],nvar2+1,ncons2);
		newU[nvar2] = ub;
		oldU.swap(newU);

		denseVector newC(nvar2+ncons2+1);
		denseVector &oldC = c.getSecondStageVec(scen);
		newC.copyBeginning(&oldC[0],nvar2);
		newC.copyToPosition(&oldC[nvar2],nvar2+1,ncons2);
		newC[nvar2] = cobj;
		oldC.swap(newC);

		dims.inner.addSecondStageVar(scen);

		assert(oldU.length() == nvar2+ncons2+1);
		assert(oldL.length() == nvar2+ncons2+1);
		returnIndex=nvar2;
	}

	vartype.deallocate();
	names.deallocate();

	vartype.allocate(dims, ctx, PrimalVector);

	// we're dropping the names here by not copying them
	// TODO: fix this
	names.allocate(dims, ctx, PrimalVector);

	const vector<int> &localScen = ctx.localScenarios();
	for (unsigned i = 0; i < localScen.size(); i++) {
		int scen = localScen[i];
		checkConstraintType(l.getVec(scen),u.getVec(scen),vartype.getVec(scen));
	}
	return returnIndex;
}

void BAData::deleteLastFirstStageRows(int nRows) {

	assert(nRows>=0 && nRows<dims.numFirstStageCons());
	int rowStart=dims.numFirstStageCons()-nRows;
	vector<int> indices(nRows);
	for (int i=0; i<indices.size();i++)indices[i]=rowStart+i;
	Arow->deleteRows(nRows,&indices[0]);
	Acol->reverseOrderedCopyOf(*Arow);

	int nvar2 = dims.inner.numFirstStageVars();
	int ncons2 = dims.numFirstStageCons();
	denseVector newL(nvar2+ncons2-nRows);
	denseVector &oldL = l.getFirstStageVec();
	newL.copyBeginning(&oldL[0],nvar2+rowStart);
	oldL.swap(newL);

	denseVector newU(nvar2+ncons2-nRows);
	denseVector &oldU = u.getFirstStageVec();
	newU.copyBeginning(&oldU[0],nvar2+rowStart);
	oldU.swap(newU);

	denseVector newC(nvar2+ncons2-nRows);
	denseVector &oldC = c.getFirstStageVec();
	newC.copyBeginning(&oldC[0],nvar2+rowStart);
	oldC.swap(newC);

	for(int i=0; i< nRows; i++)dims.inner.removeFirstStageRow();

	assert(oldU.length() == nvar2+rowStart);
	assert(oldL.length() == nvar2+rowStart);

	vartype.deallocate();
	names.deallocate();

	vartype.allocate(dims, ctx, PrimalVector);

	// we're dropping the names here by not copying them
	// TODO: fix this
	names.allocate(dims, ctx, PrimalVector);

	const vector<int> &localScen = ctx.localScenarios();
	for (unsigned i = 0; i < localScen.size(); i++) {
		int scen = localScen[i];
		checkConstraintType(l.getVec(scen),u.getVec(scen),vartype.getVec(scen));
	}
	out1Send.reserve(dims.numFirstStageVars());

}

void BAData::deleteLastSecondStageConsecutiveRows(int scenario, int nRows){

	assert(scenario >= 0 && scenario < dims.numScenarios());
	if (ctx.assignedScenario(scenario)) {
		assert(nRows>=0 && nRows<dims.numSecondStageCons(scenario));

		int rowStart=dims.numSecondStageCons(scenario)-nRows;

		vector<int> indices(nRows);
		for (int i=0; i<indices.size();i++)indices[i]=rowStart+i;

		Trow[scenario]->deleteRows(nRows,&indices[0]);
		Tcol[scenario]->reverseOrderedCopyOf(*Trow[scenario]);
		Wrow[scenario]->deleteRows(nRows,&indices[0]);
		Wcol[scenario]->reverseOrderedCopyOf(*Wrow[scenario]);

		int nvar2 = dims.inner.numSecondStageVars(scenario);
		int ncons2 = dims.numSecondStageCons(scenario);
		denseVector newL(nvar2+ncons2-nRows);
		denseVector &oldL = l.getSecondStageVec(scenario);
		newL.copyBeginning(&oldL[0],nvar2+rowStart);
		oldL.swap(newL);

		denseVector newU(nvar2+ncons2-nRows);
		denseVector &oldU = u.getSecondStageVec(scenario);
		newU.copyBeginning(&oldU[0],nvar2+rowStart);
		oldU.swap(newU);

		denseVector newC(nvar2+ncons2-nRows);
		denseVector &oldC = c.getSecondStageVec(scenario);
		newC.copyBeginning(&oldC[0],nvar2+rowStart);
		oldC.swap(newC);

		for(int i=0; i< nRows; i++) dims.inner.removeSecondStageRow(scenario);
		assert(oldU.length() == nvar2+rowStart);
		assert(oldL.length() == nvar2+rowStart);
	}

	vartype.deallocate();
	names.deallocate();

	vartype.allocate(dims, ctx, PrimalVector);

	// we're dropping the names here by not copying them
	// TODO: fix this
	names.allocate(dims, ctx, PrimalVector);

	const vector<int> &localScen = ctx.localScenarios();
	for (unsigned i = 0; i < localScen.size(); i++) {
		int scen = localScen[i];
		checkConstraintType(l.getVec(scen),u.getVec(scen),vartype.getVec(scen));
	}

}

void BAData::deleteLastFirstStageColumns(int nCols){

	int ncons = dims.numFirstStageCons();
	int nvar = dims.inner.numFirstStageVars();
	//TODO add guard to make sure we don't delete a column that still has nonzero coefficients in the constraint matrix
	assert(nCols<=nvar);
	int colStart=dims.inner.numFirstStageVars()-nCols;
	vector<int> indices(nCols);
	for (int i=0; i<indices.size();i++)indices[i]=colStart+i;
	Acol->deleteCols(nCols,&indices[0]);
	Arow->reverseOrderedCopyOf(*Acol);

	for (int scen=0; scen < Tcol.size(); scen++){
		if (!ctx.assignedScenario(scen)) continue;
		Tcol[scen]->deleteCols(nCols,&indices[0]);
		Trow[scen]->reverseOrderedCopyOf(*Tcol[scen]);
	}

	//Shrink l, u, and c
	denseVector newL(nvar+ncons-nCols);
	denseVector &oldL = l.getFirstStageVec();
	newL.copyBeginning(&oldL[0],nvar-nCols);
	newL.copyToPosition(&oldL[nvar],nvar-nCols,ncons);
	oldL.swap(newL);

	denseVector newU(nvar+ncons-nCols);
	denseVector &oldU = u.getFirstStageVec();
	newU.copyBeginning(&oldU[0],nvar-nCols);
	newU.copyToPosition(&oldU[nvar],nvar-nCols,ncons);
	oldU.swap(newU);

	denseVector newC(nvar+ncons-nCols);
	denseVector &oldC = c.getFirstStageVec();
	newC.copyBeginning(&oldC[0],nvar-nCols);
	newC.copyToPosition(&oldC[nvar],nvar-nCols,ncons);
	oldC.swap(newC);

	assert(oldU.length() == nvar+ncons-nCols);
	assert(oldL.length() == nvar+ncons-nCols);

	//Update dims
	for (int i=0; i< nCols; i++)dims.inner.removeFirstStageVar();

	//check constraint type?
	vartype.deallocate();
	names.deallocate();

	vartype.allocate(dims, ctx, PrimalVector);
	// we're dropping the names here by not copying them
	// TODO: fix this
	names.allocate(dims, ctx, PrimalVector);

	assert(l.getFirstStageVec().length()==u.getFirstStageVec().length());

	const vector<int> &localScen = ctx.localScenarios();
	for (unsigned i = 0; i < localScen.size(); i++) {
		int scen = localScen[i];
		checkConstraintType(l.getVec(scen),u.getVec(scen),vartype.getVec(scen));
	}

	out1Send.reserve(dims.numFirstStageVars());

}

void BAData::deleteLastSecondStageConsecutiveColumns(int scen, int nCols){

	assert(scen >= 0 && scen < dims.numScenarios());

	if (ctx.assignedScenario(scen)) {

		int nvar2 = dims.inner.numSecondStageVars(scen);
		int ncons2 = dims.numSecondStageCons(scen);
		//TODO add guard to make sure we don't delete a column that still has nonzero coefficients in the constraint matrix
		assert(nCols<=nvar2);

		int colStart=dims.inner.numSecondStageVars(scen)-nCols;

		vector<int> indices(nCols);
		for (int i=0; i<indices.size();i++)indices[i]=colStart+i;

		Wcol[scen]->deleteCols(nCols,&indices[0]);
		Wrow[scen]->reverseOrderedCopyOf(*Wcol[scen]);

		//Shrink l, u, and c
		denseVector newL(nvar2+ncons2-nCols);
		denseVector &oldL = l.getSecondStageVec(scen);
		newL.copyBeginning(&oldL[0],nvar2-nCols);
		newL.copyToPosition(&oldL[nvar2],nvar2-nCols,ncons2);
		oldL.swap(newL);

		denseVector newU(nvar2+ncons2-nCols);
		denseVector &oldU = u.getSecondStageVec(scen);
		newU.copyBeginning(&oldU[0],nvar2-nCols);
		newU.copyToPosition(&oldU[nvar2],nvar2-nCols,ncons2);
		oldU.swap(newU);

		denseVector newC(nvar2+ncons2-nCols);
		denseVector &oldC = c.getSecondStageVec(scen);
		newC.copyBeginning(&oldC[0],nvar2-nCols);
		newC.copyToPosition(&oldC[nvar2],nvar2-nCols,ncons2);
		oldC.swap(newC);

		assert(oldU.length() == nvar2+ncons2-nCols);
		assert(oldL.length() == nvar2+ncons2-nCols);

		//Update dims
		for (int i=0; i< nCols; i++)dims.inner.removeSecondStageVar(scen);

	}
	//check constraint type?
	vartype.deallocate();
	names.deallocate();

	vartype.allocate(dims, ctx, PrimalVector);
	// we're dropping the names here by not copying them
	// TODO: fix this
	names.allocate(dims, ctx, PrimalVector);

	assert(l.getSecondStageVec(scen).length()==u.getSecondStageVec(scen).length());

	const vector<int> &localScen = ctx.localScenarios();
	for (unsigned i = 0; i < localScen.size(); i++) {
		int scen = localScen[i];
		checkConstraintType(l.getVec(scen),u.getVec(scen),vartype.getVec(scen));
	}

}