/*
MIT License

Copyright (c) 2020 Rik Baehnemann, ASL, ETH Zurich, Switzerland

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.
*/

#include "fm_trajectories/circle_trajectory.h"

#include <algorithm>
#include <cmath>
#include <limits>

#include <angles/angles.h>
#include <mav_trajectory_generation/motion_defines.h>
#include <ros/ros.h>
#include <yaml-cpp/yaml.h>
#include <Eigen/Core>

namespace fm_trajectories {

namespace mtg = mav_trajectory_generation;

CircleTrajectory::Settings::Settings(
    double center_east, double center_north, double radius,
    double start_heading_deg, double arc_length_deg_in, double altitude,
    double velocity_in, double offset_heading_deg,
    const std::weak_ptr<BaseTrajectory>& prev_trajectory,
    const mtg::InputConstraints& input_constraints,
    double circle_deviation_ratio /* = 0.01 */)
    : BaseTrajectory::Settings(input_constraints, velocity_in,
                               offset_heading_deg, prev_trajectory),
      center_east(center_east),
      center_north(center_north),
      radius(std::max(std::fabs(radius), kZeroGuard)),
      start_heading_deg(start_heading_deg),
      arc_length_deg(std::max(std::fabs(arc_length_deg_in), kMinArcLength)),
      altitude(altitude),
      circle_deviation_ratio(
          std::max(std::min(std::fabs(circle_deviation_ratio), 0.999), 0.001)) {
  // Arc feasibility.
  ROS_WARN_COND(arc_length_deg_in < 0.0,
                "Input arc length negative. Inverted.");
  if (std::fabs(arc_length_deg_in) < kMinArcLength)
    ROS_WARN_STREAM("Arc length too small: " << arc_length_deg_in
                                             << " degrees. Clipped to: "
                                             << arc_length_deg << " degrees.");

  // Yaw rate feasibility.
  double omega_z_max = std::numeric_limits<double>::max();
  if (input_constraints.getConstraint(
          mav_trajectory_generation::InputConstraintType::kOmegaZMax,
          &omega_z_max)) {
    if (computeAngularVelocity() > omega_z_max) {
      double new_velocity =
          std::copysign(omega_z_max * radius - kVelocityResolution, velocity);
      ROS_WARN_STREAM("Trajectory yaw rate too large: "
                      << computeAngularVelocity()
                      << " rad/s. Clipping velocity to: " << new_velocity
                      << " m/s");
      velocity = new_velocity;
    }
  }
}

double CircleTrajectory::Settings::computeAngularVelocity() const {
  return velocity / radius;
}

CircleTrajectory::CircleTrajectory(
    const std::shared_ptr<CircleTrajectory::Settings>& settings)
    : BaseTrajectory(settings),
      start_rad_(angles::from_degrees(
          std::static_pointer_cast<CircleTrajectory::Settings>(settings_)
              ->start_heading_deg -
          90)),
      arc_length_rad_(angles::from_degrees(
          std::static_pointer_cast<CircleTrajectory::Settings>(settings_)
              ->arc_length_deg)) {
  // Angle between vertices.
  // https://stackoverflow.com/questions/11774038/how-to-render-a-circle-with-as-few-vertices-as-possible
  const double kDeltaAngleMax = std::acos(
      2.0 * std::pow(
                (1.0 -
                 std::static_pointer_cast<CircleTrajectory::Settings>(settings_)
                     ->circle_deviation_ratio),
                2.0) -
      1.0);
  const double kNumVerticesMin = std::fabs(arc_length_rad_) / kDeltaAngleMax;
  num_vertices_ = std::ceil(kNumVerticesMin);
  num_vertices_ += 1;  // To close circle.
}

bool CircleTrajectory::toYaml(YAML::Node* node) {
  CHECK_NOTNULL(node);
  *node = YAML::Node();

  Eigen::Vector3d center_wgs84;
  if (!geotf_.convert(
          "enu",
          Eigen::Vector3d(
              std::static_pointer_cast<CircleTrajectory::Settings>(settings_)
                  ->center_east,
              std::static_pointer_cast<CircleTrajectory::Settings>(settings_)
                  ->center_north,
              std::static_pointer_cast<CircleTrajectory::Settings>(settings_)
                  ->altitude),
          "wgs84", &center_wgs84))
    return false;

  (*node)["altitude"] =
      std::static_pointer_cast<CircleTrajectory::Settings>(settings_)->altitude;
  (*node)["velocity"] =
      std::static_pointer_cast<CircleTrajectory::Settings>(settings_)->velocity;
  (*node)["trajectory_type"] = static_cast<int>(TrajectoryType::Hotpoint);
  (*node)["center_lat"] = center_wgs84.x();
  (*node)["center_lon"] = center_wgs84.y();
  (*node)["radius"] =
      std::static_pointer_cast<CircleTrajectory::Settings>(settings_)->radius;
  (*node)["start_heading_deg"] =
      std::static_pointer_cast<CircleTrajectory::Settings>(settings_)
          ->start_heading_deg;
  (*node)["arc_length_deg"] =
      std::static_pointer_cast<CircleTrajectory::Settings>(settings_)
          ->arc_length_deg;
  (*node)["offset_heading_deg"] =
      std::static_pointer_cast<CircleTrajectory::Settings>(settings_)
          ->offset_heading_deg;
  (*node)["circle_deviation_ratio"] =
      std::static_pointer_cast<CircleTrajectory::Settings>(settings_)
          ->circle_deviation_ratio;

  return true;
}

void CircleTrajectory::samplePositionVertices() {
  const Eigen::Vector3d kOffset(
      std::static_pointer_cast<CircleTrajectory::Settings>(settings_)
          ->center_east,
      std::static_pointer_cast<CircleTrajectory::Settings>(settings_)
          ->center_north,
      std::static_pointer_cast<CircleTrajectory::Settings>(settings_)
          ->altitude);
  const double kAngularVelocity =
      std::static_pointer_cast<CircleTrajectory::Settings>(settings_)
          ->computeAngularVelocity();
  const double kDeltaAngle = computeDeltaAngle(kAngularVelocity);

  position_vertices_.resize(num_vertices_, kPositionDimension);
  for (int i = 0; i < num_vertices_; ++i) {
    const double kAngle = i * kDeltaAngle + start_rad_;

    Eigen::Vector3d position;
    position.x() =
        std::static_pointer_cast<CircleTrajectory::Settings>(settings_)
            ->radius *
        std::cos(kAngle);
    position.y() =
        std::static_pointer_cast<CircleTrajectory::Settings>(settings_)
            ->radius *
        std::sin(kAngle);
    position.z() = 0.0;
    if (kMaxDerivativePos >= mtg::derivative_order::POSITION) {
      position_vertices_[i].addConstraint(mtg::derivative_order::POSITION,
                                          position + kOffset);
    }

    Eigen::Vector3d velocity;
    velocity.x() =
        -std::static_pointer_cast<CircleTrajectory::Settings>(settings_)
             ->radius *
        kAngularVelocity * std::sin(kAngle);
    velocity.y() =
        std::static_pointer_cast<CircleTrajectory::Settings>(settings_)
            ->radius *
        kAngularVelocity * std::cos(kAngle);
    velocity.z() = 0.0;
    if (kMaxDerivativePos >= mtg::derivative_order::VELOCITY) {
      position_vertices_[i].addConstraint(mtg::derivative_order::VELOCITY,
                                          velocity);
    }

    if (kMaxDerivativePos >= mtg::derivative_order::ACCELERATION) {
      Eigen::Vector3d acceleration;
      acceleration.x() = -std::pow(kAngularVelocity, 2.0) * position.x();
      acceleration.y() = -std::pow(kAngularVelocity, 2.0) * position.y();
      acceleration.z() = 0.0;
      position_vertices_[i].addConstraint(mtg::derivative_order::ACCELERATION,
                                          acceleration);
    }

    if (kMaxDerivativePos >= mtg::derivative_order::JERK) {
      Eigen::Vector3d jerk;
      jerk.x() = -std::pow(kAngularVelocity, 3.0) * velocity.x();
      jerk.y() = -std::pow(kAngularVelocity, 3.0) * velocity.y();
      jerk.z() = 0.0;
      position_vertices_[i].addConstraint(mtg::derivative_order::JERK, jerk);
    }

    if (kMaxDerivativePos >= mtg::derivative_order::SNAP) {
      Eigen::Vector3d snap;
      snap.x() = std::pow(kAngularVelocity, 4.0) * position.x();
      snap.y() = std::pow(kAngularVelocity, 4.0) * position.y();
      snap.z() = 0.0;
      position_vertices_[i].addConstraint(mtg::derivative_order::SNAP, snap);
    }
  }
}

void CircleTrajectory::sampleYawVertices() {
  const double kOffsetHeading = angles::from_degrees(
      std::static_pointer_cast<CircleTrajectory::Settings>(settings_)
          ->offset_heading_deg);
  const double kAngularVelocity =
      std::static_pointer_cast<CircleTrajectory::Settings>(settings_)
          ->computeAngularVelocity();
  const double kDeltaAngle = computeDeltaAngle(kAngularVelocity);

  yaw_vertices_.resize(num_vertices_, kYawDimension);
  for (int i = 0; i < num_vertices_; ++i) {
    typedef Eigen::Matrix<double, 1, 1> Vector1d;
    if (kMaxDerivativeYaw >= mtg::derivative_order::ORIENTATION) {
      Vector1d orientation;
      const double kAngle = i * kDeltaAngle + start_rad_;
      orientation[0] = kAngle + 0.5 * M_PI + kOffsetHeading;
      yaw_vertices_[i].addConstraint(mtg::derivative_order::ORIENTATION,
                                     orientation);
    }

    if (kMaxDerivativeYaw >= mtg::derivative_order::ANGULAR_VELOCITY) {
      Vector1d angular_velocity;
      angular_velocity[0] = kAngularVelocity;
      yaw_vertices_[i].addConstraint(mtg::derivative_order::ANGULAR_VELOCITY,
                                     angular_velocity);
    }

    if (kMaxDerivativeYaw >= mtg::derivative_order::ANGULAR_ACCELERATION) {
      Vector1d angular_acceleration;
      angular_acceleration[0] = 0.0;
      yaw_vertices_[i].addConstraint(
          mtg::derivative_order::ANGULAR_ACCELERATION, angular_acceleration);
    }
  }
}

double CircleTrajectory::computeDeltaAngle(double angular_velocity) const {
  return std::copysign(arc_length_rad_ / (num_vertices_ - 1), angular_velocity);
}

void CircleTrajectory::sampleTimes() {
  const double kAngularVelocity =
      std::static_pointer_cast<CircleTrajectory::Settings>(settings_)
          ->computeAngularVelocity();
  const double kDeltaAngle = computeDeltaAngle(kAngularVelocity);
  const double kSamplePeriod = std::fabs(kDeltaAngle / kAngularVelocity);

  segment_times_.resize(position_vertices_.size() - 1);
  for (double& t : segment_times_) t = kSamplePeriod;
}

}  // namespace fm_trajectories


/* test_old_uniform_int_distribution.cpp
 *
 * Copyright Steven Watanabe 2011
 * 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)
 *
 * $Id$
 *
 */

#include <boost/random/uniform_int.hpp>
#include <limits>

#define BOOST_RANDOM_DISTRIBUTION boost::uniform_int<>
#define BOOST_RANDOM_ARG1 a
#define BOOST_RANDOM_ARG2 b
#define BOOST_RANDOM_ARG1_DEFAULT 0
#define BOOST_RANDOM_ARG2_DEFAULT 9
#define BOOST_RANDOM_ARG1_VALUE 5
#define BOOST_RANDOM_ARG2_VALUE 250

#define BOOST_RANDOM_DIST0_MIN 0
#define BOOST_RANDOM_DIST0_MAX 9
#define BOOST_RANDOM_DIST1_MIN 5
#define BOOST_RANDOM_DIST1_MAX 9
#define BOOST_RANDOM_DIST2_MIN 5
#define BOOST_RANDOM_DIST2_MAX 250

#define BOOST_RANDOM_TEST1_PARAMS (0, 9)
#define BOOST_RANDOM_TEST1_MIN 0
#define BOOST_RANDOM_TEST1_MAX 9

#define BOOST_RANDOM_TEST2_PARAMS (10, 19)
#define BOOST_RANDOM_TEST2_MIN 10
#define BOOST_RANDOM_TEST2_MAX 19

#include "test_distribution.ipp"

#define BOOST_RANDOM_UNIFORM_INT boost::uniform_int

#include "test_uniform_int.ipp"

#include <algorithm>
#include <boost/random/random_number_generator.hpp>

// Test that uniform_int<> can be used with std::random_shuffle
// Author: Jos Hickson
BOOST_AUTO_TEST_CASE(test_random_shuffle)
{
#ifndef BOOST_NO_CXX98_RANDOM_SHUFFLE
    typedef boost::uniform_int<> distribution_type;
    typedef boost::variate_generator<boost::mt19937 &, distribution_type> generator_type;

    boost::mt19937 engine1(1234);
    boost::mt19937 engine2(1234);

    boost::random::random_number_generator<boost::mt19937> referenceRand(engine1);

    distribution_type dist(0,10);
    generator_type testRand(engine2, dist);

    std::vector<int> referenceVec;

    for (int i = 0; i < 200; ++i) {
        referenceVec.push_back(i);
    }

    std::vector<int> testVec(referenceVec);

    std::random_shuffle(referenceVec.begin(), referenceVec.end(), referenceRand);
    std::random_shuffle(testVec.begin(), testVec.end(), testRand);

    BOOST_CHECK_EQUAL_COLLECTIONS(
        testVec.begin(), testVec.end(),
        referenceVec.begin(), referenceVec.end());
#endif
}


/*

Copyright (c) 2005-2019, University of Oxford.
All rights reserved.

University of Oxford means the Chancellor, Masters and Scholars of the
University of Oxford, having an administrative office at Wellington
Square, Oxford OX1 2JD, UK.

This file is part of Chaste.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
 * Redistributions of source code must retain the above copyright notice,
   this list of conditions and the following disclaimer.
 * Redistributions in binary form must reproduce the above copyright notice,
   this list of conditions and the following disclaimer in the documentation
   and/or other materials provided with the distribution.
 * Neither the name of the University of Oxford nor the names of its
   contributors may be used to endorse or promote products derived from this
   software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/

#ifndef TYSONNOVAK2001ODESYSTEM_HPP_
#define TYSONNOVAK2001ODESYSTEM_HPP_

#include "ChasteSerialization.hpp"
#include <boost/serialization/base_object.hpp>

#include <cmath>
#include <iostream>

#include "AbstractOdeSystemWithAnalyticJacobian.hpp"

/**
 * Represents the Tyson & Novak (2001) system of ODEs.
 * [doi:10.1006/jtbi.2001.2293]
 */
class TysonNovak2001OdeSystem : public AbstractOdeSystemWithAnalyticJacobian
{
private:

    /**
     * Parameters for the Tyson & Novak (2001) model.
     */

    /** Dimensional parameter k_1. */
    double mK1;
    /** Dimensional parameter k_2'. */
    double mK2d;
    /** Dimensional parameter k_2''. */
    double mK2dd;
    /** Dimensional parameter k_2'''. */
    double mK2ddd;
    /** Dimensionless parameter [CycB]_threshold. */
    double mCycB_threshold;
    /** Dimensional parameter k_3'. */
    double mK3d;
    /** Dimensional parameter k_3''. */
    double mK3dd;
    /** Dimensional parameter k_4'. */
    double mK4d;
    /** Dimensional parameter k_4. */
    double mK4;
    /** Dimensionless parameter J_3. */
    double mJ3;
    /** Dimensionless parameter J_4. */
    double mJ4;
    /** Dimensional parameter k_5'. */
    double mK5d;
    /** Dimensional parameter k_5''. */
    double mK5dd;
    /** Dimensional parameter k_6. */
    double mK6;
    /** Dimensionless parameter J_5. */
    double mJ5;
    /** Dimensionless parameter n. */
    unsigned mN;
    /** Dimensional parameter k_7. */
    double mK7;
    /** Dimensional parameter k_8. */
    double mK8;
    /** Dimensionless parameter J_7. */
    double mJ7;
    /** Dimensionless parameter J_8. */
    double mJ8;
    /** Dimensionless parameter [Mad]. */
    double mMad;
    /** Dimensional parameter k_9. */
    double mK9;
    /** Dimensional parameter k_10. */
    double mK10;
    /** Dimensional parameter k_11. */
    double mK11;
    /** Dimensional parameter k_12'. */
    double mK12d;
    /** Dimensional parameter k_12''. */
    double mK12dd;
    /** Dimensional parameter k_12'''. */
    double mK12ddd;
    /** Dimensionless parameter K_eq. */
    double mKeq;
    /** Dimensional parameter k_13. */
    double mK13;
    /** Dimensional parameter k_14. */
    double mK14;
    /** Dimensional parameter k_15'. */
    double mK15d;
    /** Dimensional parameter k_15''. */
    double mK15dd;
    /** Dimensional parameter k_16'. */
    double mK16d;
    /** Dimensional parameter k_16''. */
    double mK16dd;
    /** Dimensionless parameter J_15. */
    double mJ15;
    /** Dimensionless parameter J_16. */
    double mJ16;
    /** Dimensional parameter mu. */
    double mMu;
    /** Dimensionless parameter m_star. */
    double mMstar;

    friend class boost::serialization::access;
    /**
     * Serialize the object and its member variables.
     *
     * @param archive the archive
     * @param version the current version of this class
     */
    template<class Archive>
    void serialize(Archive & archive, const unsigned int version)
    {
        archive & boost::serialization::base_object<AbstractOdeSystem>(*this);
    }

public:

    /**
     * Constructor.
     *
     * @param stateVariables optional initial conditions for state variables (only used in archiving)
     */
    TysonNovak2001OdeSystem(std::vector<double> stateVariables=std::vector<double>());

    /**
     * Destructor.
     */
    ~TysonNovak2001OdeSystem();

    /**
     * Initialise parameter values.
     */
    void Init();

    /**
     * Compute the RHS of the Alarcon et al. (2004) system of ODEs.
     *
     * Returns a vector representing the RHS of the ODEs at each time step, y' = [y1' ... yn'].
     * An ODE solver will call this function repeatedly to solve for y = [y1 ... yn].
     *
     * @param time used to evaluate the RHS.
     * @param rY value of the solution vector used to evaluate the RHS.
     * @param rDY filled in with the resulting derivatives (using Alarcons et al. (2004) system of equations).
     */
    void EvaluateYDerivatives(double time, const std::vector<double>& rY, std::vector<double>& rDY);

    /**
     * Calculate whether the conditions for the cell cycle to finish have been met.
     * (Used by Chaste solvers to find whether or not to stop solving)
     *
     * @param time at which to calculate whether the stopping event has occurred
     * @param rY value of the solution vector used to evaluate the RHS.
     *
     * @return whether or not stopping conditions have been met
     */
    bool CalculateStoppingEvent(double time, const std::vector<double>& rY);

    /**
     * Calculate whether the conditions for the cell cycle to finish have been met.
     * (Used by CVODE solver to find exact stopping position)
     *
     * @param time at which to calculate whether the stopping event has occurred
     * @param rY value of the solution vector used to evaluate the RHS.
     *
     * @return How close we are to the root of the stopping condition
     */
    double CalculateRootFunction(double time, const std::vector<double>& rY);

    /**
     * Compute the Jacobian of the ODE system.
     *
     * @param rSolutionGuess initial guess for the solution vector.
     * @param jacobian the Jacobian of the ODE system.
     * @param time at which to calculate the Jacobian.
     * @param timeStep used to calculate the Jacobian.
     */
    virtual void AnalyticJacobian(const std::vector<double>& rSolutionGuess, double** jacobian, double time, double timeStep);
};

// Declare identifier for the serializer
#include "SerializationExportWrapper.hpp"
CHASTE_CLASS_EXPORT(TysonNovak2001OdeSystem)

namespace boost
{
namespace serialization
{
/**
 * Serialize information required to construct a TysonNovak2001OdeSystem.
 */
template<class Archive>
inline void save_construct_data(
    Archive & ar, const TysonNovak2001OdeSystem * t, const unsigned int file_version)
{
    // Save data required to construct instance
    const std::vector<double>& state_variables = t->rGetConstStateVariables();
    ar & state_variables;
}

/**
 * De-serialize constructor parameters and initialise a TysonNovak2001OdeSystem.
 */
template<class Archive>
inline void load_construct_data(
    Archive & ar, TysonNovak2001OdeSystem * t, const unsigned int file_version)
{
    // Retrieve data from archive required to construct new instance
    std::vector<double> state_variables;
    ar & state_variables;

    // Invoke inplace constructor to initialise instance
    ::new(t)TysonNovak2001OdeSystem(state_variables);
}
}
} // namespace ...

#endif /*TYSONNOVAK2001ODESYSTEM_HPP_*/


/**
 * @file test/ellipse_iterator.cpp
 */
/*****************************************************************************
** Includes
*****************************************************************************/

#include "../include/cost_map_core/common.hpp"
#include <Eigen/Core>

// gtest
#include <gtest/gtest.h>

// Limits
#include <cfloat>

// Vector
#include <vector>
#include "../include/cost_map_core/cost_map.hpp"
#include "../include/cost_map_core/iterators/ellipse_iterator.hpp"

using namespace std;
using namespace Eigen;

TEST(EllipseIterator, OneCellWideEllipse)
{
  cost_map::CostMap map( { "types" });
  map.setGeometry(cost_map::Length(8.0, 5.0), 1.0, cost_map::Position(0.0, 0.0));

  cost_map::EllipseIterator iterator(map, cost_map::Position(0.0, 0.0), cost_map::Length(8.0, 1.0));

  EXPECT_FALSE(iterator.isPastEnd());
  EXPECT_EQ(0, (*iterator)(0));
  EXPECT_EQ(2, (*iterator)(1));

  ++iterator;
  EXPECT_FALSE(iterator.isPastEnd());
  EXPECT_EQ(1, (*iterator)(0));
  EXPECT_EQ(2, (*iterator)(1));

  ++iterator;
  EXPECT_FALSE(iterator.isPastEnd());
  EXPECT_EQ(2, (*iterator)(0));
  EXPECT_EQ(2, (*iterator)(1));

  ++iterator;
  ++iterator;
  ++iterator;
  ++iterator;
  ++iterator;
  EXPECT_FALSE(iterator.isPastEnd());
  EXPECT_EQ(7, (*iterator)(0));
  EXPECT_EQ(2, (*iterator)(1));

  ++iterator;
  EXPECT_TRUE(iterator.isPastEnd());
}

int main(int argc, char **argv)
{
  testing::InitGoogleTest(&argc, argv);
  srand((int)time(0));
  return RUN_ALL_TESTS();
}


/*
 *
 * Copyright (c) Toon Knapen & Kresimir Fresl 2003
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * KF acknowledges the support of the Faculty of Civil Engineering,
 * University of Zagreb, Croatia.
 *
 */

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_GESDD_HPP
#define BOOST_NUMERIC_BINDINGS_LAPACK_GESDD_HPP

#include <boost/numeric/bindings/traits/type_traits.hpp>
#include <boost/numeric/bindings/traits/traits.hpp>
#include <boost/numeric/bindings/lapack/lapack.h>
#include <boost/numeric/bindings/traits/detail/array.hpp>
#include <boost/numeric/bindings/traits/detail/utils.hpp>

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

#include <cassert>

namespace boost { namespace numeric { namespace bindings {

  namespace lapack {

    ///////////////////////////////////////////////////////////////////
    //
    // singular value decomposition
    //
    ///////////////////////////////////////////////////////////////////

    /*
     * (divide and conquer driver)
     * gesdd() computes the singular value decomposition (SVD) of
     * M-by-N matrix A, optionally computing the left and/or right
     * singular vectors, by using divide-and-conquer method.
     * The SVD is written
     *
     *     A = U * S * V^T    or    A = U * S * V^H
     *
     * where S is an M-by-N matrix which is zero except for its min(m,n)
     * diagonal elements, U is an M-by-M orthogonal/unitary matrix, and V
     * is an N-by-N orthogonal/unitary matrix. The diagonal elements of S
     * are the singular values of A; they are real and non-negative, and
     * are returnede in descending  order. The first min(m,n) columns of
     * U and V are the left and right singular vectors of A. (Note that
     * the routine returns V^T or V^H, not V.
     */

    namespace detail {

      inline
      void gesdd (char const jobz, int const m, int const n,
                  float* a, int const lda,
                  float* s, float* u, int const ldu,
                  float* vt, int const ldvt,
                  float* work, int const lwork, float* /* dummy */,
                  int* iwork, int* info)
      {
        LAPACK_SGESDD (&jobz, &m, &n, a, &lda, s,
                       u, &ldu, vt, &ldvt, work, &lwork, iwork, info);
      }

      inline
      void gesdd (char const jobz, int const m, int const n,
                  double* a, int const lda,
                  double* s, double* u, int const ldu,
                  double* vt, int const ldvt,
                  double* work, int const lwork, double* /* dummy */,
                  int* iwork, int* info)
      {
        LAPACK_DGESDD (&jobz, &m, &n, a, &lda, s,
                       u, &ldu, vt, &ldvt, work, &lwork, iwork, info);
      }

      inline
      void gesdd (char const jobz, int const m, int const n,
                  traits::complex_f* a, int const lda,
                  float* s, traits::complex_f* u, int const ldu,
                  traits::complex_f* vt, int const ldvt,
                  traits::complex_f* work, int const lwork,
                  float* rwork, int* iwork, int* info)
      {
        LAPACK_CGESDD (&jobz, &m, &n,
                       traits::complex_ptr (a), &lda, s,
                       traits::complex_ptr (u), &ldu,
                       traits::complex_ptr (vt), &ldvt,
                       traits::complex_ptr (work), &lwork,
                       rwork, iwork, info);
      }

      inline
      void gesdd (char const jobz, int const m, int const n,
                  traits::complex_d* a, int const lda,
                  double* s, traits::complex_d* u, int const ldu,
                  traits::complex_d* vt, int const ldvt,
                  traits::complex_d* work, int const lwork,
                  double* rwork, int* iwork, int* info)
      {
        LAPACK_ZGESDD (&jobz, &m, &n,
                       traits::complex_ptr (a), &lda, s,
                       traits::complex_ptr (u), &ldu,
                       traits::complex_ptr (vt), &ldvt,
                       traits::complex_ptr (work), &lwork,
                       rwork, iwork, info);
      }

      inline
      int gesdd_min_work (float, char jobz, int m, int n) {
        int minmn = m < n ? m : n;
        int maxmn = m < n ? n : m;
        int m3 = 3 * minmn;
        int m4 = 4 * minmn;
        int minw;
        if (jobz == 'N') {
          // leading comments:
          //   LWORK >= 3*min(M,N) + max(max(M,N), 6*min(M,N))
          // code:
          //   LWORK >= 3*min(M,N) + max(max(M,N), 7*min(M,N))
          int m7 = 7 * minmn;
          minw = maxmn < m7 ? m7 : maxmn;
          minw += m3;
        }
        if (jobz == 'O') {
          // LWORK >= 3*min(M,N)*min(M,N)
          //          + max(max(M,N), 5*min(M,N)*min(M,N)+4*min(M,N))
          int m5 = 5 * minmn * minmn + m4;
          minw = maxmn < m5 ? m5 : maxmn;
          minw += m3 * minmn;
        }
        if (jobz == 'S' || jobz == 'A') {
          // LWORK >= 3*min(M,N)*min(M,N)
          //          + max(max(M,N), 4*min(M,N)*min(M,N)+4*min(M,N)).
          int m44 = m4 * minmn + m4;
          minw = maxmn < m44 ? m44 : maxmn;
          minw += m3 * minmn;
        }
        return minw;
      }
      inline
      int gesdd_min_work (double, char jobz, int m, int n) {
        int minmn = m < n ? m : n;
        int maxmn = m < n ? n : m;
        int m3 = 3 * minmn;
        int m4 = 4 * minmn;
        int minw;
        if (jobz == 'N') {
          // leading comments:
          //   LWORK >= 3*min(M,N) + max(max(M,N), 6*min(M,N))
          // code:
          //   LWORK >= 3*min(M,N) + max(max(M,N), 7*min(M,N))
          int m7 = 7 * minmn;
          minw = maxmn < m7 ? m7 : maxmn;
          minw += m3;
        }
        else if (jobz == 'O') {
          // LWORK >= 3*min(M,N)*min(M,N)
          //          + max(max(M,N), 5*min(M,N)*min(M,N)+4*min(M,N))
          int m5 = 5 * minmn * minmn + m4;
          minw = maxmn < m5 ? m5 : maxmn;
          minw += m3 * minmn;
        }
        else if (jobz == 'S' || jobz == 'A') {
          // LWORK >= 3*min(M,N)*min(M,N)
          //          + max(max(M,N), 4*min(M,N)*min(M,N)+4*min(M,N)).
          int m44 = m4 * minmn + m4;
          minw = maxmn < m44 ? m44 : maxmn;
          minw += m3 * minmn;
        }
        else {
          std::cerr << "Invalid option passed to gesdd" << std::endl ;
          return 0 ;
        }
        return minw;
      }
      inline
      int gesdd_min_work (traits::complex_f, char jobz, int m, int n) {
        int minmn = m < n ? m : n;
        int maxmn = m < n ? n : m;
        int m2 = 2 * minmn;
        int minw = m2 + maxmn;
        if (jobz == 'N')
          // LWORK >= 2*min(M,N)+max(M,N)
          ;
        if (jobz == 'O')
          // LWORK >= 2*min(M,N)*min(M,N) + 2*min(M,N) + max(M,N)
          minw += m2 * minmn;
        if (jobz == 'S' || jobz == 'A')
          // LWORK >= min(M,N)*min(M,N) + 2*min(M,N) + max(M,N)
          minw += minmn * minmn;
        return minw;
      }
      inline
      int gesdd_min_work (traits::complex_d, char jobz, int m, int n) {
        int minmn = m < n ? m : n;
        int maxmn = m < n ? n : m;
        int m2 = 2 * minmn;
        int minw = m2 + maxmn;
        if (jobz == 'N')
          // LWORK >= 2*min(M,N)+max(M,N)
          ;
        if (jobz == 'O')
          // LWORK >= 2*min(M,N)*min(M,N) + 2*min(M,N) + max(M,N)
          minw += m2 * minmn;
        if (jobz == 'S' || jobz == 'A')
          // LWORK >= min(M,N)*min(M,N) + 2*min(M,N) + max(M,N)
          minw += minmn * minmn;
        return minw;
      }

      inline
      int gesdd_rwork (float, char, int, int) { return 1; }
      inline
      int gesdd_rwork (double, char, int, int) { return 1; }
      inline
      int gesdd_rwork (traits::complex_f, char jobz, int m, int n) {
        int minmn = m < n ? m : n;
        int minw;
        if (jobz == 'N')
          // LWORK >= 7*min(M,N)
          minw = 7 * minmn;
        else
          // LRWORK >= 5*min(M,N)*min(M,N) + 5*min(M,N)
          minw = 5 * (minmn * minmn + minmn);
        return minw;
      }
      inline
      int gesdd_rwork (traits::complex_d, char jobz, int m, int n) {
        int minmn = m < n ? m : n;
        int minw;
        if (jobz == 'N')
          // LWORK >= 7*min(M,N)
          minw = 7 * minmn;
        else
          // LRWORK >= 5*min(M,N)*min(M,N) + 5*min(M,N)
          minw = 5 * (minmn * minmn + minmn);
        return minw;
      }

      inline
      int gesdd_iwork (int m, int n) {
        int minmn = m < n ? m : n;
        return 8 * minmn;
      }

    } // detail

    template <typename MatrA>
    inline
    int gesdd_work (char const q, char const jobz, MatrA const& a)
    {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrA>::matrix_structure,
        traits::general_t
      >::value));
#endif

#ifdef BOOST_NUMERIC_BINDINGS_LAPACK_2
      assert (q == 'M');
#else
      assert (q == 'M' || q == 'O');
#endif
      assert (jobz == 'N' || jobz == 'O' || jobz == 'A' || jobz == 'S');

      int const m = traits::matrix_size1 (a);
      int const n = traits::matrix_size2 (a);
      int lw = -13;

#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<MatrA>::value_type val_t;
#else
      typedef typename MatrA::value_type val_t;
#endif

      if (q == 'M')
        lw = detail::gesdd_min_work (val_t(), jobz, m, n);

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_2
      MatrA& a2 = const_cast<MatrA&> (a);
      if (q == 'O') {
        // traits::detail::array<val_t> w (1);
        val_t w;
        int info;
        detail::gesdd (jobz, m, n,
                       traits::matrix_storage (a2),
                       traits::leading_dimension (a2),
                       0, // traits::vector_storage (s),
                       0, // traits::matrix_storage (u),
                       m, // traits::leading_dimension (u),
                       0, // traits::matrix_storage (vt),
                       n, // traits::leading_dimension (vt),
                       &w, // traits::vector_storage (w),
                       -1, // traits::vector_size (w),
                       0, // traits::vector_storage (rw),
                       0, // traits::vector_storage (iw),
                       &info);
        assert (info == 0);

        lw = traits::detail::to_int (w);
        // // lw = traits::detail::to_int (w[0]);
        /*
         * is there a bug in LAPACK? or in Mandrake's .rpm?
         * if m == 3, n == 4 and jobz == 'N' (real A),
         * gesdd() returns optimal size == 1 while minimum size == 27
         */
        // int lwo = traits::detail::to_int (w);
        // int lwmin = detail::gesdd_min_work (val_t(), jobz, m, n);
        // lw = lwo < lwmin ? lwmin : lwo;
      }
#endif

      return lw;
    }

    template <typename MatrA>
    inline
    int gesdd_rwork (char jobz, MatrA const& a) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrA>::matrix_structure,
        traits::general_t
      >::value));
#endif

      assert (jobz == 'N' || jobz == 'O' || jobz == 'A' || jobz == 'S');

#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<MatrA>::value_type val_t;
#else
      typedef typename MatrA::value_type val_t;
#endif

      return detail::gesdd_rwork (val_t(), jobz,
                                  traits::matrix_size1 (a),
                                  traits::matrix_size2 (a));
    }

    template <typename MatrA>
    inline
    int gesdd_iwork (MatrA const& a) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrA>::matrix_structure,
        traits::general_t
      >::value));
#endif

      return detail::gesdd_iwork (traits::matrix_size1 (a),
                                  traits::matrix_size2 (a));
    }

    template <typename MatrA, typename VecS,
              typename MatrU, typename MatrV, typename VecW, typename VecIW>
    inline
    int gesdd (char const jobz, MatrA& a,
               VecS& s, MatrU& u, MatrV& vt, VecW& w, VecIW& iw)
    {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrA>::matrix_structure,
        traits::general_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrU>::matrix_structure,
        traits::general_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrV>::matrix_structure,
        traits::general_t
      >::value));

      BOOST_STATIC_ASSERT(
        (boost::is_same<
          typename traits::matrix_traits<MatrA>::value_type, float
        >::value
        ||
        boost::is_same<
          typename traits::matrix_traits<MatrA>::value_type, double
        >::value));
#endif

      int const m = traits::matrix_size1 (a);
      int const n = traits::matrix_size2 (a);
      int const minmn = m < n ? m : n;

      assert (minmn == traits::vector_size (s));
      assert ((jobz == 'N')
              || ((jobz == 'O' || jobz == 'A') && m >= n)
              || ((jobz == 'O' || jobz == 'A')
                  && m < n
                  && m == traits::matrix_size2 (u))
              || (jobz == 'S' && minmn == traits::matrix_size2 (u)));
      assert ((jobz == 'N' && traits::leading_dimension (u) >= 1)
              || (jobz == 'O'
                  && m >= n
                  && traits::leading_dimension (u) >= 1)
              || (jobz == 'O'
                  && m < n
                  && traits::leading_dimension (u) >= m)
              || (jobz == 'A' && traits::leading_dimension (u) >= m)
              || (jobz == 'S' && traits::leading_dimension (u) >= m));
      assert (n == traits::matrix_size2 (vt));
      assert ((jobz == 'N' && traits::leading_dimension (vt) >= 1)
              || (jobz == 'O'
                  && m < n
                  && traits::leading_dimension (vt) >= 1)
              || (jobz == 'O'
                  && m >= n
                  && traits::leading_dimension (vt) >= n)
              || (jobz == 'A' && traits::leading_dimension (vt) >= n)
              || (jobz == 'S' && traits::leading_dimension (vt) >= minmn));
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<MatrA>::value_type val_t;
#else
      typedef typename MatrA::value_type val_t;
#endif
      assert (traits::vector_size (w)
              >= detail::gesdd_min_work (val_t(), jobz, m, n));
      assert (traits::vector_size (iw) >= detail::gesdd_iwork (m, n));

      int info;
      detail::gesdd (jobz, m, n,
                     traits::matrix_storage (a),
                     traits::leading_dimension (a),
                     traits::vector_storage (s),
                     traits::matrix_storage (u),
                     traits::leading_dimension (u),
                     traits::matrix_storage (vt),
                     traits::leading_dimension (vt),
                     traits::vector_storage (w),
                     traits::vector_size (w),
                     0, // dummy argument
                     traits::vector_storage (iw),
                     &info);
      return info;
    }

    template <typename MatrA, typename VecS, typename MatrU,
              typename MatrV, typename VecW, typename VecRW, typename VecIW>
    inline
    int gesdd (char const jobz, MatrA& a,
               VecS& s, MatrU& u, MatrV& vt, VecW& w, VecRW& rw, VecIW& iw)
    {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrA>::matrix_structure,
        traits::general_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrU>::matrix_structure,
        traits::general_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrV>::matrix_structure,
        traits::general_t
      >::value));
#endif

      int const m = traits::matrix_size1 (a);
      int const n = traits::matrix_size2 (a);
      int const minmn = m < n ? m : n;

      assert (minmn == traits::vector_size (s));
      assert ((jobz == 'N')
              || ((jobz == 'O' || jobz == 'A') && m >= n)
              || ((jobz == 'O' || jobz == 'A')
                  && m < n
                  && m == traits::matrix_size2 (u))
              || (jobz == 'S' && minmn == traits::matrix_size2 (u)));
      assert ((jobz == 'N' && traits::leading_dimension (u) >= 1)
              || (jobz == 'O'
                  && m >= n
                  && traits::leading_dimension (u) >= 1)
              || (jobz == 'O'
                  && m < n
                  && traits::leading_dimension (u) >= m)
              || (jobz == 'A' && traits::leading_dimension (u) >= m)
              || (jobz == 'S' && traits::leading_dimension (u) >= m));
      assert (n == traits::matrix_size2 (vt));
      assert ((jobz == 'N' && traits::leading_dimension (vt) >= 1)
              || (jobz == 'O'
                  && m < n
                  && traits::leading_dimension (vt) >= 1)
              || (jobz == 'O'
                  && m >= n
                  && traits::leading_dimension (vt) >= n)
              || (jobz == 'A' && traits::leading_dimension (vt) >= n)
              || (jobz == 'S' && traits::leading_dimension (vt) >= minmn));
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<MatrA>::value_type val_t;
#else
      typedef typename MatrA::value_type val_t;
#endif
      assert (traits::vector_size (w)
              >= detail::gesdd_min_work (val_t(), jobz, m, n));
      assert (traits::vector_size (rw)
              >= detail::gesdd_rwork (val_t(), jobz, m, n));
      assert (traits::vector_size (iw) >= detail::gesdd_iwork (m, n));

      int info;
      detail::gesdd (jobz, m, n,
                     traits::matrix_storage (a),
                     traits::leading_dimension (a),
                     traits::vector_storage (s),
                     traits::matrix_storage (u),
                     traits::leading_dimension (u),
                     traits::matrix_storage (vt),
                     traits::leading_dimension (vt),
                     traits::vector_storage (w),
                     traits::vector_size (w),
                     traits::vector_storage (rw),
                     traits::vector_storage (iw),
                     &info);
      return info;
    }

    template <typename MatrA, typename VecS, typename MatrU, typename MatrV>
    inline
    int gesdd (char const opt, char const jobz,
               MatrA& a, VecS& s, MatrU& u, MatrV& vt)
    {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrA>::matrix_structure,
        traits::general_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrU>::matrix_structure,
        traits::general_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrV>::matrix_structure,
        traits::general_t
      >::value));
#endif

      int const m = traits::matrix_size1 (a);
      int const n = traits::matrix_size2 (a);
#ifndef NDEBUG
      int const minmn = m < n ? m : n;
#endif // NDEBUG

      assert (minmn == traits::vector_size (s));
      assert ((jobz == 'N')
              || ((jobz == 'O' || jobz == 'A') && m >= n)
              || ((jobz == 'O' || jobz == 'A')
                  && m < n
                  && m == traits::matrix_size2 (u))
              || (jobz == 'S' && minmn == traits::matrix_size2 (u)));
      assert ((jobz == 'N' && traits::leading_dimension (u) >= 1)
              || (jobz == 'O'
                  && m >= n
                  && traits::leading_dimension (u) >= 1)
              || (jobz == 'O'
                  && m < n
                  && traits::leading_dimension (u) >= m)
              || (jobz == 'A' && traits::leading_dimension (u) >= m)
              || (jobz == 'S' && traits::leading_dimension (u) >= m));
      assert (n == traits::matrix_size2 (vt));
      assert ((jobz == 'N' && traits::leading_dimension (vt) >= 1)
              || (jobz == 'O'
                  && m < n
                  && traits::leading_dimension (vt) >= 1)
              || (jobz == 'O'
                  && m >= n
                  && traits::leading_dimension (vt) >= n)
              || (jobz == 'A' && traits::leading_dimension (vt) >= n)
              || (jobz == 'S' && traits::leading_dimension (vt) >= minmn));

#ifdef BOOST_NUMERIC_BINDINGS_LAPACK_2
      assert (opt == 'M');
#else
      assert (opt == 'M' || opt == 'O');
#endif

#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<MatrA>::value_type val_t;
#else
      typedef typename MatrA::value_type val_t;
#endif
      typedef typename traits::type_traits<val_t>::real_type real_t;

      int const lw = gesdd_work (opt, jobz, a);
      traits::detail::array<val_t> w (lw);
      if (!w.valid()) return -101;

      int const lrw = gesdd_rwork (jobz, a);
      traits::detail::array<real_t> rw (lrw);
      if (!rw.valid()) return -102;

      int const liw = gesdd_iwork (a);
      traits::detail::array<int> iw (liw);
      if (!iw.valid()) return -103;

      int info;
      detail::gesdd (jobz, m, n,
                     traits::matrix_storage (a),
                     traits::leading_dimension (a),
                     traits::vector_storage (s),
                     traits::matrix_storage (u),
                     traits::leading_dimension (u),
                     traits::matrix_storage (vt),
                     traits::leading_dimension (vt),
                     traits::vector_storage (w),
                     lw, //traits::vector_size (w),
                     traits::vector_storage (rw),
                     traits::vector_storage (iw),
                     &info);
      return info;
    }

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_2

    template <typename MatrA, typename VecS, typename MatrU, typename MatrV>
    inline
    int gesdd (char const jobz, MatrA& a, VecS& s, MatrU& u, MatrV& vt) {
      return gesdd ('O', jobz, a, s, u, vt);
    }

    template <typename MatrA, typename VecS, typename MatrU, typename MatrV>
    inline
    int gesdd (MatrA& a, VecS& s, MatrU& u, MatrV& vt) {
      return gesdd ('O', 'S', a, s, u, vt);
    }

    template <typename MatrA, typename VecS>
    inline
    int gesdd (MatrA& a, VecS& s) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrA>::matrix_structure,
        traits::general_t
      >::value));
#endif

      int const m = traits::matrix_size1 (a);
      int const n = traits::matrix_size2 (a);
      int const minmn = m < n ? m : n;

      assert (minmn == traits::vector_size (s));

#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<MatrA>::value_type val_t;
#else
      typedef typename MatrA::value_type val_t;
#endif
      typedef typename traits::type_traits<val_t>::real_type real_t;

      int const lw = gesdd_work ('O', 'N', a);
      traits::detail::array<val_t> w (lw);
      if (!w.valid()) return -101;

      int const lrw = gesdd_rwork ('N', a);
      traits::detail::array<real_t> rw (lrw);
      if (!rw.valid()) return -102;

      int const liw = gesdd_iwork (a);
      traits::detail::array<int> iw (liw);
      if (!iw.valid()) return -103;

      int info;
      detail::gesdd ('N', m, n,
                     traits::matrix_storage (a),
                     traits::leading_dimension (a),
                     traits::vector_storage (s),
                     0, // traits::matrix_storage (u),
                     1, // traits::leading_dimension (u),
                     0, // traits::matrix_storage (vt),
                     1, // traits::leading_dimension (vt),
                     traits::vector_storage (w),
                     traits::vector_size (w),
                     traits::vector_storage (rw),
                     traits::vector_storage (iw),
                     &info);
      return info;
    }

#endif

  } // namespace lapack

}}}

#endif