#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <std_msgs/Int32.h>
#include <std_msgs/String.h>
#include <fstream>
#include <ros_control_boilerplate/MatchSpecificData.h>
#include <frc_msgs/PDPData.h>
#include <boost/lexical_cast.hpp>
#include <boost/foreach.hpp>
#define foreach BOOST_FOREACH

int main(int argc, char **argv)
{
	std::string bag_name = argv[1];

	std::string c_string = argv[2];
	int current_needed = boost::lexical_cast<int>(c_string);
	std::cout << current_needed;

	rosbag::Bag bag;
	bag.open(bag_name, rosbag::bagmode::Read);

	std::ofstream temp_file;
	temp_file.open ("temp_file.txt");

	std::ofstream times;
	times.open ("times.txt");

	std::vector<std::string> topics;
	topics.push_back(std::string("/frcrobot/pdp_states"));

	rosbag::View view(bag, rosbag::TopicQuery(topics));

	frc_msgs::PDPData::ConstPtr s = view[0].instantiate<frc_msgs::PDPData>();
	if (s != NULL)
		int start_time = floor(s->header.stamp.toSec());

	foreach(rosbag::MessageInstance const m, view)
	{
		frc_msgs::PDPData::ConstPtr s = m.instantiate<frc_msgs::PDPData>();
		if (s != NULL){
			temp_file << "current at " << current_needed << ": " <<  s->current[current_needed] << std::endl;
			if  (s->current[current_needed] > 0)
					times << "current at channel " << current_needed << " is significant at header " << floor(s->header.stamp.toSec()) - start_time << std::endl;
		}

	}
	bag.close();

	return 0;
}


//
// Copyright © 2017 Arm Ltd. All rights reserved.
// SPDX-License-Identifier: MIT
//

#include <boost/test/unit_test.hpp>
#include "armnnTfParser/ITfParser.hpp"
#include "ParserPrototxtFixture.hpp"

BOOST_AUTO_TEST_SUITE(TensorflowParser)

struct SplitFixture : public armnnUtils::ParserPrototxtFixture<armnnTfParser::ITfParser>
{
    SplitFixture(bool withDimZero=false) {
        m_Prototext = R"(
        node {
          name: "graphInput"
          op: "Placeholder"
          attr {
            key: "dtype"
            value {
              type: DT_FLOAT
            }
          }
          attr {
            key: "shape"
            value {
              shape {
              }
            }
          }
        }
        node {
          name: "graphInput2"
          op: "Placeholder"
          attr {
            key: "dtype"
            value {
              type: DT_FLOAT
            }
          }
          attr {
            key: "shape"
            value {
              shape {
              }
            }
          }
        }
        node {
        name: "multiplication"
        op : "Mul"
        input: "graphInput"
        input: "graphInput2"
        attr {
        key: "T"
        value {
            type: DT_FLOAT
        }
        }
        }
        node {
          name: "SplitInput"
          op: "Const"
          attr {
            key: "dtype"
            value {
              type: DT_INT32
            }
          }
          attr {
            key: "value"
            value {
              tensor {
                dtype: DT_INT32
                tensor_shape {
                }
                int_val: )";

        if(withDimZero)
        {
            m_Prototext += std::to_string(3);
        }
        else
        {
            m_Prototext += std::to_string(1);
        }

        m_Prototext += R"(
        }
        }
        }
        }
        node {
          name: "Split"
          op: "Split" )";
        if(withDimZero)
        {
            m_Prototext += "input: \"SplitInput\"\n";
            m_Prototext += "input: \"multiplication\"\n";
        }
        else
        {
            m_Prototext += "input: \"graphInput\"\n";
            m_Prototext += "input: \"SplitInput\"\n";
        }
        m_Prototext += R"(
          attr {
            key: "num_split"
            value {
              i: 2
            }
          }
        }
        node {
            name: "Relu_1"
            op: "Relu"
            input: "Split:0"
            attr {
            key: "T"
            value {
            type: DT_FLOAT
             }
            }
            }
         node {
            name: "Relu_2"
            op: "Relu"
            input:"Split:1"
            attr {
            key: "T"
            value {
            type: DT_FLOAT
             }
            }
            } )";

        Setup( { { "graphInput", { 1,  2,  2 , 2} } , { "graphInput2", { 1,  2,  2 , 2} }},
               { "Relu_1", "Relu_2" });
    }
};

struct InputFirstSplitFixture : SplitFixture
{
    InputFirstSplitFixture() : SplitFixture(true) {}
};

BOOST_FIXTURE_TEST_CASE(ParseAxisOneSplitTwo, SplitFixture)
{
    BOOST_TEST(
        (m_Parser->GetNetworkOutputBindingInfo("Relu_1").second.GetShape() == armnn::TensorShape({ 1, 1, 2, 2 })));

    BOOST_TEST(
        (m_Parser->GetNetworkOutputBindingInfo("Relu_2").second.GetShape() == armnn::TensorShape({ 1, 1, 2, 2 })));

    RunTest<4>({ { "graphInput", { -1.0f, -0.5f, 1.25f, -3.0f, 0.0f, 0.5f, -0.75f, 1.75f } } },
               { { "Relu_1", { 0.0f, 0.0f, 1.25f, 0.0f } },
                 { "Relu_2", { 0.0f, 0.5f, 0.0f, 1.75f } } });
}

BOOST_FIXTURE_TEST_CASE(ParseSplit, InputFirstSplitFixture)
{

    BOOST_TEST(
            (m_Parser->GetNetworkOutputBindingInfo("Relu_1").second.GetShape() == armnn::TensorShape({ 1, 2, 2, 1 })));

    BOOST_TEST(
            (m_Parser->GetNetworkOutputBindingInfo("Relu_2").second.GetShape() == armnn::TensorShape({ 1, 2, 2, 1 })));

    RunTest<4>({ { "graphInput", { -1.0f, -0.5f, 1.25f, -3.0f, 0.0f, 0.5f, -0.75f , 1.75f } } ,
                 { "graphInput2", { -1.0f, -0.5f, 1.25f, -3.0f, 0.0f, 0.5f, -0.75f , 1.75f } } },
               { { "Relu_1", { 1.0f, 1.5625f, 0, 0.5625f } },
                 { "Relu_2", { 0.25, 9.0f, 0.25f, 3.0625f } } });
}

BOOST_AUTO_TEST_SUITE_END()


/**
* @file dwa/main.cpp
* @brief Dynamic Window Approach¶ algorithm sample
* @author Shunya Hara
* @date 2022.3.16
* @details Dynamic Window Approach¶ algorithm sample (c++)
* @ref https://pythonrobotics.readthedocs.io/en/latest/modules/path_planning/path_planning.html#dynamic-window-approach
**/

#include <iostream>
#include <cmath>
#include <string>

#include <Eigen/Eigen>
#include <matplotlib-cpp/matplotlibcpp.h>

#include "include/DWA.h"
#include "include/Position.h"

namespace plt = matplotlibcpp;

int main() {

    Position robot_pos(0.0, 0.0);
    Position goal_pos(1.0, 1.0);
    std::vector<Position> map;
    DWA dwa;

    dwa.calc(map,robot_pos,goal_pos);

    // show plots
    plt::show();
    return 0;
}


/*
    This file is part of Mitsuba, a physically based rendering system.

    Copyright (c) 2007-2014 by Wenzel Jakob and others.

    Mitsuba is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License Version 3
    as published by the Free Software Foundation.

    Mitsuba is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program. If not, see <http://www.gnu.org/licenses/>.
*/

#include <mitsuba/core/chisquare.h>
#include <mitsuba/core/quad.h>
#include <mitsuba/core/timer.h>
#include <boost/math/distributions/chi_squared.hpp>
#include <boost/bind.hpp>
#include <boost/filesystem/fstream.hpp>
#include <set>

MTS_NAMESPACE_BEGIN

/* Simple ordering for storing vectors in a set */
struct VectorOrder {
    inline int compare(const Vector &v1, const Vector &v2) const {
        if (v1.x < v2.x) return -1;
        else if (v1.x > v2.x) return 1;
        if (v1.y < v2.y) return -1;
        else if (v1.y > v2.y) return 1;
        if (v1.z < v2.z) return -1;
        else if (v1.z > v2.z) return 1;
        return 0;
    }

    bool operator()(const Vector &v1, const Vector &v2) const {
        return compare(v1, v2) < 0;
    }
};

ChiSquare::ChiSquare(int thetaBins, int phiBins, int numTests,
        size_t sampleCount) : m_logLevel(EInfo), m_thetaBins(thetaBins),
          m_phiBins(phiBins), m_numTests(numTests), m_sampleCount(sampleCount) {
    if (m_phiBins == 0)
        m_phiBins = 2*m_thetaBins;
    if (m_sampleCount == 0)
        m_sampleCount = m_thetaBins * m_phiBins * 1000;
    m_table = new Float[m_thetaBins*m_phiBins];
    m_refTable = new Float[m_thetaBins*m_phiBins];
    m_tolerance = m_sampleCount * 1e-4f;
}

ChiSquare::~ChiSquare() {
    delete[] m_table;
    delete[] m_refTable;
}

void ChiSquare::dumpTables(const fs::path &filename) {
    fs::ofstream out(filename);
    out << "tbl_counts = [ ";
    for (int i=0; i<m_thetaBins; ++i) {
        for (int j=0; j<m_phiBins; ++j) {
            out << m_table[i*m_phiBins+j];
            if (j+1 < m_phiBins)
                out << ", ";
        }
        if (i+1 < m_thetaBins)
            out << "; ";
    }
    out << " ];" << endl
        << "tbl_ref = [ ";
    for (int i=0; i<m_thetaBins; ++i) {
        for (int j=0; j<m_phiBins; ++j) {
            out << m_refTable[i*m_phiBins+j];
            if (j+1 < m_phiBins)
                out << ", ";
        }
        if (i+1 < m_thetaBins)
            out << "; ";
    }
    out << " ];" << endl;
    out.close();
}

void ChiSquare::fill(
    const boost::function<boost::tuple<Vector, Float, EMeasure>()> &sampleFn,
    const boost::function<Float (const Vector &, EMeasure measure)> &pdfFn) {
    memset(m_table, 0, m_thetaBins*m_phiBins*sizeof(Float));
    memset(m_refTable, 0, m_thetaBins*m_phiBins*sizeof(Float));

    Log(m_logLevel, "Accumulating " SIZE_T_FMT " samples into a %ix%i"
            " contingency table", m_sampleCount, m_thetaBins, m_phiBins);
    Point2 factor(m_thetaBins / M_PI, m_phiBins / (2*M_PI));

    std::set<Vector, VectorOrder> discreteDirections;

    ref<Timer> timer = new Timer();
    for (size_t i=0; i<m_sampleCount; ++i) {
        boost::tuple<Vector, Float, EMeasure> sample = sampleFn();
        Point2 sphCoords = toSphericalCoordinates(boost::get<0>(sample));

        int thetaBin = std::min(std::max(0,
            math::floorToInt(sphCoords.x * factor.x)), m_thetaBins-1);
        int phiBin = std::min(std::max(0,
            math::floorToInt(sphCoords.y * factor.y)), m_phiBins-1);
        m_table[thetaBin * m_phiBins + phiBin] += boost::get<1>(sample);
        if (boost::get<1>(sample) > 0 && boost::get<2>(sample) == EDiscrete)
            discreteDirections.insert(boost::get<0>(sample));
    }

    if (discreteDirections.size() > 0) {
        Log(EDebug, "Incorporating the disrete density over "
            SIZE_T_FMT " direction(s) into the contingency table", discreteDirections.size());
        for (std::set<Vector, VectorOrder>::const_iterator it = discreteDirections.begin();
            it != discreteDirections.end(); ++it) {
            const Vector &direction = *it;
            Point2 sphCoords = toSphericalCoordinates(direction);
            Float pdf = pdfFn(direction, EDiscrete);

            int thetaBin = std::min(std::max(0,
                math::floorToInt(sphCoords.x * factor.x)), m_thetaBins-1);
            int phiBin = std::min(std::max(0,
                math::floorToInt(sphCoords.y * factor.y)), m_phiBins-1);

            m_refTable[thetaBin * m_phiBins + phiBin] += pdf * m_sampleCount;
        }
    }

    factor = Point2(M_PI / m_thetaBins, (2*M_PI) / m_phiBins);

    Log(m_logLevel, "Done, took %i ms. Integrating reference "
        "contingency table ..", timer->getMilliseconds());
    timer->reset();
    Float min[2], max[2];
    size_t idx = 0;

    NDIntegrator integrator(1, 2, 100000, 0, 1e-6f);
    Float maxError = 0, integral = 0;
    for (int i=0; i<m_thetaBins; ++i) {
        min[0] = i * factor.x;
        max[0] = (i+1) * factor.x;
        for (int j=0; j<m_phiBins; ++j) {
            min[1] = j * factor.y;
            max[1] = (j+1) * factor.y;
            Float result, error;

            integrator.integrateVectorized(
                boost::bind(&ChiSquare::integrand, pdfFn, _1, _2, _3),
                min, max, &result, &error
            );

            integral += result;
            m_refTable[idx++] += result * m_sampleCount;
            maxError = std::max(maxError, error);
        }
    }

    Log(m_logLevel, "Done, took %i ms (max error = %f, integral=%f).",
            timer->getMilliseconds(), maxError, integral);
}

struct SortedCell {
    Float expCount;
    int idx;
};

struct SortedCellFunctor {
    inline bool operator()(const SortedCell &c1, const SortedCell &c2) const {
        return c1.expCount < c2.expCount;
    }
};

ChiSquare::ETestResult ChiSquare::runTest(Float pvalThresh) {
    /* Compute the chi-square statistic */
    Float pooledCounts = 0, pooledRef = 0, chsq = 0.0f;
    int pooledCells = 0, df = 0;

    /* Process cells in order sorted by their expected counts */
    std::vector<SortedCell> cells(m_thetaBins*m_phiBins);
    for (int i=0; i<m_thetaBins*m_phiBins; ++i) {
        cells[i].expCount = m_refTable[i];
        cells[i].idx = i;
    }

    std::sort(cells.begin(), cells.end(), SortedCellFunctor());

    std::vector<SortedCell>::iterator it = cells.begin();

    while (it != cells.end()) {
        int idx = it->idx;

        if (m_refTable[idx] == 0) {
            if (m_table[idx] > m_tolerance) {
                /* Special handler for cells with an expected frequency of zero */
                Log(EWarn, "Encountered a cell (%i) with an expected frequency of zero, "
                    "where the actual number of observations is %f! Rejecting the "
                    "null hypothesis.", idx, m_table[idx]);
                return EReject;
            }
        } else if (m_refTable[idx] < CHISQR_MIN_EXP_FREQUENCY) {
            /* Pool cells with low expected frequencies */
            pooledCounts += m_table[idx];
            pooledRef += m_refTable[idx];
            ++pooledCells;
        } else if (pooledRef > 0 && pooledRef < CHISQR_MIN_EXP_FREQUENCY) {
            /* Pool more cells until the merged cell
               has a sufficiently high frequency */
            pooledCounts += m_table[idx];
            pooledRef += m_refTable[idx];
            ++pooledCells;
        } else {
            Float diff = m_table[idx]-m_refTable[idx];
            chsq += (diff*diff) / m_refTable[idx];
            ++df;
        }

        ++it;
    }

    if (pooledCells > 0) {
        Log(m_logLevel, "Pooled %i cells to ensure sufficiently "
            "high expected frequencies (> %f).", pooledCells,
            (Float) CHISQR_MIN_EXP_FREQUENCY);
        Float diff = pooledCounts - pooledRef;
        chsq += (diff*diff) / pooledRef;
        ++df;
    }

    /* All parameters are assumed to be known, so there is no
       DF reduction due to model parameters */
    df -= 1;

    Log(m_logLevel, "Chi-square statistic = %e (df=%i)", chsq, df);

    if (df <= 0) {
        Log(m_logLevel, "The number of degrees of freedom (%i) is too low!", df);
        return ELowDoF;
    }

    /* Probability of obtaining a test statistic at least
       as extreme as the one observed under the assumption
       that the distributions match */
    boost::math::chi_squared chSqDist(df);
    Float pval = 1 - (Float) boost::math::cdf(chSqDist, chsq);

    /* Apply the Sidak correction for multiple independent hypothesis tests */
    Float alpha = 1 - std::pow(1 - pvalThresh, 1 / (Float) m_numTests);

    if (pval < alpha) {
        Log(EWarn, "Rejected the null hypothesis (P-value = %e, "
            "significance level = %e)", pval, alpha);
        return EReject;
    } else {
        Log(m_logLevel, "Accepted the null hypothesis (P-value = %e, "
            "significance level = %e)", pval, alpha);
        return EAccept;
    }
}

MTS_IMPLEMENT_CLASS(ChiSquare, false, Object)
MTS_NAMESPACE_END


#include <cstdint>
#include <iostream>
#include <fstream>
#include <vector>
#include <map>
#include <algorithm>
#include <boost/program_options.hpp>
#include <boost/python/numpy.hpp>
#include "latte.h"

namespace py = boost::python;
namespace np = boost::python::numpy;

using namespace std;
using namespace latte;

int main (int argc, char *argv[]) {
    string ref_path;
	string output_prefix;
    {
        namespace po = boost::program_options;
        po::options_description desc_visible("Allowed options");
        desc_visible.add_options()
            ("help,h", "produce help message.")
            ("version,v", "print version.")
            ("ref", po::value(&ref_path)->default_value("data/ref"), "")
            ("output,o", po::value(&output_prefix)->default_value("out"), "output prefix")
            ;

        po::options_description desc("Allowed options");
        desc.add(desc_visible);

        po::positional_options_description p;
        p.add("output", 1);

        po::variables_map vm;
        po::store(po::command_line_parser(argc, argv).
                         options(desc).positional(p).run(), vm);
        po::notify(vm);
        if (vm.count("help")) {
            cout << "Usage:" << endl;
            cout << desc_visible;
            cout << endl;
            return 0;
        }
	}

    Ref genes(ref_path);
    vector<string> paths;
    {
        string path;
        while (getline(cin, path)) {
            paths.push_back(path);
        }
        cerr << paths.size() << " paths loaded." << endl;
    }

    Py_Initialize();
    np::initialize();

    np::ndarray exprs = np::zeros(py::make_tuple(paths.size(), genes.size()), np::dtype::get_builtin<float>());
    np::ndarray ranks = np::zeros(py::make_tuple(paths.size(), genes.size()), np::dtype::get_builtin<uint16_t>());

    ofstream index(output_prefix + ".idx");
    vector<pair<float, uint16_t>> pv(genes.size());
    for (unsigned i = 0; i < paths.size(); ++i) {
        float *expr = (float *)(exprs.get_data()) + i * Ref::GENES;
        uint16_t *rank = (uint16_t *)(ranks.get_data()) + i * Ref::GENES;
        index << paths[i] << endl;

        try {
            ifstream is(paths[i]);
            string line, gene;
            float value;
            getline(is, line);
            uint16_t j = 0;
            while (is >> gene >> value) {
                if (gene != genes[j]) throw 0;
                expr[j] = value;
                pv[j] = make_pair(value, j);
                ++j;
            }
            if (j != genes.size()) throw 0;
            sort(pv.begin(), pv.end());
            for (unsigned j = 0; j < genes.size(); ++j) {
                rank[pv[j].second] = j;
            }
        }
        catch (...) {
            cerr << "Failed to load " << paths[i] << endl;
        }
    }
    py::object np_save = py::import("numpy").attr("save");
    np_save(output_prefix + ".expr", exprs);
    np_save(output_prefix + ".rank", ranks);
    return 0;
}


#pragma once

// Armadillo
#include <armadillo>

// Mantella
#include "mantella_bits/optimisationAlgorithm.hpp"
namespace mant {
  class OptimisationProblem;
}

namespace mant {
  class SimulatedAnnealing : public OptimisationAlgorithm {
   public:
    explicit SimulatedAnnealing();

    using OptimisationAlgorithm::optimise;

    void optimise(
        OptimisationProblem& optimisationProblem);

    void setMinimalStepSize(
        const double minimalStepSize);
    double getMinimalStepSize() const;

    void setMaximalStepSize(
        const double maximalStepSize);
    double getMaximalStepSize() const;

    // Public accessible to be used in lambdas
    arma::vec state_;

   protected:
    double minimalStepSize_;
    double maximalStepSize_;
  };
}


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

  @copyright 2016 NumScale SAS

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

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

 /*!

    @ingroup group-bitwise
    Function object implementing twopower capabilities

    Returns \f$2^n\f$ (or 0 if n is less than zero)

    @par semantic:
    For any given value @c n  of integral type @c N:

    @code
    N r = twopower(n);
    @endcode

    code is similar to:

    @code
    N r = 1 << n;
    @endcode

    @par Note:

    This function is not defined for floating entries

    @see  ilog2

  **/
  Value twopower(Value const & v0);
} }
#endif

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

#endif


/*
 *
 * Copyright (c) Kresimir Fresl 2003
 * Copyright (c) Georg Baum 2004
 *
 * Permission to copy, modify, use and distribute this software
 * for any non-commercial or commercial purpose is granted provided
 * that this license appear on all copies of the software source code.
 *
 * Author assumes no responsibility whatsoever for its use and makes
 * no guarantees about its quality, correctness or reliability.
 *
 * Author acknowledges the support of the Faculty of Civil Engineering,
 * University of Zagreb, Croatia.
 *
 */
/**
 * Adapted to SuperLU_MT by Janosch Stascheit
 * Institute for Structural Mechanics
 * Ruhr-University Bochum
 */

#ifndef BOOST_NUMERIC_BINDINGS_SUPERLU_MT_H
#define BOOST_NUMERIC_BINDINGS_SUPERLU_MT_H

// #define BOOST_NUMERIC_BINDINGS_SUPERLU_PRINT

#include <boost/numeric/bindings/traits/sparse_traits.hpp>
#include "external_includes/superlu/superlu_mt_overloads.hpp"

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
#  include <boost/static_assert.hpp>
#  include <boost/type_traits/same_traits.hpp>
#endif

#ifdef BOOST_NUMERIC_BINDINGS_SUPERLU_PRINT
#  include <iostream>
#  include <algorithm>
#  include <iterator>
#endif

// #include "external_includes/superlu/slu_mt_util.h"
// #include "external_includes/superlu/pdsp_defs.h"
// #include "external_includes/superlu/slu_mt_Cnames.h"

namespace boost { namespace numeric { namespace bindings {

    namespace superlu {

        enum permc_spec_t {
            natural = 0,            // Pc = I
      // .. is there a bug in SuperLU ?
      // .. get_perm_c.c & mmd.c ?
      // attma_min_degree = 1,  // min. degree on the structure of A^T * A
            atpla_min_degree = 2,   // min. degree on the structure of A^T + A
            colamd = 3              // approx. min. degree for unsymmetric matrices
        };

    template <typename MatrA, typename MatrB> inline
            int pgssv (int nproc, MatrA const& a, MatrB& b, permc_spec_t perm_spec = colamd)
    {
        typedef traits::sparse_matrix_traits<MatrA> matraits;
        typedef traits::matrix_traits<MatrB> mbtraits;
        std::cout << "I am in parallel pgssv, running with " << nproc << " threads" << std::endl;

        BOOST_STATIC_ASSERT((boost::is_same<
                typename matraits::matrix_structure, traits::general_t
                >::value));
        BOOST_STATIC_ASSERT((boost::is_same<
                typename matraits::storage_format, traits::compressed_t
                >::value));
        BOOST_STATIC_ASSERT((boost::is_same<
                typename matraits::ordering_type, traits::column_major_t
                >::value));

        BOOST_STATIC_ASSERT(matraits::index_base == 0);

        BOOST_STATIC_ASSERT((boost::is_same<
                typename mbtraits::matrix_structure, traits::general_t
                >::value));
        BOOST_STATIC_ASSERT((boost::is_same<
                typename mbtraits::ordering_type, traits::column_major_t
                >::value));

        typedef typename matraits::value_type val_t;

        MatrA& aa = const_cast<MatrA&> (a);

        int m = matraits::size1 (aa);
        assert (m == matraits::size2 (aa));
        assert (m == mbtraits::size1 (b));

        SuperMatrix A, B, L, U;

        /** manual index vector generation
         */
        int *index1_vector = new (std::nothrow) int[a.index1_data().size()];
        int *index2_vector = new (std::nothrow) int[a.index2_data().size()];
        for( int i = 0; i < a.index1_data().size(); i++ )
        {
            index1_vector[i] = (int)a.index1_data()[i];
//           std::cout << index1_vector[i] << " ";
        }
//         std::cout << std::endl;
//         std::cout << std::endl;
        for( int i = 0; i < a.index2_data().size(); i++ )
        {
            index2_vector[i] = (int)a.index2_data()[i];
//           std::cout << index2_vector[i] << " ";
        }
//         std::cout << std::endl;

        detail::Create_CompCol_Matrix (&A, m, m, matraits::num_nonzeros (aa),
                                        matraits::value_storage (aa),
                                                index2_vector,
                                                        index1_vector
//                                      (int*)matraits::index1_storage (aa),
//                                      (int*)matraits::index2_storage (aa)
                                      );

        std::cout << "Matrix storage type: " << A.Mtype << std::endl;

        detail::Create_Dense_Matrix (&B, mbtraits::size1(b), mbtraits::size2(b),
                                      mbtraits::storage (b),
//                                    mbtraits::leading_dimension (b));
                                              mbtraits::size1(b));

#ifdef BOOST_NUMERIC_BINDINGS_SUPERLU_PRINT
        detail::Print_CompCol_Matrix (val_t(), "A", &A);
        detail::Print_Dense_Matrix (val_t(), "B", &B);
        std::cout << std::endl;
#endif

        int *perm_r = new (std::nothrow) int[m];
        if (!perm_r)
            return -100;
        int *perm_c = new (std::nothrow) int[m];
        if (!perm_c) {
//             std::cout << "####### deleting perm_r" << std::endl;
            delete[] perm_r;
            return -101;
        }

        /*
         * Get column permutation vector perm_c[], according to permc_spec:
         *   permc_spec = 0: natural ordering
         *   permc_spec = 1: minimum degree on structure of A’*A
         *   permc_spec = 2: minimum degree on structure of A’+A
         *   permc_spec = 3: approximate minimum degree for unsymmetric matrices
         */
        int permc_spec = 3;
        get_perm_c(permc_spec, &A, perm_c);

#ifdef BOOST_NUMERIC_BINDINGS_SUPERLU_PRINT
        std::cout << "perm_c: ";
        std::copy (perm_c, perm_c + m,
                   std::ostream_iterator<int> (std::cout, " "));
        std::cout << std::endl << std::endl;
#endif

//         superlu_mt_options_t options;
//         set_default_options(&options);
//         options.RowPerm = static_cast<rowperm_t>(perm_spec);
        /**
         * testing options
         */
//         options.ColPerm = static_cast<colperm_t>(colamd);

        int info = 0;
//         std::cout << "A.ncol: " << A.ncol << std::endl;
//         for( int i=0; i<4; i++ )
//         {
//             std::cout<< "####superlu.hpp#### running solver in iteration no. " << i << std::endl;
            boost::numeric::bindings::superlu::detail::pgssv( nproc, &A, perm_c, perm_r, &L, &U, &B, &info );
//         }

//         std::cout << "########## free memory..." << std::endl;
        //StatFree(&stat);
        Destroy_CompCol_Matrix (&U);
        Destroy_SuperNode_Matrix (&L);
        delete[] perm_c;
        delete[] perm_r;
        Destroy_SuperMatrix_Store (&B);
        Destroy_SuperMatrix_Store (&A);
        delete[] index1_vector;
        delete[] index2_vector;

        return info;
    }

    /**
     * specialisation for single equation systems Ax=b
     * note: template parameter DataType is only for distinguation
     * with AX=B specialisation
     * by Janosch Stascheit
     */
//     template <typename MatrA, typename VecB>
//             inline
//         int gssv (MatrA const& a, VecB& b, int single, permc_spec_t perm_spec = colamd)
//     {
//         // adapted to row_major
//         typedef traits::sparse_matrix_traits<MatrA> matraits;
//         typedef traits::vector_traits<VecB> vtraits;
//
// #ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
//       BOOST_STATIC_ASSERT((boost::is_same<
//         typename matraits::matrix_structure, traits::general_t
//       >::value));
//       BOOST_STATIC_ASSERT((boost::is_same<
//               typename matraits::storage_format, traits::compressed_t
//               >::value));
//       BOOST_STATIC_ASSERT((boost::is_same<
//               typename matraits::ordering_type, traits::row_major_t
//               >::value));
//
//       BOOST_STATIC_ASSERT(matraits::index_base == 0);
//
// //       BOOST_STATIC_ASSERT((boost::is_same<
// //               typename mbtraits::matrix_structure, traits::general_t
// //               >::value));
// //       BOOST_STATIC_ASSERT((boost::is_same<
// //               typename mbtraits::ordering_type, traits::row_major_t
// //               >::value));
// #endif
//
//       typedef typename matraits::value_type val_t;
//
//       MatrA& aa = const_cast<MatrA&> (a);
//
//       int m = matraits::size1 (aa);
//       assert (m == matraits::size2 (aa));
//       assert (m == vtraits::size (b));
//
//       SuperMatrix A, B, L, U;
//
//       detail::Create_CompRow_Matrix (&A, m, m, matraits::num_nonzeros (aa),
//                                       matraits::value_storage (aa),
//                                       (int*)matraits::index2_storage (aa),
//                                       (int*)matraits::index1_storage (aa) );
//
//       detail::Create_Dense_Matrix (&B, m, 1,
//                                     vtraits::storage(b),
//                                     b.size()
// //                                     vtraits::vector_storage(b),
// //                                     vtraits::leading_dimension (b)
//                                   );
//
// #ifdef BOOST_NUMERIC_BINDINGS_SUPERLU_PRINT
//       detail::Print_CompCol_Matrix (val_t(), "A", &A);
//       detail::Print_Dense_Matrix (val_t(), "B", &B);
//       std::cout << std::endl;
// #endif
//
//       int *perm_r = new (std::nothrow) int[m];
//       if (!perm_r)
//           return -100;
//       int *perm_c = new (std::nothrow) int[m];
//       if (!perm_c) {
//           delete[] perm_r;
//           return -101;
//       }
// #ifdef BOOST_NUMERIC_BINDINGS_SUPERLU_PRINT
//       std::cout << "perm_c: ";
//       std::copy (perm_c, perm_c + m,
//                  std::ostream_iterator<int> (std::cout, " "));
//       std::cout << std::endl << std::endl;
// #endif
//
//       superlu_options_t options;
//       set_default_options(&options);
//       options.RowPerm = static_cast<rowperm_t>(perm_spec);
//       SuperLUStat_t stat;
//       StatInit(&stat);
//
//       int info = 0;
//       detail::gssv (val_t(), &options, &A, perm_c, perm_r, &L, &U, &B, &stat, &info);
//
// #ifdef BOOST_NUMERIC_BINDINGS_SUPERLU_PRINT
//       // detail::Print_SuperNode_Matrix (val_t(), "L + U - I", &L);
//       // detail::Print_CompCol_Matrix (val_t(), "U", &U);
//
//       // std::cout << std::endl;
//       // SCformat* Lstore = (SCformat*) L.Store;
//       // NCformat* Ustore = (NCformat*) U.Store;
//       // std::cout << "No of nonzeros in L = " << Lstore->nnz << std::endl;
//       // std::cout << "No of nonzeros in U = " << Ustore->nnz << std::endl;
//       // std::cout << std::endl;
//       // SCformat* Lstore = (SCformat*) L.Store;
//       // for (int i = 0; i < Lstore->nnz; ++i)
//       //   std::cout << ((val_t*) (Lstore->nzval))[i] << " ";
//       // std::cout << std::endl << std::endl;
//
// //       detail::Print_Dense_Matrix (val_t(), "X", &B);
//       std::cout << std::endl;
// #endif
//
//       if (options.PrintStat) {
//     StatPrint(&stat);
//       }
//       StatFree(&stat);
//       Destroy_CompCol_Matrix (&U);
//       Destroy_SuperNode_Matrix (&L);
//       delete[] perm_c;
//       delete[] perm_r;
//       Destroy_SuperMatrix_Store (&B);
//       Destroy_SuperMatrix_Store (&A);
//
//       return info;
//                     }
//
  }

}}}

#endif


#include "testsuite.h"
#include <blitz/array.h>

BZ_USING_NAMESPACE(blitz)

int main(int argc, char** argv)
{
  // test 64-bit compatibility if enabled
  if(sizeof(sizeType)>4) {

    // wrap in try so we can catch allocation failure if test platform
    // has insufficient memory. To minimize memory reqs we use arrays
    // of char, but we still need at least 4GB. (We want it to be a
    // contiguous array so the loops are coalesced.)
    Array<unsigned char,4> test(contiguousArray);
    try {
      test.resize(shape(256,256,256,256));
    }
    catch (std::bad_alloc&) {
      cerr << "Allocation failure - not enough memory to test 64-bit compatibility\n";
      return 0;
    }

    // we MUST do a full assignment, because if we use a subarray the
    // offset is wrt the subarray. But we avoid using some complicated
    // index expression since that makes it very slow.
    test = 42;
    unsigned char crap= test(255,255,255,255);
    cerr << int(crap) << endl;
    BZTEST(test(255,255,255,255)==42);
    test = test + test;
    BZTEST(test(255,255,255,255)==84);
    test.free();

#ifdef BZ_FULLY64BIT
    // test arrays with long dimensions
    Array<char, 1> test2(1<<32);
    test2 = 1;
    BZTEST(sum(test2)==1<<32);
    test2.free();
#endif
  }
  else {
    cerr << "Not a 64-bit machine, can't test 64-bit compatibility.\n";
  }

  return 0;
}


/*
 [auto_generated]
 boost/numeric/odeint/integrate/detail/integrate_adaptive.hpp

 [begin_description]
 Default Integrate adaptive implementation.
 [end_description]

 Copyright 2009-2011 Karsten Ahnert
 Copyright 2009-2011 Mario Mulansky

 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_NUMERIC_ODEINT_INTEGRATE_DETAIL_INTEGRATE_ADAPTIVE_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_INTEGRATE_DETAIL_INTEGRATE_ADAPTIVE_HPP_INCLUDED

#include <stdexcept>

#include <boost/numeric/odeint/stepper/stepper_categories.hpp>
#include <boost/numeric/odeint/stepper/controlled_step_result.hpp>
#include <boost/numeric/odeint/iterator/integrate/detail/integrate_const.hpp>
#include <boost/numeric/odeint/iterator/adaptive_time_iterator.hpp>
#include <boost/numeric/odeint/iterator/integrate/detail/functors.hpp>
#include <boost/numeric/odeint/util/bind.hpp>
#include <boost/numeric/odeint/util/unwrap_reference.hpp>
#include <boost/numeric/odeint/util/copy.hpp>

#include <boost/numeric/odeint/util/detail/less_with_sign.hpp>

namespace boost {
namespace numeric {
namespace odeint {
namespace detail {

// forward declaration
template< class Stepper , class System , class State , class Time , class Observer>
size_t integrate_const(
        Stepper stepper , System system , State &start_state ,
        Time start_time , Time end_time , Time dt ,
        Observer observer , stepper_tag );

/*
 * integrate_adaptive for simple stepper is basically an integrate_const + some last step
 */
template< class Stepper , class System , class State , class Time , class Observer >
size_t integrate_adaptive(
        Stepper stepper , System system , State &start_state ,
        Time start_time , Time end_time , Time dt ,
        Observer observer , stepper_tag
)
{
    size_t steps = detail::integrate_const( stepper , system , start_state , start_time ,
                                            end_time , dt , observer , stepper_tag() );
    typename odeint::unwrap_reference< Observer >::type &obs = observer;
    typename odeint::unwrap_reference< Stepper >::type &st = stepper;

    Time end = start_time + dt*steps;
    if( less_with_sign( end , end_time , dt ) )
    {   //make a last step to end exactly at end_time
        st.do_step( system , start_state , end , end_time - end );
        steps++;
        obs( start_state , end_time );
    }
    return steps;
}

/*
 * classical integrate adaptive
 */
template< class Stepper , class System , class State , class Time , class Observer >
size_t integrate_adaptive(
        Stepper stepper , System system , State &start_state ,
        Time &start_time , Time end_time , Time &dt ,
        Observer observer , controlled_stepper_tag
)
{
    size_t obs_calls = 0;

    boost::for_each( make_adaptive_time_range( stepper , system , start_state ,
                                               start_time , end_time , dt ) ,
                     obs_caller< Observer >( obs_calls , observer ) );

    return obs_calls-1;
}

/*
 * integrate adaptive for dense output steppers
 *
 * step size control is used if the stepper supports it
 */
template< class Stepper , class System , class State , class Time , class Observer >
size_t integrate_adaptive(
        Stepper stepper , System system , State &start_state ,
        Time start_time , Time end_time , Time dt ,
        Observer observer , dense_output_stepper_tag )
{
    size_t obs_calls = 0;

    boost::for_each( make_adaptive_time_range( stepper , system , start_state ,
                                               start_time , end_time , dt ) ,
                     obs_caller< Observer >( obs_calls , observer ) );

    return obs_calls-1;
}

} // namespace detail
} // namespace odeint
} // namespace numeric
} // namespace boost

#endif // BOOST_NUMERIC_ODEINT_INTEGRATE_DETAIL_INTEGRATE_ADAPTIVE_HPP_INCLUDED


// Copyright (C) 2008-2018 Lorenzo Caminiti
// Distributed under the Boost Software License, Version 1.0 (see accompanying
// file LICENSE_1_0.txt or a copy at http://www.boost.org/LICENSE_1_0.txt).
// See: http://www.boost.org/doc/libs/release/libs/contract/doc/html/index.html

//[cline90_calendar
#include <boost/contract.hpp>
#include <cassert>

class calendar {
    friend class boost::contract::access;

    void invariant() const {
        BOOST_CONTRACT_ASSERT(month() >= 1);
        BOOST_CONTRACT_ASSERT(month() <= 12);
        BOOST_CONTRACT_ASSERT(date() >= 1);
        BOOST_CONTRACT_ASSERT(date() <= days_in(month()));
    }

public:
    calendar() : month_(1), date_(31) {
        boost::contract::check c = boost::contract::constructor(this)
            .postcondition([&] {
                BOOST_CONTRACT_ASSERT(month() == 1);
                BOOST_CONTRACT_ASSERT(date() == 31);
            })
        ;
    }

    virtual ~calendar() {
        // Check invariants.
        boost::contract::check c = boost::contract::destructor(this);
    }

    int month() const {
        // Check invariants.
        boost::contract::check c = boost::contract::public_function(this);
        return month_;
    }

    int date() const {
        // Check invariants.
        boost::contract::check c = boost::contract::public_function(this);
        return date_;
    }

    void reset(int new_month) {
        boost::contract::check c = boost::contract::public_function(this)
            .precondition([&] {
                BOOST_CONTRACT_ASSERT(new_month >= 1);
                BOOST_CONTRACT_ASSERT(new_month <= 12);
            })
            .postcondition([&] {
                BOOST_CONTRACT_ASSERT(month() == new_month);
            })
        ;

        month_ = new_month;
    }

private:
    static int days_in(int month) {
        int result;
        boost::contract::check c = boost::contract::function()
            .precondition([&] {
                BOOST_CONTRACT_ASSERT(month >= 1);
                BOOST_CONTRACT_ASSERT(month <= 12);
            })
            .postcondition([&] {
                BOOST_CONTRACT_ASSERT(result >= 1);
                BOOST_CONTRACT_ASSERT(result <= 31);
            })
        ;

        return result = 31; // For simplicity, assume all months have 31 days.
    }

    int month_, date_;
};

int main() {
    calendar cal;
    assert(cal.date() == 31);
    assert(cal.month() == 1);

    cal.reset(8); // Set month
    assert(cal.month() == 8);

    return 0;
}
//]


#ifndef VIENNACL_LINALG_SVD_HPP
#define VIENNACL_LINALG_SVD_HPP

/* =========================================================================
   Copyright (c) 2010-2014, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.
   Portions of this software are copyright by UChicago Argonne, LLC.

                            -----------------
                  ViennaCL - The Vienna Computing Library
                            -----------------

   Project Head:    Karl Rupp                   rupp@iue.tuwien.ac.at

   (A list of authors and contributors can be found in the PDF manual)

   License:         MIT (X11), see file LICENSE in the base directory
============================================================================= */

/** @file viennacl/linalg/svd.hpp
    @brief Provides singular value decomposition using a block-based approach.  Experimental.

    Contributed by Volodymyr Kysenko.
*/

// Note: Boost.uBLAS is required at the moment
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>

#include <cmath>

#include "viennacl/matrix.hpp"
#include "viennacl/linalg/opencl/kernels/svd.hpp"
#include "viennacl/linalg/qr-method-common.hpp"

namespace viennacl
{
  namespace linalg
  {

    namespace detail
    {

      template<typename MatrixType, typename VectorType>
      void givens_prev(MatrixType & matrix,
                       VectorType & tmp1,
                       VectorType & tmp2,
                       int n,
                       int l,
                       int k
                      )
      {
        typedef typename MatrixType::value_type                                   ScalarType;
        typedef typename viennacl::result_of::cpu_value_type<ScalarType>::type    CPU_ScalarType;

        viennacl::ocl::context & ctx = const_cast<viennacl::ocl::context &>(viennacl::traits::opencl_handle(matrix).context());
        viennacl::ocl::kernel & kernel = ctx.get_kernel(viennacl::linalg::opencl::kernels::svd<CPU_ScalarType>::program_name(), SVD_GIVENS_PREV_KERNEL);

        kernel.global_work_size(0, viennacl::tools::align_to_multiple<vcl_size_t>(viennacl::traits::size1(matrix), 256));
        kernel.local_work_size(0, 256);

        viennacl::ocl::enqueue(kernel(
                                      matrix,
                                      tmp1,
                                      tmp2,
                                      static_cast<cl_uint>(n),
                                      static_cast<cl_uint>(matrix.internal_size1()),
                                      static_cast<cl_uint>(l + 1),
                                      static_cast<cl_uint>(k + 1)
                              ));
      }

      template<typename MatrixType, typename VectorType>
      void change_signs(MatrixType& matrix, VectorType& signs, int n)
      {
        typedef typename MatrixType::value_type                                   ScalarType;
        typedef typename viennacl::result_of::cpu_value_type<ScalarType>::type    CPU_ScalarType;

        viennacl::ocl::context & ctx = const_cast<viennacl::ocl::context &>(viennacl::traits::opencl_handle(matrix).context());
        viennacl::ocl::kernel & kernel = ctx.get_kernel(viennacl::linalg::opencl::kernels::svd<CPU_ScalarType>::program_name(), SVD_INVERSE_SIGNS_KERNEL);

        kernel.global_work_size(0, viennacl::tools::align_to_multiple<vcl_size_t>(viennacl::traits::size1(matrix), 16));
        kernel.global_work_size(1, viennacl::tools::align_to_multiple<vcl_size_t>(viennacl::traits::size2(matrix), 16));

        kernel.local_work_size(0, 16);
        kernel.local_work_size(1, 16);

        viennacl::ocl::enqueue(kernel(
                                      matrix,
                                      signs,
                                      static_cast<cl_uint>(n),
                                      static_cast<cl_uint>(matrix.internal_size1())
                              ));
      }

      template<typename MatrixType, typename CPU_VectorType>
      void svd_qr_shift(MatrixType & vcl_u,
                        MatrixType & vcl_v,
                        CPU_VectorType & q,
                        CPU_VectorType & e)
      {
        typedef typename MatrixType::value_type                                   ScalarType;
        typedef typename viennacl::result_of::cpu_value_type<ScalarType>::type    CPU_ScalarType;

        vcl_size_t n = q.size();
        int m = static_cast<int>(vcl_u.size1());

        detail::transpose(vcl_u);
        detail::transpose(vcl_v);

        std::vector<CPU_ScalarType> signs_v(n, 1);
        std::vector<CPU_ScalarType> cs1(n), ss1(n), cs2(n), ss2(n);

        viennacl::vector<CPU_ScalarType> tmp1(n, viennacl::traits::context(vcl_u)), tmp2(n, viennacl::traits::context(vcl_u));

        bool goto_test_conv = false;

        for (int k = static_cast<int>(n) - 1; k >= 0; k--)
        {
          // std::cout << "K = " << k << std::endl;

          vcl_size_t iter = 0;
          for (iter = 0; iter < detail::ITER_MAX; iter++)
          {
            // test for split
            int l;
            for (l = k; l >= 0; l--)
            {
              goto_test_conv = false;
              if (std::fabs(e[vcl_size_t(l)]) <= detail::EPS)
              {
                // set it
                goto_test_conv = true;
                break;
              }

              if (std::fabs(q[vcl_size_t(l) - 1]) <= detail::EPS)
              {
                // goto
                break;
              }
            }

            if (!goto_test_conv)
            {
              CPU_ScalarType c = 0.0;
              CPU_ScalarType s = 1.0;

              //int l1 = l - 1;
              //int l2 = k;

              for (int i = l; i <= k; i++)
              {
                CPU_ScalarType f = s * e[vcl_size_t(i)];
                e[vcl_size_t(i)] = c * e[vcl_size_t(i)];

                if (std::fabs(f) <= detail::EPS)
                {
                  //l2 = i - 1;
                  break;
                }

                CPU_ScalarType g = q[vcl_size_t(i)];
                CPU_ScalarType h = detail::pythag(f, g);
                q[vcl_size_t(i)] = h;
                c = g / h;
                s = -f / h;

                cs1[vcl_size_t(i)] = c;
                ss1[vcl_size_t(i)] = s;
              }

              // std::cout << "Hitted!" << l1 << " " << l2 << "\n";

              // for (int i = l; i <= l2; i++)
              // {
              //   for (int j = 0; j < m; j++)
              //   {
              //     CPU_ScalarType y = u(j, l1);
              //     CPU_ScalarType z = u(j, i);
              //     u(j, l1) = y * cs1[i] + z * ss1[i];
              //     u(j, i) = -y * ss1[i] + z * cs1[i];
              //   }
              // }
            }

            CPU_ScalarType z = q[vcl_size_t(k)];

            if (l == k)
            {
              if (z < 0)
              {
                q[vcl_size_t(k)] = -z;

                signs_v[vcl_size_t(k)] *= -1;
              }

              break;
            }

            if (iter >= detail::ITER_MAX - 1)
              break;

            CPU_ScalarType x = q[vcl_size_t(l)];
            CPU_ScalarType y = q[vcl_size_t(k) - 1];
            CPU_ScalarType g = e[vcl_size_t(k) - 1];
            CPU_ScalarType h = e[vcl_size_t(k)];
            CPU_ScalarType f = ((y - z) * (y + z) + (g - h) * (g + h)) / (2 * h * y);

            g = detail::pythag<CPU_ScalarType>(f, 1);

            if (f < 0) {
              f = ((x - z) * (x + z) + h * (y / (f - g) - h)) / x;
            } else {
              f = ((x - z) * (x + z) + h * (y / (f + g) - h)) / x;
            }

            CPU_ScalarType c = 1;
            CPU_ScalarType s = 1;

            for (vcl_size_t i = static_cast<vcl_size_t>(l) + 1; i <= static_cast<vcl_size_t>(k); i++)
            {
              g = e[i];
              y = q[i];
              h = s * g;
              g = c * g;
              CPU_ScalarType z = detail::pythag(f, h);
              e[i - 1] = z;
              c = f / z;
              s = h / z;
              f = x * c + g * s;
              g = -x * s + g * c;
              h = y * s;
              y = y * c;

              cs1[i] = c;
              ss1[i] = s;

              z = detail::pythag(f, h);
              q[i - 1] = z;
              c = f / z;
              s = h / z;
              f = c * g + s * y;
              x = -s * g + c * y;

              cs2[i] = c;
              ss2[i] = s;
            }

            {
              viennacl::copy(cs1, tmp1);
              viennacl::copy(ss1, tmp2);

              givens_prev(vcl_v, tmp1, tmp2, static_cast<int>(n), l, k);
            }

            {
              viennacl::copy(cs2, tmp1);
              viennacl::copy(ss2, tmp2);

              givens_prev(vcl_u, tmp1, tmp2, m, l, k);
            }

            e[vcl_size_t(l)] = 0.0;
            e[vcl_size_t(k)] = f;
            q[vcl_size_t(k)] = x;
          }

        }

        viennacl::copy(signs_v, tmp1);
        change_signs(vcl_v, tmp1, static_cast<int>(n));

        // transpose singular matrices again
        detail::transpose(vcl_u);
        detail::transpose(vcl_v);
      }

      /*template<typename SCALARTYPE, unsigned int ALIGNMENT>
      bool householder_c(viennacl::matrix<SCALARTYPE, row_major, ALIGNMENT> & A,
                          viennacl::matrix<SCALARTYPE, row_major, ALIGNMENT> & Q,
                          viennacl::vector<SCALARTYPE, ALIGNMENT> & D,
                          vcl_size_t start)
      {

        vcl_size_t row_start = start;
        vcl_size_t col_start = start;

        if (row_start + 1 >= A.size1())
          return false;

        std::vector<SCALARTYPE> tmp(A.size1(), 0);

        copy_vec(A, D, row_start, col_start, true);
        fast_copy(D.begin(), D.begin() + (A.size1() - row_start), tmp.begin() + row_start);

        detail::householder_vector(tmp, row_start);

        fast_copy(tmp, D);

        viennacl::ocl::kernel & kernel = viennacl::ocl::get_kernel(viennacl::linalg::opencl::kernels::svd<SCALARTYPE>::program_name(), SVD_HOUSEHOLDER_COL_KERNEL);

        //kernel.global_work_size(0, A.size1() << 1);

        viennacl::ocl::enqueue(kernel(
                                      A,
                                      Q,
                                      D,
                                      static_cast<cl_uint>(row_start),
                                      static_cast<cl_uint>(col_start),
                                      static_cast<cl_uint>(A.size1()),
                                      static_cast<cl_uint>(A.size2()),
                                      static_cast<cl_uint>(A.internal_size2()),
                                      static_cast<cl_uint>(Q.internal_size2()),
                                      viennacl::ocl::local_mem(static_cast<cl_uint>(128 * sizeof(SCALARTYPE)))
                              ));

        return true;
      }*/

      template<typename SCALARTYPE, unsigned int ALIGNMENT>
      bool householder_c(viennacl::matrix<SCALARTYPE, row_major, ALIGNMENT>& A,
                          viennacl::matrix<SCALARTYPE, row_major, ALIGNMENT>& Q,
                          viennacl::vector<SCALARTYPE, ALIGNMENT>& D,
                          vcl_size_t row_start, vcl_size_t col_start)
      {
        viennacl::ocl::context & ctx = const_cast<viennacl::ocl::context &>(viennacl::traits::opencl_handle(A).context());

        if (row_start + 1 >= A.size1())
          return false;

        prepare_householder_vector(A, D, A.size1(), row_start, col_start, row_start, true);

        {
          viennacl::ocl::kernel& kernel = ctx.get_kernel(viennacl::linalg::opencl::kernels::svd<SCALARTYPE>::program_name(), SVD_HOUSEHOLDER_UPDATE_A_LEFT_KERNEL);

          viennacl::ocl::enqueue(kernel(
                                        A,
                                        D,
                                        static_cast<cl_uint>(row_start),
                                        static_cast<cl_uint>(col_start),
                                        static_cast<cl_uint>(A.size1()),
                                        static_cast<cl_uint>(A.size2()),
                                        static_cast<cl_uint>(A.internal_size2()),
                                        viennacl::ocl::local_mem(static_cast<cl_uint>(128 * sizeof(SCALARTYPE)))
                                ));

        }

        {
          viennacl::ocl::kernel& kernel = ctx.get_kernel(viennacl::linalg::opencl::kernels::svd<SCALARTYPE>::program_name(), SVD_HOUSEHOLDER_UPDATE_QL_KERNEL);

          viennacl::ocl::enqueue(kernel(
                                        Q,
                                        D,
                                        static_cast<cl_uint>(A.size1()),
                                      //  static_cast<cl_uint>(A.size2()),
                                        static_cast<cl_uint>(Q.internal_size2()),
                                        viennacl::ocl::local_mem(static_cast<cl_uint>(128 * sizeof(SCALARTYPE)))
                                ));

        }

        return true;
      }

      /*
      template<typename SCALARTYPE, unsigned int ALIGNMENT>
      bool householder_r(viennacl::matrix<SCALARTYPE, row_major, ALIGNMENT>& A,
                          viennacl::matrix<SCALARTYPE, row_major, ALIGNMENT>& Q,
                          viennacl::vector<SCALARTYPE, ALIGNMENT>& S,
                          vcl_size_t start)
      {

        vcl_size_t row_start = start;
        vcl_size_t col_start = start + 1;

        if (col_start + 1 >= A.size2())
          return false;

        std::vector<SCALARTYPE> tmp(A.size2(), 0);

        copy_vec(A, S, row_start, col_start, false);
        fast_copy(S.begin(),
                  S.begin() + (A.size2() - col_start),
                  tmp.begin() + col_start);

        detail::householder_vector(tmp, col_start);
        fast_copy(tmp, S);

        viennacl::ocl::kernel& kernel = viennacl::ocl::get_kernel(viennacl::linalg::opencl::kernels::svd<SCALARTYPE>::program_name(), SVD_HOUSEHOLDER_ROW_KERNEL);

        viennacl::ocl::enqueue(kernel(
                                      A,
                                      Q,
                                      S,
                                      static_cast<cl_uint>(row_start),
                                      static_cast<cl_uint>(col_start),
                                      static_cast<cl_uint>(A.size1()),
                                      static_cast<cl_uint>(A.size2()),
                                      static_cast<cl_uint>(A.internal_size2()),
                                      static_cast<cl_uint>(Q.internal_size2()),
                                      viennacl::ocl::local_mem(static_cast<cl_uint>(128 * sizeof(SCALARTYPE)))
                                ));
        return true;
      } */

      template<typename SCALARTYPE, unsigned int ALIGNMENT>
      bool householder_r(viennacl::matrix<SCALARTYPE, row_major, ALIGNMENT> & A,
                          viennacl::matrix<SCALARTYPE, row_major, ALIGNMENT> & Q,
                          viennacl::vector<SCALARTYPE, ALIGNMENT>& D,
                          vcl_size_t row_start, vcl_size_t col_start)
      {
        viennacl::ocl::context & ctx = const_cast<viennacl::ocl::context &>(viennacl::traits::opencl_handle(A).context());

        if (col_start + 1 >= A.size2())
          return false;

        prepare_householder_vector(A, D, A.size2(), row_start, col_start, col_start, false);

        {
          viennacl::ocl::kernel& kernel = ctx.get_kernel(viennacl::linalg::opencl::kernels::svd<SCALARTYPE>::program_name(), SVD_HOUSEHOLDER_UPDATE_A_RIGHT_KERNEL);

          viennacl::ocl::enqueue(kernel(
                                        A,
                                        D,
                                        static_cast<cl_uint>(row_start),
                                        static_cast<cl_uint>(col_start),
                                        static_cast<cl_uint>(A.size1()),
                                        static_cast<cl_uint>(A.size2()),
                                        static_cast<cl_uint>(A.internal_size2()),
                                        viennacl::ocl::local_mem(static_cast<cl_uint>(128 * sizeof(SCALARTYPE)))
                                ));
        }

        {
          viennacl::ocl::kernel& kernel = ctx.get_kernel(viennacl::linalg::opencl::kernels::svd<SCALARTYPE>::program_name(), SVD_HOUSEHOLDER_UPDATE_QR_KERNEL);

          viennacl::ocl::enqueue(kernel(
                                        Q,
                                        D,
                                        static_cast<cl_uint>(A.size1()),
                                        static_cast<cl_uint>(A.size2()),
                                        static_cast<cl_uint>(Q.internal_size2()),
                                        viennacl::ocl::local_mem(static_cast<cl_uint>(128 * sizeof(SCALARTYPE)))
                                ));
        }

        return true;
      }

      template<typename SCALARTYPE, unsigned int ALIGNMENT>
      void bidiag(viennacl::matrix<SCALARTYPE, row_major, ALIGNMENT> & Ai,
                  viennacl::matrix<SCALARTYPE, row_major, ALIGNMENT> & QL,
                  viennacl::matrix<SCALARTYPE, row_major, ALIGNMENT> & QR)
      {
        vcl_size_t row_num = Ai.size1();
        vcl_size_t col_num = Ai.size2();

        vcl_size_t to = std::min(row_num, col_num);
        vcl_size_t big_to = std::max(row_num, col_num);

        //for storing householder vector
        viennacl::vector<SCALARTYPE, ALIGNMENT> hh_vector(big_to, viennacl::traits::context(Ai));

        QL = viennacl::identity_matrix<SCALARTYPE>(QL.size1(), viennacl::traits::context(QL));
        QR = viennacl::identity_matrix<SCALARTYPE>(QR.size1(), viennacl::traits::context(QR));

        for (vcl_size_t i = 0; i < to; i++)
        {
          householder_c(Ai, QL, hh_vector, i, i);
          householder_r(Ai, QR, hh_vector, i, i+1);
        }
      }

    } // namespace detail

    /** @brief Computes the singular value decomposition of a matrix A. Experimental in 1.3.x
     *
     * @param A     The input matrix. Will be overwritten with a diagonal matrix containing the singular values on return
     * @param QL    The left orthogonal matrix
     * @param QR    The right orthogonal matrix
     */
    template<typename SCALARTYPE, unsigned int ALIGNMENT>
    void svd(viennacl::matrix<SCALARTYPE, row_major, ALIGNMENT> & A,
              viennacl::matrix<SCALARTYPE, row_major, ALIGNMENT> & QL,
              viennacl::matrix<SCALARTYPE, row_major, ALIGNMENT> & QR)
    {
      viennacl::ocl::context & ctx = const_cast<viennacl::ocl::context &>(viennacl::traits::opencl_handle(A).context());
      viennacl::linalg::opencl::kernels::svd<SCALARTYPE>::init(ctx);

      vcl_size_t row_num = A.size1();
      vcl_size_t col_num = A.size2();

      vcl_size_t to = std::min(row_num, col_num);

      //viennacl::vector<SCALARTYPE, ALIGNMENT> d(to);
      //viennacl::vector<SCALARTYPE, ALIGNMENT> s(to + 1);

      // first stage
      detail::bidiag(A, QL, QR);

      // second stage
      //std::vector<SCALARTYPE> dh(to, 0);
      //std::vector<SCALARTYPE> sh(to + 1, 0);
      boost::numeric::ublas::vector<SCALARTYPE> dh = boost::numeric::ublas::scalar_vector<SCALARTYPE>(to, 0);
      boost::numeric::ublas::vector<SCALARTYPE> sh = boost::numeric::ublas::scalar_vector<SCALARTYPE>(to + 1, 0);

      viennacl::linalg::opencl::bidiag_pack_svd(A, dh, sh);

      detail::svd_qr_shift( QL, QR, dh, sh);

      // Write resulting diagonal matrix with singular values to A:
      boost::numeric::ublas::matrix<SCALARTYPE> h_Sigma(row_num, col_num);
      h_Sigma.clear();

      for (vcl_size_t i = 0; i < to; i++)
        h_Sigma(i, i) = dh[i];

      copy(h_Sigma, A);
    }
  }
}
#endif


#include <boost/numeric/ublas/matrix_proxy.hpp>