/// @file
/// @brief shared pointers to gsl types
/// @details We use gsl for linear algebra routines. However, the interface is C-like and is based on
/// raw pointers. The file contains code to leverage on boost shared pointer and use it with gsl types
/// such as vector and matrix.
///
/// @copyright (c) 2008 CSIRO
/// Australia Telescope National Facility (ATNF)
/// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
/// PO Box 76, Epping NSW 1710, Australia
/// atnf-enquiries@csiro.au
///
/// This file is part of the ASKAP software distribution.
///
/// The ASKAP software distribution is free software: you can redistribute it
/// and/or modify it under the terms of the GNU General Public License as
/// published by the Free Software Foundation; either version 2 of the License,
/// or (at your option) any later version.
///
/// This program is distributed in the hope that it will be useful,
/// but WITHOUT ANY WARRANTY; without even the implied warranty of
/// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
/// GNU General Public License for more details.
///
/// You should have received a copy of the GNU General Public License
/// along with this program; if not, write to the Free Software
/// Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
///
/// @author Max Voronkov <maxim.voronkov@csiro.au>

#ifndef ASKAP_SCIMATH_SHARED_GSL_TYPES_H
#define ASKAP_SCIMATH_SHARED_GSL_TYPES_H

// boost includes
#include <boost/shared_ptr.hpp>
#include <boost/static_assert.hpp>

// gsl includes
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_eigen.h>

// own includes
#include "askap/AskapError.h"

namespace askap {

namespace utility {

// typedefs for shared pointers, we can add more here when needed

/// @brief shared pointer to gsl vector
/// @ingroup utils
typedef boost::shared_ptr<gsl_vector> SharedGSLVector;

/// @brief shared pointer to gsl matrix
/// @ingroup utils
typedef boost::shared_ptr<gsl_matrix> SharedGSLMatrix;

/// @brief custom deleter for gsl types
/// @details This is a custom deleter class to allow deallocation of
/// a gsl object. Exact operation should be present in specialised types
/// @ingroup utils
template<typename GSLType> struct CustomGSLDeleter {
    /// @brief this method frees the object
    /// @param[in] obj pointer to gsl object
    void operator()(GSLType *obj) const { BOOST_STATIC_ASSERT_MSG(sizeof(GSLType) == 0,
                            "Generic version is not supposed to be used - specialise it for your type"); }
};

/// @brief specialised deleter for a gsl vector
/// @ingroup utils
template<> struct CustomGSLDeleter<gsl_vector> {
    /// @brief this method frees the object
    /// @param[in] obj pointer to gsl vector
    void operator()(gsl_vector *obj) const;
};

/// @brief specialised deleter for a gsl matrix
/// @ingroup utils
template<> struct CustomGSLDeleter<gsl_matrix> {
    /// @brief this method frees the object
    /// @param[in] obj pointer to gsl matrix
    void operator()(gsl_matrix *obj) const;
};

/// @brief specialised deleter for a gsl eigen_symmv_workspace
/// @ingroup utils
template<> struct CustomGSLDeleter<gsl_eigen_symmv_workspace> {
    /// @brief this method frees the object
    /// @param[in] obj pointer to gsl eigen_symmv_workspace
    void operator()(gsl_eigen_symmv_workspace *obj) const;
};

/// @brief templated helper method to wrap newly allocated gsl object
/// @details This method processes a pointer to a gsl object and transfers the ownership (i.e.
/// responsibility for deallocation to the boost shared pointer. Templates are used to automatically
/// deduce the object type and add an appropriate deleter.
/// @note the pointer passed as a parameter is tested to be non-zero. Otherwise an exception is thrown.
/// @param[in] obj a pointer to gsl object
/// @return boost shared pointer to the object
template<typename GSLType>
inline boost::shared_ptr<GSLType> createGSLObject(GSLType *obj)
   { ASKAPASSERT(obj); return boost::shared_ptr<GSLType>(obj, CustomGSLDeleter<GSLType>()); }

/// @brief helper method to allocate gsl vector
/// @details This method allocates a gsl vector of requested length and returns a
/// shared pointer.
/// @param[in] size size of the vector
/// @return shared pointer to gsl vector
inline SharedGSLVector createGSLVector(size_t size) { return createGSLObject(gsl_vector_alloc(size));}

/// @brief helper method to allocate gsl matrix
/// @details This method allocates a gsl matrix of requested shape and returns a
/// shared pointer.
/// @param[in] nrow number of rows
/// @param[in] ncol number of columns
/// @return shared pointer to gsl matrix
inline SharedGSLMatrix createGSLMatrix(size_t nrow, size_t ncol) { return createGSLObject(gsl_matrix_alloc(nrow,ncol));}

} // namespace utility

} // namespace askap

#endif // #ifndef ASKAP_SCIMATH_SHARED_GSL_TYPES_H


#ifndef ___BEST_SCORE_SO_FAR_FOR_NLOPT_H____
#define ___BEST_SCORE_SO_FAR_FOR_NLOPT_H____

#include "OptimizedNonlinearController.h"
#include <mutex>
#include <nlopt.h>
#include <thread>

class BestNLOptScoreSoFar
{
public:
	std::mutex my_mutex;
	std::thread* thethread;

	bool force_stop;
	nlopt_opt opt;

	MyNLOPTdata my_nloptdata;
	int num_iterations_so_far;

	double* my_best_params;
	double my_best_score;

	double original_starting_score;
	int which_algorithm;

	BestNLOptScoreSoFar() : my_best_params(nullptr), thethread(nullptr), opt(nullptr), force_stop(false) {}
	BestNLOptScoreSoFar(const BestNLOptScoreSoFar & other) : my_nloptdata(other.my_nloptdata), thethread(other.thethread), my_best_params(other.my_best_params), force_stop(other.force_stop), opt(other.opt) {}

	void Reset() {
		num_iterations_so_far = 0;
		if(my_best_params != nullptr) {free(my_best_params); my_best_params = nullptr;}
		my_best_score = 1e12;
		original_starting_score = 1e12;
		which_algorithm = 0;
		force_stop = false;
		if(opt != nullptr) {nlopt_destroy(opt);}
		opt = nullptr;
	}
};

#endif


#pragma once

#include "concepts/MemoryIntrospection.h"
#include "math/Vector3.h"
#include "pointcloud/PointAttributes.h"

#include <gsl/gsl>
#include <optional>
#include <vector>

// TECH_DEBT Make attributes more dynamic (map<AttributeType, GenericAttribute*>
// or something like that)

/// <summary>
/// Buffer structure that stores point attributes (position, color etc.) for
/// multiple points at once in a structure-of-array fashion. Compared to storing
/// all points as Point structures, this has better performance for data access
/// </summary>
struct PointBuffer
{
  struct PointConstReference;

  /// <summary>
  /// Mutable indirect reference to a single point inside the PointBuffer
  /// </summary>
  struct PointReference
  {
    friend struct PointBuffer;
    friend struct PointConstReference;

    PointReference();
    PointReference(const PointReference&) = default;
    PointReference& operator=(const PointReference&) = default;

    Vector3<double>& position() const;
    Vector3<uint8_t>* rgbColor() const;
    Vector3<float>* normal() const;
    uint16_t* intensity() const;
    uint8_t* classification() const;
    uint8_t* edge_of_flight_line() const;
    double* gps_time() const;
    uint8_t* number_of_returns() const;
    uint8_t* return_number() const;
    uint16_t* point_source_id() const;
    uint8_t* scan_direction_flag() const;
    int8_t* scan_angle_rank() const;
    uint8_t* user_data() const;

  private:
    PointReference(PointBuffer* pointBuffer, size_t index);

    PointBuffer* _pointBuffer;
    size_t _index;
  };

  /// <summary>
  /// Constant indirect reference to a single point inside the PointBuffer
  /// </summary>
  struct PointConstReference
  {
    friend struct PointBuffer;

    PointConstReference();
    PointConstReference(const PointReference& point_reference);
    PointConstReference(const PointConstReference&) = default;
    PointConstReference& operator=(const PointConstReference&) = default;

    const Vector3<double>& position() const;
    const Vector3<uint8_t>* rgbColor() const;
    const Vector3<float>* normal() const;
    const uint16_t* intensity() const;
    const uint8_t* classification() const;
    const uint8_t* edge_of_flight_line() const;
    const double* gps_time() const;
    const uint8_t* number_of_returns() const;
    const uint8_t* return_number() const;
    const uint16_t* point_source_id() const;
    const uint8_t* scan_direction_flag() const;
    const int8_t* scan_angle_rank() const;
    const uint8_t* user_data() const;

  private:
    PointConstReference(PointBuffer const* pointBuffer, size_t index);

    PointBuffer const* _pointBuffer;
    size_t _index;
  };

  /// <summary>
  /// Creates an empty point buffer
  /// </summary>
  PointBuffer();

  /// <summary>
  /// Creates a PointBuffer from the given range of PointReferences
  /// </summary>
  explicit PointBuffer(gsl::span<PointReference> points);
  /// <summary>
  /// Creates a PointBuffer from the given range of PointConstReferences
  /// </summary>
  explicit PointBuffer(gsl::span<PointConstReference> points);

  /// <summary>
  /// Creates a new PointBuffer storing count points. For all passed attribute
  /// vectors, their count has to be equal to the specified count, otherwise a
  /// invalid_argument error is thrown
  /// </summary>
  PointBuffer(size_t count,
              std::vector<Vector3<double>> positions,
              std::vector<Vector3<uint8_t>> rgbColors = {},
              std::vector<Vector3<float>> normals = {},
              std::vector<uint16_t> intensities = {},
              std::vector<uint8_t> classifications = {},
              std::vector<uint8_t> edge_of_flight_lines = {},
              std::vector<double> gps_times = {},
              std::vector<uint8_t> number_of_returns = {},
              std::vector<uint8_t> return_numbers = {},
              std::vector<uint16_t> point_source_ids = {},
              std::vector<uint8_t> scan_direction_flags = {},
              std::vector<int8_t> scan_angle_ranks = {},
              std::vector<uint8_t> user_data = {});

  /**
   * Creates a new PointBuffer storing 'count' default-constructed points with the
   * attributes given by 'attributes'
   */
  PointBuffer(size_t count, const PointAttributes& attributes);

  PointBuffer(const PointBuffer&) = default;
  PointBuffer(PointBuffer&&) = default;

  PointBuffer& operator=(const PointBuffer&) = default;
  PointBuffer& operator=(PointBuffer&&) = default;

  /// <summary>
  /// Push a single point into this PointBuffer. If the point has attributes
  /// that are not defined in this PointBuffer they are ignored. Attributes from
  /// this PointBuffer that are not defined in the given point are filled with
  /// default values
  /// </summary>
  void push_point(PointConstReference point);

  /// <summary>
  /// Returns a constant indirect reference to the point with the given index
  /// </summary>
  PointConstReference get_point(size_t point_index) const;

  /// <summary>
  /// Returns a mutable indirect reference to the point with the given index
  /// </summary>
  PointReference get_point(size_t point_index);

  /// <summary>
  /// Appends the contents of the given PointBuffer to this PointBuffer. This
  /// will copy all attributes that exist from the given buffer into this
  /// buffer. Attributes that exist in this buffer but not in the other buffer
  /// are filled with default values
  /// </summary>
  void append_buffer(const PointBuffer& other);

  /**
   * Applies a new attribute schema to this PointBuffer. Clears all attributes that are not in the
   * new schema and fills all attributes that are in the new schema but weren't in the PointBuffers
   * old schema (i.e. the state of the PointBuffer prior to calling 'apply_schema') with default
   * values
   */
  void apply_schema(const PointAttributes& schema);

  size_t count() const { return _count; }
  bool empty() const { return _count == 0; }
  void clear();
  void shrink_to_fit();
  void resize(size_t new_size);

  std::vector<Vector3<double>>& positions() { return _positions; }
  std::vector<Vector3<uint8_t>>& rgbColors() { return _rgbColors; }
  std::vector<Vector3<float>>& normals() { return _normals; }
  std::vector<uint16_t>& intensities() { return _intensities; }
  std::vector<uint8_t>& classifications() { return _classifications; }

  auto& edge_of_flight_lines() { return _edge_of_flight_lines; }
  auto& gps_times() { return _gps_times; }
  auto& number_of_returns() { return _number_of_returns; }
  auto& return_numbers() { return _return_numbers; }
  auto& point_source_ids() { return _point_source_ids; }
  auto& scan_direction_flags() { return _scan_direction_flags; }
  auto& scan_angle_ranks() { return _scan_angle_ranks; }
  auto& user_data() { return _user_data; }

  const std::vector<Vector3<double>>& positions() const { return _positions; }
  const std::vector<Vector3<uint8_t>>& rgbColors() const { return _rgbColors; }
  const std::vector<Vector3<float>>& normals() const { return _normals; }
  const std::vector<uint16_t>& intensities() const { return _intensities; }
  const std::vector<uint8_t>& classifications() const { return _classifications; }

  const auto& edge_of_flight_lines() const { return _edge_of_flight_lines; }
  const auto& gps_times() const { return _gps_times; }
  const auto& number_of_returns() const { return _number_of_returns; }
  const auto& return_numbers() const { return _return_numbers; }
  const auto& point_source_ids() const { return _point_source_ids; }
  const auto& scan_direction_flags() const { return _scan_direction_flags; }
  const auto& scan_angle_ranks() const { return _scan_angle_ranks; }
  const auto& user_data() const { return _user_data; }

  bool hasColors() const;
  bool hasNormals() const;
  bool hasIntensities() const;
  bool hasClassifications() const;
  bool has_edge_of_flight_lines() const;
  bool has_gps_times() const;
  bool has_number_of_returns() const;
  bool has_return_numbers() const;
  bool has_point_source_ids() const;
  bool has_scan_direction_flags() const;
  bool has_scan_angle_ranks() const;
  bool has_user_data() const;

  void verify() const;

  /// <summary>
  /// Returns the raw size in bytes of the contents (positions, normals etc.) of
  /// this PointBuffer. This does NOT include the in-memory size of a
  /// PointBuffer structure itself, but rather the allocated memory of all the
  /// vectors of the PointBuffer
  /// </summary>
  size_t content_byte_size() const;

  struct PointIterator
  {
    PointIterator(PointBuffer& pointBuffer, size_t idx);

    PointReference operator*() const;
    PointIterator& operator++();
    PointIterator operator++(int);
    PointIterator& operator--();
    PointIterator operator--(int);
    PointIterator operator+(std::ptrdiff_t count) const;
    PointIterator operator-(std::ptrdiff_t count) const;
    PointIterator& operator+=(std::ptrdiff_t count);
    PointIterator& operator-=(std::ptrdiff_t count);
    friend std::ptrdiff_t operator-(const PointIterator& l, const PointIterator& r);
    PointReference operator[](std::ptrdiff_t idx) const;

    bool operator==(const PointIterator& other) const;
    bool operator!=(const PointIterator& other) const;
    friend bool operator<(const PointIterator& l, const PointIterator& r);
    friend bool operator<=(const PointIterator& l, const PointIterator& r);
    friend bool operator>(const PointIterator& l, const PointIterator& r);
    friend bool operator>=(const PointIterator& l, const PointIterator& r);

  private:
    PointBuffer* _pointBuffer;
    size_t _index;
  };

  struct PointConstIterator
  {
    PointConstIterator(PointBuffer const& pointBuffer, size_t idx);

    PointConstReference operator*() const;
    PointConstIterator& operator++();
    PointConstIterator operator++(int);
    PointConstIterator& operator--();
    PointConstIterator operator--(int);
    PointConstIterator operator+(std::ptrdiff_t count) const;
    PointConstIterator operator-(std::ptrdiff_t count) const;
    PointConstIterator& operator+=(std::ptrdiff_t count);
    PointConstIterator& operator-=(std::ptrdiff_t count);
    friend std::ptrdiff_t operator-(const PointConstIterator& l, const PointConstIterator& r);
    PointConstReference operator[](std::ptrdiff_t idx) const;

    bool operator==(const PointConstIterator& other) const;
    bool operator!=(const PointConstIterator& other) const;
    friend bool operator<(const PointConstIterator& l, const PointConstIterator& r);
    friend bool operator<=(const PointConstIterator& l, const PointConstIterator& r);
    friend bool operator>(const PointConstIterator& l, const PointConstIterator& r);
    friend bool operator>=(const PointConstIterator& l, const PointConstIterator& r);

  private:
    PointBuffer const* _pointBuffer;
    size_t _index;
  };

  PointIterator begin();
  PointIterator end();

  PointConstIterator begin() const;
  PointConstIterator end() const;

private:
  size_t _count;
  std::vector<Vector3<double>> _positions;
  std::vector<Vector3<uint8_t>> _rgbColors;
  std::vector<Vector3<float>> _normals;
  std::vector<uint16_t> _intensities;
  std::vector<uint8_t> _classifications;
  std::vector<uint8_t> _edge_of_flight_lines;
  std::vector<double> _gps_times;
  std::vector<uint8_t> _number_of_returns;
  std::vector<uint8_t> _return_numbers;
  std::vector<uint16_t> _point_source_ids;
  std::vector<uint8_t> _scan_direction_flags;
  std::vector<int8_t> _scan_angle_ranks;
  std::vector<uint8_t> _user_data;
};

namespace std {
template<>
struct iterator_traits<::PointBuffer::PointIterator>
{
  using difference_type = std::ptrdiff_t;
  using value_type = ::PointBuffer::PointReference;
  using iterator_category = std::random_access_iterator_tag;
};

template<>
struct iterator_traits<::PointBuffer::PointConstIterator>
{
  using difference_type = std::ptrdiff_t;
  using value_type = ::PointBuffer::PointConstReference;
  using iterator_category = std::random_access_iterator_tag;
};
} // namespace std

namespace concepts {

namespace detail {
template<>
constexpr bool has_constant_size_impl<::PointBuffer> = false;
}

template<>
inline unit::byte
size_in_memory(PointBuffer const& point_buffer)
{
  return (sizeof(size_t) * boost::units::information::byte) +
         size_in_memory(point_buffer.positions()) + size_in_memory(point_buffer.rgbColors()) +
         size_in_memory(point_buffer.normals()) + size_in_memory(point_buffer.intensities()) +
         size_in_memory(point_buffer.classifications());
}
} // namespace concepts


#include <pygsl/solver.h>
#include <pygsl/utils.h>
#include <gsl/gsl_odeiv.h>

static const char odeiv_step_type_name   [] = "Odeiv-Step";
static const char odeiv_control_type_name[] = "Odeiv-Control";
static const char odeiv_evolve_type_name [] = "Odeiv-Evolve";

const char  * filename = __FILE__;
PyObject *module = NULL;

struct _mycontrol{
     gsl_odeiv_control *control;
     gsl_odeiv_step    *step;
     PyGSL_solver      *step_ob;
};
typedef struct _mycontrol mycontrol;

struct _myevolve{
     gsl_odeiv_evolve  *evolve;
     gsl_odeiv_control *control;
     gsl_odeiv_step    *step;
     PyGSL_solver      *control_ob;
     PyGSL_solver      *step_ob;
};
typedef struct _myevolve myevolve;

void
_mycontrol_free(mycontrol *c)
{
     FUNC_MESS_BEGIN();
     gsl_odeiv_control_free(c->control);
     if(c->step_ob){
	  DEBUG_MESS(3, "Decreasing step @ %p refcont %d", c->step_ob,
		     c->step_ob->ob_refcnt);
	  Py_DECREF(c->step_ob);
     }else{
	  DEBUG_MESS(3, "Freeing GSL Step @ %p", c->step);
	  gsl_odeiv_step_free(c->step);
     }
     memset(c, 0, sizeof(mycontrol));
     free(c);
     c = NULL;
     FUNC_MESS_END();
}

void
_myevolve_free(myevolve *c)
{
     FUNC_MESS_BEGIN();
     gsl_odeiv_evolve_free(c->evolve);
     if(c->control_ob){
	  DEBUG_MESS(3, "Decreasing control @ %p refcont %d", c->control_ob,
		     c->control_ob->ob_refcnt);
	  Py_DECREF(c->control_ob);
     }else{
	  DEBUG_MESS(3, "Freeing GSL Control @ %p", c->control);
	  gsl_odeiv_control_free(c->control);
     }
     if(c->step_ob){
	  DEBUG_MESS(3, "Decreasing step @ %p refcont %d", c->step_ob,
		     c->step_ob->ob_refcnt);
	  Py_DECREF(c->step_ob);
     }else{
	  DEBUG_MESS(3, "Freeing GSL Step @ %p", c->step);
	  gsl_odeiv_step_free(c->step);
     }
     memset(c, 0, sizeof(myevolve));
     free(c);
     c = NULL;
     FUNC_MESS_END();
}

const char *
PyGSL_mycontrol_getname(void *v)
{
     mycontrol *c = (mycontrol *) v;
     return gsl_odeiv_control_name(c->control);
}

#define _PyGSL_ODEIV_GENERIC_CHECK(ob, type_desc) \
  ((PyGSL_solver_check((ob))) && (((PyGSL_solver *)ob)->mstatic->type_name == (type_desc)))

#define PyGSL_ODEIV_STEP_Check(ob)    _PyGSL_ODEIV_GENERIC_CHECK((ob), odeiv_step_type_name)
#define PyGSL_ODEIV_CONTROL_Check(ob) _PyGSL_ODEIV_GENERIC_CHECK((ob), odeiv_control_type_name)
#define PyGSL_ODEIV_EVOLVE_Check(ob)  _PyGSL_ODEIV_GENERIC_CHECK((ob), odeiv_evolve_type_name)

static const char *this_file = __FILE__;
static char odeiv_step_init_err_msg[] = "odeiv_step.__init__";
static char odeiv_step_apply_doc[] =  "XXX Documentation missing\n";
static char odeiv_step_order_doc[] =  "XXX Documentation missing\n";

static char odeiv_control_hadjust_doc[] =  "XXX Documentation missing\n";
static char odeiv_evolve_apply_doc[] =  "XXX Documentation missing\n";

static char  PyGSL_odeiv_step_doc[] = "XXX Documentation missing\n";
static char  PyGSL_odeiv_control_doc[] = "XXX Documentation missing\n";
static char  PyGSL_odeiv_evolve_doc[] = "XXX Documentation missing\n";

/*---------------------------------------------------------------------------
  Wrapper functions to push call the approbriate Python Objects
  ---------------------------------------------------------------------------*/
static int
PyGSL_odeiv_func(double t, const double y[], double f[], void *params)
{
    int dimension, flag = GSL_FAILURE;

    PyObject *arglist = NULL, *result = NULL;
    PyArrayObject *yo = NULL;
    PyGSL_solver * step;
    gsl_vector_view yv, fv;
    PyGSL_error_info  info;

    FUNC_MESS_BEGIN();

    step = (PyGSL_solver *) params;
    if(!PyGSL_ODEIV_STEP_Check(step)){
	  PyGSL_add_traceback(module, this_file, __FUNCTION__,
			      __LINE__ - 2);
	  pygsl_error("Param not a step type!",
		    this_file, __LINE__ -2, GSL_EFAULT);
	  goto fail;
    }
    dimension =  step->problem_dimensions[0];

    /* Do I need to copy the array ??? */
    yv = gsl_vector_view_array((double *) y, dimension);
    yo = PyGSL_copy_gslvector_to_pyarray(&yv.vector);
    if (yo == NULL) goto fail;

    FUNC_MESS("\t\tBuild args");
    arglist = Py_BuildValue("(dOO)", t, yo, step->args);
    FUNC_MESS("\t\tEnd Build args");

    info.callback = step->cbs[0];
    info.message  = "odeiv_func";
    result  = PyEval_CallObject(step->cbs[0], arglist);

    if((flag = PyGSL_CHECK_PYTHON_RETURN(result, 1, &info)) != GSL_SUCCESS){
	  goto fail;
     }
    info.argnum = 1;
    fv = gsl_vector_view_array(f, dimension);
    if((flag = PyGSL_copy_pyarray_to_gslvector(&fv.vector, result, dimension,
					       &info)) != GSL_SUCCESS){
	  goto fail;
     }

    Py_DECREF(arglist);    arglist = NULL;
    Py_DECREF(yo);         yo = NULL;
    Py_DECREF(result);     result = NULL;
    FUNC_MESS_END();
    return GSL_SUCCESS;

 fail:
    FUNC_MESS("    IN Fail BEGIN");
    Py_XDECREF(yo);
    Py_XDECREF(result);
    Py_XDECREF(arglist);
    assert(flag != GSL_SUCCESS);
    FUNC_MESS("    IN Fail END");
    if(step->isset)
	 longjmp(step->buffer, flag);
    return flag;
}

static int
PyGSL_odeiv_jac(double t, const double y[], double *dfdy, double dfdt[],
		void *params)
{
    int dimension, flag = GSL_FAILURE;
    PyGSL_solver *step;
    PyGSL_error_info  info;

    PyObject *arglist = NULL, *result = NULL, *tmp=NULL;
    PyArrayObject *yo = NULL;

    gsl_vector_view yv, dfdtv;
    gsl_matrix_view dfdyv;

    FUNC_MESS_BEGIN();

    step = (PyGSL_solver *) params;
    if(!PyGSL_ODEIV_STEP_Check(step)){
	  PyGSL_add_traceback(module, this_file, __FUNCTION__,
			      __LINE__ - 2);
	  pygsl_error("Param not a step type!",
		    this_file, __LINE__ -2, GSL_EFAULT);
	  goto fail;
    }
    dimension = step->problem_dimensions[0];

    yv = gsl_vector_view_array((double *) y, dimension);
    yo = PyGSL_copy_gslvector_to_pyarray(&yv.vector);
    if (yo == NULL) goto fail;

    arglist = Py_BuildValue("(dOO)", t, yo, step->args);

    result  = PyEval_CallObject(step->cbs[1], arglist);

    info.callback = step->cbs[1];
    info.message  = "odeiv_jac";
    if((flag = PyGSL_CHECK_PYTHON_RETURN(result, 2, &info)) != GSL_SUCCESS){
	  goto fail;
     }

    info.argnum = 1;
    tmp = PyTuple_GET_ITEM(result, 0);
    dfdyv = gsl_matrix_view_array((double *) dfdy, dimension, dimension);
    if((flag = PyGSL_copy_pyarray_to_gslmatrix(&dfdyv.matrix, tmp, dimension, dimension, &info)) != GSL_SUCCESS){
	  goto fail;
    }

    info.argnum = 2;
    tmp = PyTuple_GET_ITEM(result, 1);
    dfdtv = gsl_vector_view_array((double *) dfdt, dimension);
    if((flag = PyGSL_copy_pyarray_to_gslvector(&dfdtv.vector, tmp, dimension, &info)) != GSL_SUCCESS){
	  goto fail;
    }

    Py_DECREF(arglist);    arglist = NULL;
    Py_DECREF(result);     result = NULL;
    Py_DECREF(yo);         yo = NULL;
    FUNC_MESS_END();
    return GSL_SUCCESS;
 fail:
    FUNC_MESS("IN Fail");
    assert(flag != GSL_SUCCESS);
    longjmp(step->buffer, flag);
    return flag;
}

/* Wrappers for the evaluation of the system */
static PyObject *
PyGSL_odeiv_step_apply(PyGSL_solver *self, PyObject *args)
{
    PyObject *result = NULL;
    PyObject *y0_o = NULL, *dydt_in_o = NULL;
    PyArrayObject *volatile y0 = NULL, * volatile yerr = NULL,
	 *volatile dydt_in = NULL, *volatile dydt_out = NULL,
	 *volatile yout = NULL;

    double t=0, h=0, *volatile dydt_in_d;
    int  r, flag;
    PyGSL_array_index_t dimension;

    FUNC_MESS_BEGIN();
    assert(PyGSL_ODEIV_STEP_Check(self));
    if(! PyArg_ParseTuple(args, "ddOOO", &t, &h, &y0_o,  &dydt_in_o)){
      return NULL;
    }

    dimension = self->problem_dimensions[0];
    y0 = PyGSL_vector_check(y0_o, dimension, PyGSL_DARRAY_CINPUT(1), NULL, NULL);
    if(y0 == NULL) goto fail;

    if (Py_None == dydt_in_o){
	 dydt_in_d = NULL;
    }else{
	 dydt_in = PyGSL_vector_check(dydt_in_o, dimension, PyGSL_DARRAY_CINPUT(2), NULL, NULL);
	 if(dydt_in == NULL) goto fail;
	 dydt_in_d = (double *) dydt_in->data;
    }

    dydt_out =  PyGSL_New_Array(1, &dimension, PyArray_DOUBLE);
    if (dydt_out == NULL) goto fail;

    yerr = PyGSL_New_Array(1, &dimension, PyArray_DOUBLE);
    if(yerr == NULL) goto fail;

    yout = (PyArrayObject *) PyGSL_Copy_Array(y0);
    if(yout == NULL) goto fail;

    self->isset = 0;
    if((flag=setjmp(self->buffer)) == 0){
	  FUNC_MESS("\t\t Setting Jmp Buffer");
	  self->isset = 1;
    } else {
	  FUNC_MESS("\t\t Returning from Jmp Buffer");
	  self->isset = 0;
	  goto fail;
    }

    r = gsl_odeiv_step_apply(self->solver, t, h,
			     (double *) yout->data,
			     (double *) yerr->data,
			     dydt_in_d,
			     (double *) dydt_out->data,
			     ((gsl_odeiv_system *)self->c_sys));
    self->isset = 0;
    if (GSL_SUCCESS != r){
	PyErr_SetString(PyExc_TypeError, "Error While evaluating gsl_odeiv");
      goto fail;
    }

    FUNC_MESS("    Returnlist create ");
    assert(yout != NULL);
    assert(yerr != NULL);
    assert(dydt_out != NULL);

    result = Py_BuildValue("(OOO)", yout, yerr, dydt_out);

    FUNC_MESS("    Memory free ");
    /* Deleting the arrays */
    Py_DECREF(y0);           y0 = NULL;
    Py_DECREF(yout);         yout = NULL;
    Py_DECREF(yerr);         yerr = NULL;
    Py_DECREF(dydt_out);     dydt_out = NULL;
    /* This array does not need to exist ... */
    Py_XDECREF(dydt_in);	 dydt_in=NULL;

    FUNC_MESS_END();
    return result;

    fail:
    FUNC_MESS("IN Fail");
    self->isset = 0;
    Py_XDECREF(y0);
    Py_XDECREF(yout);
    Py_XDECREF(yerr);
    Py_XDECREF(dydt_in);
    Py_XDECREF(dydt_out);
    FUNC_MESS("IN Fail End");
    return NULL;
}

static PyObject *
PyGSL_odeiv_control_hadjust(PyGSL_solver *self, PyObject *args)
{

  PyObject *result = NULL;
  PyObject *y0_o = NULL, *yerr_o = NULL, *dydt_o = NULL;
  PyArrayObject *y0 = NULL, *yerr = NULL, *dydt = NULL;
  double h = 0;
  int r = 0;
  mycontrol *c;

  PyGSL_array_index_t dimension = 0;

  FUNC_MESS_BEGIN();
  assert(PyGSL_ODEIV_CONTROL_Check(self));
  if(!PyArg_ParseTuple(args, "OOOd",  &y0_o, &yerr_o, &dydt_o, &h)){
    return NULL;
  }

  dimension = self->problem_dimensions[0];

  y0 = PyGSL_vector_check(y0_o, dimension, PyGSL_DARRAY_CINPUT(1), NULL, NULL);
  if(y0 == NULL)   goto fail;
  yerr = PyGSL_vector_check(yerr_o, dimension, PyGSL_DARRAY_CINPUT(2), NULL, NULL);
  if(yerr == NULL) goto fail;
  dydt = PyGSL_vector_check(dydt_o, dimension, PyGSL_DARRAY_CINPUT(3), NULL, NULL);
  if(dydt == NULL) goto fail;

  FUNC_MESS("      Array Pointers End");

  c = (mycontrol *) self->solver;
  r = gsl_odeiv_control_hadjust(c->control, c->step,
				(double *) y0->data,
				(double *) yerr->data,
				(double *) dydt->data, &h);

  FUNC_MESS("      Function End");
  Py_DECREF(y0);       y0 = NULL;
  Py_DECREF(yerr);     yerr = NULL;
  Py_DECREF(dydt);     dydt = NULL;

  result = Py_BuildValue("di",h,r);
  FUNC_MESS_END();
  return result;

 fail:
  FUNC_MESS("IN Fail");
  Py_XDECREF(y0);
  Py_XDECREF(yerr);
  Py_XDECREF(dydt);
  FUNC_MESS("IN Fail END");
  return NULL;
}

static PyObject *
PyGSL_odeiv_evolve_apply(PyGSL_solver *self, PyObject *args)
{
    PyObject *result = NULL;
    PyObject *y0_o = NULL,  *myargs = NULL;
    PyArrayObject *volatile y0 = NULL, *volatile yout = NULL;
    myevolve *e = NULL;

    double t=0, h=0, t1 = 0, flag;

    int dimension = self->problem_dimensions[0], r;

    assert(PyGSL_ODEIV_EVOLVE_Check(self));
    FUNC_MESS_BEGIN();

    if(!PyArg_ParseTuple(args, "dddOO", &t, &t1, &h, &y0_o, &myargs)){
      return NULL;
    }

    DEBUG_MESS(3, "y0_o @ %p", y0_o);

    y0 = PyGSL_vector_check(y0_o, dimension, PyGSL_DARRAY_CINPUT(1), NULL, NULL);
    if(y0 == NULL) goto fail;

    yout = (PyArrayObject *)  PyGSL_Copy_Array(y0);
    if(yout == NULL) goto fail;

    e = (myevolve *) self->solver;

    if((flag=setjmp(e->step_ob->buffer)) == 0){
	 e->step_ob->isset = 1;
	  FUNC_MESS("\t\t Setting Jmp Buffer");
     } else {
	  FUNC_MESS("\t\t Returning from Jmp Buffer");
	  e->step_ob->isset = 0;
	  goto fail;
     }
    DEBUG_MESS(3, "evolve @ %p\t control @ %p\t step @ %p", e, e->control, e->step);

    r = gsl_odeiv_evolve_apply(e->evolve,
			       e->control,
			       e->step,
			       e->step_ob->c_sys, &t, t1, &h,
			       (double * )yout->data);
   e->step_ob->isset = 0;
    if (GSL_SUCCESS != r){
	 goto fail;
    }

    assert(yout != NULL);

    result = Py_BuildValue("(ddO)", t, h, yout);

    /* Deleting the arrays */
    Py_DECREF(yout);     yout = NULL;
    Py_DECREF(y0);       y0=NULL;
    FUNC_MESS_END();
    return result;

 fail:
    FUNC_MESS("IN Fail");
    e->step_ob->isset = 0;
    PyGSL_add_traceback(module, this_file, odeiv_step_init_err_msg, __LINE__);
    Py_XDECREF(y0);
    Py_XDECREF(yout);
    FUNC_MESS("IN Fail End");
    return NULL;
}

GETINT(odeiv_step_order)

static struct PyMethodDef PyGSL_odeiv_step_methods[] = {
     {"apply", (PyCFunction) PyGSL_odeiv_step_apply, METH_VARARGS, odeiv_step_apply_doc},
     {"order", (PyCFunction) PyGSL_odeiv_step_order, METH_VARARGS, odeiv_step_order_doc},
     {NULL, NULL}
};
static struct PyMethodDef PyGSL_odeiv_control_methods[] = {
     {"hadjust", (PyCFunction) PyGSL_odeiv_control_hadjust, METH_VARARGS, odeiv_control_hadjust_doc},
     {NULL, NULL}
};

static struct PyMethodDef PyGSL_odeiv_evolve_methods[] = {
     {"apply", (PyCFunction) PyGSL_odeiv_evolve_apply, METH_VARARGS, odeiv_evolve_apply_doc},
     {NULL, NULL}
};

static const struct _SolverStatic
_StepMethods     = {{(void_m_t) gsl_odeiv_step_free,
		   (void_m_t) gsl_odeiv_step_reset,
		   (name_m_t) gsl_odeiv_step_name,
		   (int_m_t) NULL},
		    3, PyGSL_odeiv_step_methods, odeiv_step_type_name},
_ControlMethods  = {{(void_m_t) _mycontrol_free,
		      (void_m_t) NULL,
		      (name_m_t) PyGSL_mycontrol_getname,
		      (int_m_t) NULL},
		    3, PyGSL_odeiv_control_methods, odeiv_control_type_name},
_EvolveMethods   = {{(void_m_t) _myevolve_free,
		     (void_m_t) gsl_odeiv_evolve_reset,
		     (name_m_t) NULL,
		     (int_m_t) NULL,},
		    3, PyGSL_odeiv_evolve_methods, odeiv_evolve_type_name};

static PyObject *
PyGSL_odeiv_step_init(PyObject *self, PyObject *args, PyObject *kwdict, const gsl_odeiv_step_type * odeiv_type)
{

     PyObject *func=NULL, *jac=NULL, *o_params=NULL;
     PyGSL_solver *solver = NULL;

     static char * kwlist[] = {"dimension", "func", "jac", "args", NULL};
     int dim, has_jacobian = 0;
     gsl_odeiv_system * c_sys;

     solver_alloc_struct s = {odeiv_type, (void_an_t) gsl_odeiv_step_alloc,
			      &_StepMethods};

     FUNC_MESS_BEGIN();

     assert(args);
     if (0 == PyArg_ParseTupleAndKeywords(args, kwdict, "iOOO:odeiv_step.__init__", kwlist,
					  &dim, &func, &jac, &o_params)){
	  PyGSL_add_traceback(module, this_file, odeiv_step_init_err_msg, __LINE__ - 2);
	  return NULL;
     }
     if (dim <= 0){
	  PyGSL_add_traceback(module, this_file, odeiv_step_init_err_msg, __LINE__ - 1);
	  pygsl_error("The dimension of the problem must be at least 1",
		    this_file, __LINE__ -2, GSL_EDOM);
	  return NULL;
     }
     if(!PyCallable_Check(func)){
	  PyGSL_add_traceback(module, this_file, odeiv_step_init_err_msg, __LINE__ - 1);
	  pygsl_error("The function object is not callable!",
		    this_file, __LINE__ -2, GSL_EBADFUNC);
	  goto fail;
     }

     if(jac == Py_None){
	  if(odeiv_type == gsl_odeiv_step_bsimp){
	       PyGSL_add_traceback(module, this_file, odeiv_step_init_err_msg, __LINE__ - 1);
	       pygsl_error("The bsimp method needs a jacobian! You supplied None.",
			 this_file, __LINE__ -2, GSL_EBADFUNC);
	       goto fail;
	  }
     }else{
	  if(!PyCallable_Check(jac)){
	       PyGSL_add_traceback(module, this_file, odeiv_step_init_err_msg, __LINE__ - 1);
	       pygsl_error("The jacobian object must be None or callable!",
			 this_file, __LINE__ -2, GSL_EBADFUNC);
	       goto fail;
	  }
	  has_jacobian = 1;

     }

     solver = (PyGSL_solver *) PyGSL_solver_dn_init(self, args, &s, 3);

     if(solver == NULL){
	  goto fail;
     }
     DEBUG_MESS(3, "solver @ %p", solver);

     solver->solver =  gsl_odeiv_step_alloc(odeiv_type, dim);
     if(solver->solver == NULL){
	  goto fail;
     }
     DEBUG_MESS(3, "step @ %p", solver->solver);
     c_sys = (gsl_odeiv_system *)calloc(1, sizeof(gsl_odeiv_system));
     if(c_sys == NULL){
	  PyErr_NoMemory();
	  goto fail;
     }

     /* Need for cleanup in fail : */
     solver->c_sys = c_sys;
     DEBUG_MESS(3, "c_sys @ %p", solver->c_sys);
     solver->problem_dimensions[0] = dim;
     if(has_jacobian){
	  c_sys->jacobian = PyGSL_odeiv_jac;
	  if(!PyCallable_Check(jac))
	       goto fail;
	  solver->cbs[1] = jac;
     }else{
	  c_sys->jacobian = NULL;
	  solver->cbs[1] = NULL;
     }
     c_sys->function = PyGSL_odeiv_func;
     if(!PyCallable_Check(func))
	       goto fail;

     solver->cbs[0] = func;
     c_sys->params = (void *) solver;
     DEBUG_MESS(3, "params @ %p", c_sys->params);
     Py_INCREF(solver->cbs[0]);
     Py_XINCREF(solver->cbs[1]);
     Py_XINCREF(solver->args);

     solver->args  = o_params;
     Py_INCREF(solver->args);

     FUNC_MESS_END();
     return (PyObject *) solver;

 fail:
     FUNC_MESS("FAIL");
     Py_XDECREF(solver);
     return NULL;
}
#define ADD_ODESTEPPER(mytype)                                                   \
static PyObject *                                                                \
PyGSL_odeiv_step_init_ ## mytype (PyObject * self, PyObject * args, PyObject *kwdic)   \
{                                                                                \
     return PyGSL_odeiv_step_init(self, args, kwdic, gsl_odeiv_step_ ## mytype); \
}
ADD_ODESTEPPER(rk2)
ADD_ODESTEPPER(rk4)
ADD_ODESTEPPER(rkf45)
ADD_ODESTEPPER(rkck)
ADD_ODESTEPPER(rk8pd)
ADD_ODESTEPPER(rk2imp)
ADD_ODESTEPPER(rk4imp)
ADD_ODESTEPPER(bsimp)
ADD_ODESTEPPER(gear1)
ADD_ODESTEPPER(gear2)

static PyObject *
PyGSL_odeiv_control_init(PyObject *self, PyObject *args, void * type)
{
     int nargs = -1, tmp=0;
     double eps_abs, eps_rel, a_y, a_dydt;
     PyGSL_solver *step=NULL, *solver=NULL;
     mycontrol * c;

     gsl_odeiv_control *(*evaluator_5)(double , double , double , double ) = NULL;
     gsl_odeiv_control *(*evaluator_3)(double , double ) = NULL;

     solver_alloc_struct s = {type, (void_an_t) gsl_odeiv_control_alloc,
			      &_ControlMethods};

     FUNC_MESS_BEGIN();
     /* The arguments depend on the type of control */
     if(type == (void *) gsl_odeiv_control_standard_new){
	  /* step, eps_abs, eps_rel, a_y, a_dydt */
	  nargs = 5;
     }else if(type == (void *) gsl_odeiv_control_y_new ||
	      type == (void *) gsl_odeiv_control_yp_new){
	  nargs = 3;
     }else{
	  PyGSL_add_traceback(module, this_file, odeiv_step_init_err_msg,
			      __LINE__ - 2);
	  pygsl_error("Unknown control type",
		    this_file, __LINE__ -2, GSL_EFAULT);
	  goto fail;
     }
     assert(nargs > -1);

     switch(nargs){
     case 5:
	  tmp == PyArg_ParseTuple(args, "Odddd:odeiv_control.__init__",
				   &step, &eps_abs, &eps_rel, &a_y, &a_dydt);
	  break;
     case 3:
	  tmp == PyArg_ParseTuple(args, "Odd:odeiv_control.__init__",
				  &step, &eps_abs, &eps_rel);
	  break;
     default:
	  fprintf(stderr, "nargs = %d\n", nargs);
	  pygsl_error("Unknown number of arguments",
		    this_file, __LINE__ -2, GSL_EFAULT);
	  goto fail; break;
     }
     if(!PyGSL_ODEIV_STEP_Check(step)){
	  int flag;
	  flag = PyGSL_solver_check(step);
	  DEBUG_MESS(3, "is solver?  %d, %p %p ", flag,  PyGSL_API[PyGSL_solver_type_NUM], step->ob_type);
	  if(flag){
	       DEBUG_MESS(3, "solver = %s, %p !=  %p", step->mstatic->type_name, step->mstatic->type_name,
			  odeiv_step_type_name);
	       pygsl_error("First argument must be a step solver!", __FILE__, __LINE__, GSL_EINVAL);
	  }
	  goto fail;
     }

     if(tmp){
	  PyGSL_add_traceback(module, this_file, odeiv_step_init_err_msg,
			      __LINE__ - 2);
	  return NULL;
     }

     solver =  (PyGSL_solver *) PyGSL_solver_dn_init(self, args, &s, 3);
     if (NULL == solver){
	  PyErr_NoMemory();
	  goto fail;
     }
     c = calloc(1, sizeof(mycontrol));
     if(c == NULL){
	  PyErr_NoMemory();
	  goto fail;
     }
     solver->solver = c;
     switch(nargs){
     case 5:
	  evaluator_5 = type;
	  c->control = evaluator_5(eps_abs, eps_rel, a_y, a_dydt);
	  break;
     case 3:
	  evaluator_3 = type;
	  c->control = evaluator_3(eps_abs, eps_rel);
	  break;
     default:
	  goto fail;
     }
     if (NULL == c->control){
	  PyErr_NoMemory();
	  goto fail;
     }
     DEBUG_MESS(3, "c->control @ %p", c->control);
     c->step =  step->solver;
     c->step_ob = step;
     Py_INCREF(step);
     FUNC_MESS_END();
     return (PyObject *) solver;

 fail:
     FUNC_MESS("FAIL");
     Py_XDECREF(solver);
     return NULL;

}

#define ADD_ODECONTROL(name)                                                  \
static PyObject *                                                             \
PyGSL_odeiv_control_init_ ## name (PyObject * self, PyObject * args)          \
{                                                                             \
     return PyGSL_odeiv_control_init(self, args, (void *) gsl_odeiv_control_ ## name);    \
}
ADD_ODECONTROL(standard_new)
ADD_ODECONTROL(y_new)
ADD_ODECONTROL(yp_new)

static PyObject *
PyGSL_odeiv_evolve_init(PyObject *self, PyObject *args)
{
     PyGSL_solver *step, *control, *a_ev = NULL;
     myevolve *e;
     solver_alloc_struct s = {NULL, (void_an_t) gsl_odeiv_evolve_alloc,
			      &_EvolveMethods};

     /* step, control */
     FUNC_MESS_BEGIN();
     if(0== PyArg_ParseTuple(args, "OO:odeiv_evolve.__init__",
			     &step, &control)){
	  return NULL;
     }
     if(!PyGSL_ODEIV_STEP_Check(step)){
	  pygsl_error("First argument must be a step solver!", __FILE__, __LINE__, GSL_EINVAL);
	  goto fail;
     }

     if(!PyGSL_ODEIV_CONTROL_Check(control)){
	  pygsl_error("Second argument must be a control solver!", __FILE__, __LINE__, GSL_EINVAL);
	  goto fail;
     }

     a_ev =  (PyGSL_solver *) PyGSL_solver_dn_init(self, args, &s, 3);
     if(NULL == a_ev){
	  PyErr_NoMemory();
	  return NULL;
     }
     a_ev->problem_dimensions[0] = step->problem_dimensions[0];

     e = (myevolve *) calloc(1, sizeof(myevolve));
     if(e == NULL){
	  PyErr_NoMemory();
	  goto fail;
     }
     a_ev->solver = e;
     e->step_ob =  step;
     e->control_ob = control;
     Py_INCREF(step);
     Py_INCREF(control);
     e->step = step->solver;
     e->control = ((mycontrol *)control->solver)->control;
     e->evolve = gsl_odeiv_evolve_alloc(step->problem_dimensions[0]);
     if(NULL == e->evolve){
	  PyErr_NoMemory();
	  goto fail;
     }
     FUNC_MESS_END();
     return (PyObject *) a_ev;
 fail:
     FUNC_MESS("FAIL");
     Py_XDECREF(a_ev);
     return NULL;
}

static const char PyGSL_odeiv_module_doc [] = "XXX Missing ";
static PyMethodDef mMethods[] = {
     {"step_rk2",    (PyCFunction)PyGSL_odeiv_step_init_rk2,    METH_VARARGS|METH_KEYWORDS, PyGSL_odeiv_step_doc},
     {"step_rk4",    (PyCFunction)PyGSL_odeiv_step_init_rk4,    METH_VARARGS|METH_KEYWORDS, PyGSL_odeiv_step_doc},
     {"step_rkf45",  (PyCFunction)PyGSL_odeiv_step_init_rkf45,  METH_VARARGS|METH_KEYWORDS, PyGSL_odeiv_step_doc},
     {"step_rkck",   (PyCFunction)PyGSL_odeiv_step_init_rkck,   METH_VARARGS|METH_KEYWORDS, PyGSL_odeiv_step_doc},
     {"step_rk8pd",  (PyCFunction)PyGSL_odeiv_step_init_rk8pd,  METH_VARARGS|METH_KEYWORDS, PyGSL_odeiv_step_doc},
     {"step_rk2imp", (PyCFunction)PyGSL_odeiv_step_init_rk2imp, METH_VARARGS|METH_KEYWORDS, PyGSL_odeiv_step_doc},
     {"step_rk4imp", (PyCFunction)PyGSL_odeiv_step_init_rk4imp, METH_VARARGS|METH_KEYWORDS, PyGSL_odeiv_step_doc},
     {"step_bsimp",  (PyCFunction)PyGSL_odeiv_step_init_bsimp,  METH_VARARGS|METH_KEYWORDS, PyGSL_odeiv_step_doc},
     {"step_gear1",  (PyCFunction)PyGSL_odeiv_step_init_gear1,  METH_VARARGS|METH_KEYWORDS, PyGSL_odeiv_step_doc},
     {"step_gear2",  (PyCFunction)PyGSL_odeiv_step_init_gear2,  METH_VARARGS|METH_KEYWORDS, PyGSL_odeiv_step_doc},
     {"control_standard_new", PyGSL_odeiv_control_init_standard_new, METH_VARARGS, PyGSL_odeiv_control_doc},
     {"control_y_new",        PyGSL_odeiv_control_init_y_new,        METH_VARARGS, PyGSL_odeiv_control_doc},
     {"control_yp_new",       PyGSL_odeiv_control_init_yp_new,       METH_VARARGS, PyGSL_odeiv_control_doc},
     {"evolve", PyGSL_odeiv_evolve_init, METH_VARARGS, PyGSL_odeiv_evolve_doc},
     {NULL, NULL, 0, NULL}
};

void
initodeiv(void)
{
     PyObject* m, *dict, *item;
     FUNC_MESS_BEGIN();

     m=Py_InitModule("odeiv", mMethods);
     module = m;
     assert(m);
     dict = PyModule_GetDict(m);
     if(!dict)
	  goto fail;

     init_pygsl()
     import_pygsl_solver();
     assert(PyGSL_API);

     if (!(item = PyString_FromString((char*)PyGSL_odeiv_module_doc))){
	  PyErr_SetString(PyExc_ImportError,
			  "I could not generate module doc string!");
	  goto fail;
     }
     if (PyDict_SetItemString(dict, "__doc__", item) != 0){
	  PyErr_SetString(PyExc_ImportError,
			  "I could not init doc string!");
	  goto fail;
     }

     FUNC_MESS_END();

 fail:
     FUNC_MESS("FAIL");
     return;
}


#include <gsl/gsl_errno.h>
#include <gsl/matrix/gsl_matrix.h>

#define BASE_DOUBLE
#include <gsl/templates_on.h>
#include <gsl/matrix/matrix_source.c>
#include <gsl/templates_off.h>
#undef BASE_DOUBLE


/*
ODE: a program to get optime Runge-Kutta and multi-steps methods.

Copyright 2011-2019, Javier Burguete Tolosa.

Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:

	1. Redistributions of source code must retain the above copyright notice,
		this list of conditions and the following disclaimer.

	2. 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.

THIS SOFTWARE IS PROVIDED BY Javier Burguete Tolosa ``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 Javier Burguete Tolosa 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.
*/

/**
 * \file optimize.c
 * \brief Source file with optimize functions.
 * \author Javier Burguete Tolosa.
 * \copyright Copyright 2011-2019.
 */
#define _GNU_SOURCE
#include <stdio.h>
#include <string.h>
#include <float.h>
#include <math.h>
#include <libxml/parser.h>
#include <glib.h>
#include <libintl.h>
#include <gsl/gsl_rng.h>
#if HAVE_MPI
#include <mpi.h>
#endif
#include "config.h"
#include "utils.h"
#include "optimize.h"

#define DEBUG_OPTIMIZE 0        ///< macro to debug.

GMutex mutex[1];                ///< GMutex struct.
FILE *file_variables = NULL;    ///< random variables file.
int rank;                       ///< MPI rank.
int nnodes;                     ///< MPI nodes number.
unsigned nthreads;              ///< threads number.

/**
 * Function to print the random variables on a file.
 */
void
optimize_print_random (Optimize * optimize,     ///< Optimize struct.
                       FILE * file)     ///< file.
{
  unsigned int i, n;
  n = optimize->nfree;
  for (i = 0; i < n; ++i)
    fprintf (file, "o%d:%.19Le;\n", i, optimize->value_optimal[i]);
  for (i = 0; i < n; ++i)
    fprintf (file, "m%d:%.19Le;\n", i, optimize->minimum[i]);
  for (i = 0; i < n; ++i)
    fprintf (file, "i%d:%.19Le;\n", i, optimize->interval[i]);
}

/**
 * Function to perform every optimization step.
 */
void
optimize_step (Optimize * optimize)     ///< Optimize struct.
{
  long double *is, *vo, *vo2, *random;
  long double o, o2, v, f;
  unsigned long long int ii, nrandom;
  unsigned int i, j, k, n, nfree;

#if DEBUG_OPTIMIZE
  fprintf (stderr, "optimize_step: start\n");
#endif

  // save optimal values
#if DEBUG_OPTIMIZE
  fprintf (stderr, "optimize_step: save optimal values\n");
#endif
  nfree = optimize->nfree;
  o2 = *optimize->optimal;
  vo = (long double *) alloca (nfree * sizeof (long double));
  vo2 = (long double *) alloca (nfree * sizeof (long double));
  memcpy (vo, optimize->value_optimal, nfree * sizeof (long double));

  // optimization algorithm sampling
#if DEBUG_OPTIMIZE
  fprintf (stderr, "optimize_step: optimization algorithm sampling\n");
  fprintf (stderr, "optimize_step: nsimulations=%Lu\n", optimize->nsimulations);
#endif
  random = optimize->random_data;
  ii = optimize->nsimulations * (rank * nthreads + optimize->thread)
    / (nnodes * nthreads);
  nrandom = optimize->nsimulations * (rank * nthreads + optimize->thread + 1)
    / (nnodes * nthreads);
  for (; ii < nrandom; ++ii)
    {

      // random freedom degrees
#if DEBUG_OPTIMIZE
      fprintf (stderr, "optimize_step: random freedom degrees\n");
      fprintf (stderr, "optimize_step: simulation=%Lu\n", ii);
#endif
      optimize_generate_freedom (optimize, ii);

      // method coefficients
#if DEBUG_OPTIMIZE
      fprintf (stderr, "optimize_step: method coefficients\n");
#endif
      if (!optimize->method (optimize))
        o = INFINITY;
      else
        o = optimize->objective (optimize);
      if (o < o2)
        {
          o2 = o;
          memcpy (vo, random, nfree * sizeof (long double));
        }
      if (file_variables)
        {
          g_mutex_lock (mutex);
          print_variables (random, nfree, file_variables);
          fprintf (file_variables, "%.19Le\n", o);
          g_mutex_unlock (mutex);
        }
    }

  // array of intervals to climb around the optimal
#if DEBUG_OPTIMIZE
  fprintf (stderr,
           "optimize_step: array of intervals to climb around the optimal\n");
#endif
  is = (long double *) alloca (nfree * sizeof (long double));
  for (j = 0; j < nfree; ++j)
    is[j] = optimize->interval0[j] * optimize->climbing_factor;

  // hill climbing algorithm bucle
#if DEBUG_OPTIMIZE
  fprintf (stderr, "optimize_step: hill climbing algorithm bucle\n");
#endif
  memcpy (vo2, vo, nfree * sizeof (long double));
  n = optimize->nclimbings;
  for (i = 0; i < n; ++i)
    {
      memcpy (random, vo, nfree * sizeof (long double));
      for (j = k = 0; j < nfree; ++j)
        {
          v = vo[j];
          random[j] = v + is[j];
          if (!optimize->method (optimize))
            o = INFINITY;
          else
            o = optimize->objective (optimize);
          if (o < o2)
            {
              k = 1;
              o2 = o;
              memcpy (vo2, random, nfree * sizeof (long double));
            }
          if (file_variables)
            {
              g_mutex_lock (mutex);
              print_variables (random, nfree, file_variables);
              fprintf (file_variables, "%.19Le\n", o);
              g_mutex_unlock (mutex);
            }
          random[j] = fmaxl (0.L, v - is[j]);
          if (!optimize->method (optimize))
            o = INFINITY;
          else
            o = optimize->objective (optimize);
          if (o < o2)
            {
              k = 1;
              o2 = o;
              memcpy (vo2, random, nfree * sizeof (long double));
            }
          if (file_variables)
            {
              g_mutex_lock (mutex);
              print_variables (random, nfree, file_variables);
              fprintf (file_variables, "%.19Le\n", o);
              g_mutex_unlock (mutex);
            }
          random[j] = v;
        }

      // update optimal values and increase or reduce intervals if converging or
      // not
      if (!k)
        f = 0.5L;
      else
        {
          f = 1.2L;
          memcpy (vo, vo2, nfree * sizeof (long double));
        }
      for (j = 0; j < nfree; ++j)
        is[j] *= f;
    }

  // update optimal values
#if DEBUG_OPTIMIZE
  fprintf (stderr, "optimize_step: update optimal values\n");
#endif
  if (o2 < *optimize->optimal)
    {
      g_mutex_lock (mutex);
      *optimize->optimal = o2;
      memcpy (optimize->value_optimal, vo2, nfree * sizeof (long double));
      g_mutex_unlock (mutex);
    }

#if DEBUG_OPTIMIZE
  fprintf (stderr, "optimize_step: end\n");
#endif
}

/**
 * Function to init required variables on an Optimize struct data.
 */
void
optimize_init (Optimize * optimize,     ///< Optimize struct.
               gsl_rng * rng,   ///< GSL pseudo-random number generator struct.
               unsigned int thread)     ///< thread number.
{
#if DEBUG_OPTIMIZE
  fprintf (stderr, "optimize_init: start\n");
#endif
#if DEBUG_OPTIMIZE
  fprintf (stderr, "optimize_init: nsimulations=%Lu nfree=%u size=%u\n",
           optimize->nsimulations, optimize->nfree, optimize->size);
#endif
  optimize->random_data
    = (long double *) g_slice_alloc (optimize->nfree * sizeof (long double));
  optimize->coefficient
    = (long double *) g_slice_alloc (optimize->size * sizeof (long double));
  optimize->minimum
    = (long double *) g_slice_alloc (optimize->nfree * sizeof (long double));
  optimize->interval
    = (long double *) g_slice_alloc (optimize->nfree * sizeof (long double));
  memcpy (optimize->minimum, optimize->minimum0,
          optimize->nfree * sizeof (long double));
  memcpy (optimize->interval, optimize->interval0,
          optimize->nfree * sizeof (long double));
  optimize->rng = rng;
  optimize->thread = thread;
#if DEBUG_OPTIMIZE
  fprintf (stderr, "optimize_init: end\n");
#endif
}

/**
 * Function to free the memory allocated by an Optimize struct.
 */
void
optimize_delete (Optimize * optimize)   ///< Optimize struct.
{
  g_slice_free1 (optimize->nfree * sizeof (long double), optimize->interval);
  g_slice_free1 (optimize->nfree * sizeof (long double), optimize->minimum);
  g_slice_free1 (optimize->size * sizeof (long double), optimize->coefficient);
  g_slice_free1 (optimize->nfree * sizeof (long double), optimize->random_data);
}

/**
 * Function to do the optimization bucle.
 */
void
optimize_bucle (Optimize * optimize)    ///< Optimize struct.
{
  GThread *thread[nthreads];
#if HAVE_MPI
  long double *vo;
  MPI_Status status;
#endif
  unsigned int i, j, nfree;

#if DEBUG_OPTIMIZE
  fprintf (stderr, "optimize_bucle: start\n");
  fprintf (stderr, "optimize_bucle: nfree=%u\n", optimize->nfree);
#endif

  // Allocate local array of optimal values
#if DEBUG_OPTIMIZE
  fprintf (stderr, "optimize_bucle: allocate local array of optimal values\n");
#endif
  nfree = optimize->nfree;
#if HAVE_MPI
  vo = (long double *) alloca ((1 + nfree) * sizeof (long double));
  printf ("Rank=%d NNodes=%d\n", rank, nnodes);
#endif

  // Init some parameters
#if DEBUG_OPTIMIZE
  fprintf (stderr, "optimize_bucle: init some parameters\n");
#endif
  *optimize->optimal = INFINITY;
  for (i = 0; i < nfree; ++i)
    optimize->value_optimal[i]
      = optimize->minimum[i] + 0.5L * optimize->interval[i];

  // Iterate
#if DEBUG_OPTIMIZE
  fprintf (stderr, "optimize_bucle: iterate\n");
#endif
  for (i = 0; i < optimize->niterations; ++i)
    {

      // Optimization step parallelized for every node by GThreads
      if (nthreads > 1)
        {
          for (j = 0; j < nthreads; ++j)
            thread[j]
              = g_thread_new (NULL,
                              (GThreadFunc) (void (*)(void)) optimize_step,
                              (void *) (optimize + j));
          for (j = 0; j < nthreads; ++j)
            g_thread_join (thread[j]);
        }
      else
        optimize_step (optimize);

#if HAVE_MPI
      if (rank > 0)
        {

          // Secondary nodes send the optimal coefficients to the master node
          vo[0] = *optimize->optimal;
          memcpy (vo + 1, optimize->value_optimal,
                  nfree * sizeof (long double));
          MPI_Send (vo, 1 + nfree, MPI_LONG_DOUBLE, 0, 1, MPI_COMM_WORLD);

          // Secondary nodes receive the optimal coefficients
          MPI_Recv (vo, 1 + nfree, MPI_LONG_DOUBLE, 0, 1, MPI_COMM_WORLD,
                    &status);
          *optimize->optimal = vo[0];
          memcpy (optimize->value_optimal, vo + 1,
                  nfree * sizeof (long double));
        }
      else
        {
          printf ("rank=%d optimal=%.19Le\n", rank, *optimize->optimal);

          for (j = 1; j < nnodes; ++j)
            {

              // Master node receives the optimal coefficients obtained by
              // secondary nodes
              MPI_Recv (vo, 1 + nfree, MPI_LONG_DOUBLE, j, 1, MPI_COMM_WORLD,
                        &status);

              // Master node selects the optimal coefficients
              if (vo[0] < *optimize->optimal)
                {
                  *optimize->optimal = vo[0];
                  memcpy (optimize->value_optimal, vo + 1,
                          nfree * sizeof (long double));
                }
            }

          // Master node sends the optimal coefficients to secondary nodes
          vo[0] = *optimize->optimal;
          memcpy (vo + 1, optimize->value_optimal,
                  nfree * sizeof (long double));
          for (j = 1; j < nnodes; ++j)
            MPI_Send (vo, 1 + nfree, MPI_LONG_DOUBLE, j, 1, MPI_COMM_WORLD);
        }

#endif

      // Print the optimal coefficients
#if DEBUG_OPTIMIZE
      optimize_print_random (optimize, stderr);
      fprintf (stderr, "optimal=%.19Le\n", *optimize->optimal);
#endif

      // Updating coefficient intervals to converge
      optimize_converge (optimize);

      // Iterate
#if HAVE_MPI
      printf ("Rank %u\n", rank);
#endif
      printf ("Iteration %u Optimal %.19Le\n", i + 1, *optimize->optimal);
    }

#if DEBUG_OPTIMIZE
  fprintf (stderr, "optimize_bucle: end\n");
#endif
}

/**
 * Function to create an Optimize struct data.
 */
void
optimize_create (Optimize * optimize,   ///< Optimize struct.
                 long double *optimal,
                 ///< pointer to the optimal objective function value.
                 long double *value_optimal)
                 ///< array of optimal freedom degree values.
{
  unsigned long long int nsimulations;
  unsigned int i, nfree;
#if DEBUG_OPTIMIZE
  fprintf (stderr, "optimize_create: start\n");
#endif
  optimize->optimal = optimal;
  optimize->value_optimal = value_optimal;
  nfree = optimize->nfree;
  optimize->nsimulations = nsimulations = optimize->nvariable;
  for (i = 1; i < nfree; ++i)
    optimize->nsimulations *= nsimulations;
  optimize->nclimbings *= nfree;
#if DEBUG_OPTIMIZE
  fprintf (stderr, "optimize_create nsimulations=%Lu nclimbings=%u nfree=%u\n",
           optimize->nsimulations, optimize->nclimbings, nfree);
  fprintf (stderr, "optimize_create: end\n");
#endif
}

/**
 * Function to read the Optimize struct data on a XML node.
 *
 * \return 1 on success, 0 on error.
 */
int
optimize_read (Optimize * optimize,     ///< Optimize struct.
               xmlNode * node)  ///< XML node.
{
  int code;
#if DEBUG_OPTIMIZE
  fprintf (stderr, "optimize_read: start\n");
#endif
  optimize->nvariable = xml_node_get_uint (node, XML_NSIMULATIONS, &code);
  if (code || !optimize->nvariable)
    {
      error_message = g_strdup (_("Bad simulations number"));
      goto exit_on_error;
    }
  optimize->nclimbings
    = xml_node_get_uint_with_default (node, XML_NCLIMBINGS, 0, &code);
  if (code)
    {
      error_message = g_strdup (_("Bad hill climbings number"));
      goto exit_on_error;
    }
  optimize->niterations = xml_node_get_uint (node, XML_NITERATIONS, &code);
  if (code || !optimize->niterations)
    {
      error_message = g_strdup (_("Bad iterations number"));
      goto exit_on_error;
    }
  optimize->convergence_factor
    = xml_node_get_float (node, XML_CONVERGENCE_FACTOR, &code);
  if (code || optimize->convergence_factor < LDBL_EPSILON)
    {
      error_message = g_strdup (_("Bad convergence factor"));
      goto exit_on_error;
    }
  optimize->climbing_factor
    = xml_node_get_float (node, XML_CLIMBING_FACTOR, &code);
  if (code || optimize->climbing_factor < LDBL_EPSILON)
    {
      error_message = g_strdup (_("Bad climging factor"));
      goto exit_on_error;
    }
#if DEBUG_OPTIMIZE
  fprintf (stderr, "optimize_read: end\n");
#endif
  return 1;

exit_on_error:
#if DEBUG_OPTIMIZE
  fprintf (stderr, "optimize_read: end\n");
#endif
  return 0;
}


// Copyright (c) Microsoft Corporation.
// Licensed under the MIT License.

#pragma once

#include "BaseWorker.h"

#include "MageSlam.h"

#include <arcana/scheduling/state_machine.h>
#include <gsl/gsl>
#include <memory>

namespace mage
{
    struct MageContext;
    struct MageSlamSettings;
    struct FrameData;

    class Fuser;

    class Runtime
    {
    public:
        Runtime(const MageSlamSettings& settings, MageContext& context, Fuser& fuser, mira::state_machine_driver& driver);
        ~Runtime();

        void Run(gsl::span<const MAGESlam::CameraConfiguration> cameras);

        void TrackMono(std::shared_ptr<FrameData> frame);
        void TrackStereo(std::shared_ptr<FrameData> one, std::shared_ptr<FrameData> two);

        void AddSample(const mage::SensorSample& sample);
    private:
        struct Impl;
        const std::unique_ptr<Impl> m_impl;
    };
}


/* Internal header file for cminpack, by Frederic Devernay. */
#ifndef __CMINPACKP_H__
#define __CMINPACKP_H__

#ifndef __CMINPACK_H__
#error "cminpackP.h in an internal cminpack header, and must be included after all other headers (including cminpack.h)"
#endif

#if (defined (USE_CBLAS) || defined (USE_LAPACK)) && !defined (__cminpack_double__) && !defined (__cminpack_float__)
#error "cminpack can use cblas and lapack only in double or single precision mode"
#endif

#ifdef USE_CBLAS
#ifdef __APPLE__
#include <Accelerate/Accelerate.h>
#else
#include <cblas.h>
#endif
#define __cminpack_enorm__(n,x) __cminpack_cblas__(nrm2)(n,x,1)
#else
#define __cminpack_enorm__(n,x) __cminpack_func__(enorm)(n,x)
#endif

#ifdef USE_LAPACK
#ifdef __APPLE__
#include <Accelerate/Accelerate.h>
#else
#if defined(__LP64__) /* In LP64 match sizes with the 32 bit ABI */
typedef int 		__CLPK_integer;
typedef int 		__CLPK_logical;
typedef __CLPK_logical 	(*__CLPK_L_fp)();
typedef int 		__CLPK_ftnlen;
#else
typedef long int 	__CLPK_integer;
typedef long int 	__CLPK_logical;
typedef __CLPK_logical 	(*__CLPK_L_fp)();
typedef long int 	__CLPK_ftnlen;
#endif
int __cminpack_lapack__(lartg_)(
  __cminpack_real__ *f, __cminpack_real__ *g, __cminpack_real__ *cs,
  __cminpack_real__ *sn, __cminpack_real__ *r__);
int __cminpack_lapack__(geqp3_)(
  __CLPK_integer *m, __CLPK_integer *n, __cminpack_real__ *a, __CLPK_integer * lda,
  __CLPK_integer *jpvt, __cminpack_real__ *tau, __cminpack_real__ *work, __CLPK_integer *lwork,
  __CLPK_integer *info);
int __cminpack_lapack__(geqrf_)(
  __CLPK_integer *m, __CLPK_integer *n, __cminpack_real__ *a, __CLPK_integer * lda,
  __cminpack_real__ *tau, __cminpack_real__ *work, __CLPK_integer *lwork, __CLPK_integer *info);
#endif
#endif

#include "minpackP.h"

#endif /* !__CMINPACKP_H__ */


#ifndef SmtkMatcher_h
#define SmtkMatcher_h

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

/* SPDX-License-Identifier: CC0-1.0 */

#include <memory>

#include <QSharedPointer>
#include "SmtkStack.h"
#include "Gruen.h"
#include "SmtkPoint.h"

#include "GSLUtility.h"
#include <gsl/gsl_rng.h>

namespace Isis {

/**
 * @brief Workhorse of stereo matcher
 *
 * This class provides stereo matching functionality to the SMTK toolkit.  It
 * registers points, clones them by adjusting parameters to nearby point
 * locations and manages point selection processes.
 *
 * The Gruen algorithm is initialized here and maintained for use in the
 * stereo matching process.
 *
 * @author 2011-05-28 Kris Becker
 *
 * @internal
 *   @history 2012-12-20 Debbie A. Cook - Removed unused Projection.h
 *                           References #775.
 *   @history 2017-08-18 Summer Stapleton, Ian Humphrey, Tyler Wilson -
 *                           Changed auto_ptr reference to QSharedPointer
 *                           so this class compiles under C++14.
 *                           References #4809.
 */
class SmtkMatcher {
  public:
    SmtkMatcher();
    SmtkMatcher(const QString &regdef);
    SmtkMatcher(const QString &regdef, Cube *lhImage, Cube *rhImage);
    SmtkMatcher(Cube *lhImage, Cube *rhImage);
    ~SmtkMatcher();

    void setImages(Cube *lhImage, Cube *rhImage);
    void setGruenDef(const QString &regdef);

    bool isValid(const Coordinate &pnt);
    bool isValid(const SmtkPoint &spnt);

    /** Return pattern chip */
    Chip *PatternChip() const {
      validate();
      return (m_gruen->PatternChip());
    }

    /** Return search chip */
    Chip *SearchChip() const {
      validate();
      return (m_gruen->SearchChip());
    }

    /** Returns the fit chip */
    Chip *FitChip() const {
      validate();
      return (m_gruen->FitChip());
    }

    void setWriteSubsearchChipPattern(const QString &fileptrn = "SmtkMatcher");

    SmtkQStackIter FindSmallestEV(SmtkQStack &stack);
    SmtkQStackIter FindExpDistEV(SmtkQStack &stack, const double &seedsample,
                                 const double &minEV, const double &maxEV);

    SmtkPoint Register(const Coordinate &lpnt,
                       const AffineRadio &affrad = AffineRadio());
    SmtkPoint Register(const PointPair &pnts,
                       const AffineRadio &affrad = AffineRadio());
    SmtkPoint Register(const SmtkPoint &spnt,
                       const AffineRadio &affrad = AffineRadio());
    SmtkPoint Register(const PointGeometry &lpg, const PointGeometry &rpg,
                       const AffineRadio &affrad  = AffineRadio());

    SmtkPoint Create(const Coordinate &left, const Coordinate &right);
    SmtkPoint Clone(const SmtkPoint &point, const Coordinate &left);

    inline BigInt OffImageErrorCount() const { return (m_offImage);  }
    inline BigInt SpiceErrorCount() const { return (m_spiceErr);  }

    /** Return Gruen template parameters */
    PvlGroup RegTemplate() { return (m_gruen->RegTemplate());  }
    /** Return Gruen registration statistics */
    Pvl RegistrationStatistics() { return (m_gruen->RegistrationStatistics()); }

  private:
    SmtkMatcher &operator=(const SmtkMatcher &matcher); // Assignment disabled
    SmtkMatcher(const SmtkMatcher &matcher);            // Copy const disabled

    Cube     *m_lhCube;                // Left image cube (not owned)
    Cube     *m_rhCube;                // Right image cube (not owned)
    QSharedPointer<Gruen> m_gruen;     // Gruen matcher
    BigInt  m_offImage;                // Offimage counter
    BigInt  m_spiceErr;                // SPICE distance error
    bool    m_useAutoReg;              // Select AutoReg features
    const gsl_rng_type * T;            // GSL random number type
    gsl_rng * r;                       // GSL random number generator

    void randomNumberSetup();
    bool validate(const bool &throwError = true) const;

    inline Camera &lhCamera() { return (*m_lhCube->camera());   }
    inline Camera &rhCamera() { return (*m_rhCube->camera());   }

    Coordinate getLineSample(Camera &camera, const Coordinate &geom);
    Coordinate getLatLon(Camera &camera, const Coordinate &pnt);

    bool inCube(const Camera &camera, const Coordinate &point) const;

    SmtkPoint makeRegisteredPoint(const PointGeometry &left,
                                  const PointGeometry &right, Gruen *gruen);
};
} // namespace Isis

#endif


#include "rb_sm.h"
#include <stdio.h>
#include <gsl/gsl_nan.h>
#include <options/options.h>

struct sm_params rb_sm_params;
struct sm_result rb_sm_result;

void rb_sm_init_journal(const char*journal_file) {
	jj_set_stream(open_file_for_writing(journal_file));
/*	sm_journal_open(journal_file);*/
}

void rb_sm_close_journal() {
	FILE * s = jj_get_stream();
	if(s) fclose(s);

	jj_set_stream(0);
}

void rb_sm_odometry(double x, double y, double theta){
	rb_sm_params.first_guess[0]=x;
	rb_sm_params.first_guess[1]=y;
	rb_sm_params.first_guess[2]=theta;
}

void rb_sm_odometry_cov(double cov_x, double cov_y, double cov_theta){

}

struct option* ops = 0;

int rb_sm_set_configuration(const char*name, const char*value) {
	if(!ops) {
		ops = options_allocate(30);
		sm_options(&rb_sm_params, ops);
	}

	if(!options_try_pair(ops, name, value)) {

		return 0;
	} else
	return 1;
}

const char *rb_result_to_json() {
	static char buf[5000];
	JO jo = result_to_json(&rb_sm_params, &rb_sm_result);
	strcpy(buf, jo_to_string(jo));
	jo_free(jo);
	return buf;
}

int rb_sm_icp() {
	sm_icp(&rb_sm_params, &rb_sm_result);
	return rb_sm_result.valid;
}

int rb_sm_gpm() {
	sm_gpm(&rb_sm_params, &rb_sm_result);
	return rb_sm_result.valid;
}

void rb_set_laser_ref(const char*s) {
	rb_sm_params.laser_ref = string_to_ld(s);
/*	fprintf(stderr, "Set laser_ref to %p\n ", rb_sm_params.laser_ref );*/
}

void rb_set_laser_sens(const char*s) {
	rb_sm_params.laser_sens = string_to_ld(s);
/*	fprintf(stderr, "Set laser_sens to %p\n ", rb_sm_params.laser_sens );*/
}

void rb_sm_cleanup() {
	if(rb_sm_params.laser_ref)
	ld_free(rb_sm_params.laser_ref);
	if(rb_sm_params.laser_sens)
	ld_free(rb_sm_params.laser_sens);
}

LDP string_to_ld(const char*s) {
	JO jo = json_parse(s);
	if(!jo) {
		fprintf(stderr, "String passed from Ruby is invalid JSON: \n\n%s\n", s);
		return 0;
	}
	LDP ld = json_to_ld(jo);
	if(!ld) {
		fprintf(stderr, "String passed from Ruby is valid JSON, "
			"but can't load laser_data. \n\n%s\n", s);
		return 0;
	}
	jo_free(jo);
	return ld;
}


#include <stdio.h>
#include <stdlib.h>
#include <gsl/vector/gsl_vector.h>
#include <gsl/gsl_math.h>

#define BASE_DOUBLE
#include <gsl/templates_on.h>
#include <gsl/vector/minmax_source.c>
#include <gsl/templates_off.h>
#undef BASE_DOUBLE


#if !defined(fvSupport_h)
#define fvSupport_h

#include <petsc.h>

#define MAX_FVM_RHS_FUNCTION_FIELDS 4

typedef PetscErrorCode (*FVMRHSFluxFunction)(PetscInt dim, const PetscFVFaceGeom *fg, const PetscInt uOff[], const PetscInt uOff_x[], const PetscScalar fieldL[], const PetscScalar fieldR[],
                                             const PetscScalar gradL[], const PetscScalar gradR[], const PetscInt aOff[], const PetscInt aOff_x[], const PetscScalar auxL[], const PetscScalar auxR[],
                                             const PetscScalar gradAuxL[], const PetscScalar gradAuxR[], PetscScalar flux[], void *ctx);

typedef PetscErrorCode (*FVMRHSPointFunction)(PetscInt dim, const PetscFVCellGeom *cg, const PetscInt uOff[], const PetscScalar u[], const PetscInt aOff[], const PetscScalar a[], PetscScalar f[], void *ctx);

typedef PetscErrorCode (*FVAuxFieldUpdateFunction)(PetscReal time, PetscInt dim, const PetscFVCellGeom *cellGeom, const PetscScalar *conservedValues, PetscScalar *auxField, void *ctx);

/**
 * struct to describe how to compute RHS finite volume flux source terms
 */
struct _FVMRHSFluxFunctionDescription {
    FVMRHSFluxFunction function;
    void *context;

    PetscInt field;
    PetscInt inputFields[MAX_FVM_RHS_FUNCTION_FIELDS];
    PetscInt numberInputFields;

    PetscInt auxFields[MAX_FVM_RHS_FUNCTION_FIELDS];
    PetscInt numberAuxFields;
};

typedef struct _FVMRHSFluxFunctionDescription FVMRHSFluxFunctionDescription;

/**
 * struct to describe how to compute RHS finite volume point source terms
 */
struct _FVMRHSPointFunctionDescription {
    FVMRHSPointFunction function;
    void *context;

    PetscInt fields[MAX_FVM_RHS_FUNCTION_FIELDS];
    PetscInt numberFields;

    PetscInt inputFields[MAX_FVM_RHS_FUNCTION_FIELDS];
    PetscInt numberInputFields;

    PetscInt auxFields[MAX_FVM_RHS_FUNCTION_FIELDS];
    PetscInt numberAuxFields;
};

typedef struct _FVMRHSPointFunctionDescription FVMRHSPointFunctionDescription;

/**
  DMPlexTSComputeRHSFunctionFVM - Form the local forcing F from the local input X using flux and pointfunctions specified by the user

  Input Parameters:
+ dm - The mesh
. t - The time
. locX  - Local solution
- user - The user context

  Output Parameter:
. F  - Global output vector

  Level: developer

.seealso: DMPlexComputeJacobianActionFEM()
**/
PETSC_EXTERN PetscErrorCode ABLATE_DMPlexComputeRHSFunctionFVM(FVMRHSFluxFunctionDescription *fluxFunctionDescriptions, PetscInt numberFluxFunctionDescription,
                                                               FVMRHSPointFunctionDescription *pointFunctionDescriptions, PetscInt numberPointFunctionDescription, DM dm, PetscReal time, Vec locX, Vec F);

/**
 * Populate the boundary with gradient information
 * @param dm
 * @param auxDM
 * @param time
 * @param locXVec
 * @param locAuxField
 * @param numberUpdateFunctions
 * @param updateFunctions
 * @param data
 * @return
 */

/**
 * Takes all local vector
 * @return
 */
PETSC_EXTERN PetscErrorCode ABLATE_DMPlexComputeFluxResidual_Internal(FVMRHSFluxFunctionDescription functionDescription[], PetscInt numberFunctionDescription, DM, IS, PetscReal, Vec, Vec, PetscReal, Vec);

/**
   Form the local forcing F from the local input X using pointwise functions specified by the user

  Input Parameters:
+ dm - The mesh
. t - The time
. locX  - Local solution

  Output Parameter:
. F  - local output vector

**/
PETSC_EXTERN PetscErrorCode ABLATE_DMPlexComputePointResidual_Internal(FVMRHSPointFunctionDescription *functionDescription, PetscInt numberFunctionDescription, DM, IS, PetscReal, Vec, Vec, PetscReal, Vec);

/**
 * reproduces the petsc call with grad fixes for multiple fields
 * @param dm
 * @param fvm
 * @param locX
 * @param grad
 * @return
 */
PETSC_EXTERN PetscErrorCode DMPlexReconstructGradientsFVM_MulfiField(DM dm, PetscFV fvm, Vec locX, Vec grad);

/**
 * reproduces the petsc call with grad fixes for multiple fields
 * @param dm
 * @param fvm
 * @param locX
 * @param grad
 * @return
 */
PETSC_EXTERN PetscErrorCode DMPlexGetDataFVM_MulfiField(DM dm, PetscFV fv, Vec *cellgeom, Vec *facegeom, DM *gradDM);

/**
 * Function to update all cells.  This should be merged into other update calls
 * @param dm
 * @param auxDM
 * @param time
 * @param locXVec
 * @param locAuxField
 * @param numberUpdateFunctions
 * @param updateFunctions
 * @param ctx
 * @return
 */
PETSC_EXTERN PetscErrorCode FVFlowUpdateAuxFieldsFV(DM dm, DM auxDM, PetscReal time, Vec locXVec, Vec locAuxField, PetscInt numberUpdateFunctions, FVAuxFieldUpdateFunction *updateFunctions, void **ctx);

#endif


/* ieee-utils/read.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2007 Brian Gough
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

#include <config.h>
#include <stdlib.h>
#include <string.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_ieee_utils.h>

static int
lookup_string (const char * p, int * precision, int * rounding,
               int * exception_mask) ;

int
gsl_ieee_read_mode_string (const char * description,
                           int * precision,
                           int * rounding,
                           int * exception_mask)
{
  char * start ;
  char * end;
  char * p;

  int precision_count = 0 ;
  int rounding_count = 0 ;
  int exception_count = 0 ;

  start = (char *) malloc(strlen(description) + 1) ;

  if (start == 0)
    {
      GSL_ERROR ("no memory to parse mode string", GSL_ENOMEM) ;
    }

  strcpy (start, description) ;

  p = start ;

  *precision = 0 ;
  *rounding = 0 ;
  *exception_mask = 0 ;

  do {
    int status ;
    int new_precision, new_rounding, new_exception ;

    end = strchr (p,',') ;

    if (end)
      {
        *end = '\0' ;
        do
          {
            end++ ;  /* skip over trailing whitespace */
          }
        while (*end == ' ' || *end == ',') ;
      }

    new_precision = 0 ;
    new_rounding = 0 ;
    new_exception = 0 ;

    status = lookup_string (p, &new_precision, &new_rounding, &new_exception) ;

    if (status)
      GSL_ERROR ("unrecognized GSL_IEEE_MODE string.\nValid settings are:\n\n"
                 "  single-precision double-precision extended-precision\n"
                 "  round-to-nearest round-down round-up round-to-zero\n"
                 "  mask-invalid mask-denormalized mask-division-by-zero\n"
                 "  mask-overflow mask-underflow mask-all\n"
                 "  trap-common trap-inexact\n"
                 "\n"
                 "separated by commas. "
                 "(e.g. GSL_IEEE_MODE=\"round-down,mask-underflow\")",
                 GSL_EINVAL) ;

    if (new_precision)
      {
        *precision = new_precision ;
        precision_count ++ ;
        if (precision_count > 1)
          GSL_ERROR ("attempted to set IEEE precision twice", GSL_EINVAL) ;
      }

    if (new_rounding)
      {
        *rounding = new_rounding ;
        rounding_count ++ ;
        if (rounding_count > 1)
          GSL_ERROR ("attempted to set IEEE rounding mode twice", GSL_EINVAL) ;
      }

    if (new_exception)
      {
        *exception_mask |= new_exception ;
        exception_count ++ ;
      }

    p = end ;

  } while (end && *p != '\0') ;

  free(start) ;

  return GSL_SUCCESS ;
}

static int
lookup_string (const char * p, int * precision, int * rounding,
               int * exception_mask)
{
  if (strcmp(p,"single-precision") == 0)
    {
      *precision = GSL_IEEE_SINGLE_PRECISION ;
    }
  else if (strcmp(p,"double-precision") == 0)
    {
      *precision = GSL_IEEE_DOUBLE_PRECISION ;
    }
  else if (strcmp(p,"extended-precision") == 0)
    {
      *precision = GSL_IEEE_EXTENDED_PRECISION ;
    }
  else if (strcmp(p,"round-to-nearest") == 0)
    {
      *rounding = GSL_IEEE_ROUND_TO_NEAREST ;
    }
  else if (strcmp(p,"round-down") == 0)
    {
      *rounding = GSL_IEEE_ROUND_DOWN ;
    }
  else if (strcmp(p,"round-up") == 0)
    {
      *rounding = GSL_IEEE_ROUND_UP ;
    }
  else if (strcmp(p,"round-to-zero") == 0)
    {
      *rounding = GSL_IEEE_ROUND_TO_ZERO ;
    }
  else if (strcmp(p,"mask-all") == 0)
    {
      *exception_mask = GSL_IEEE_MASK_ALL ;
    }
  else if (strcmp(p,"mask-invalid") == 0)
    {
      *exception_mask = GSL_IEEE_MASK_INVALID ;
    }
  else if (strcmp(p,"mask-denormalized") == 0)
    {
      *exception_mask = GSL_IEEE_MASK_DENORMALIZED ;
    }
  else if (strcmp(p,"mask-division-by-zero") == 0)
    {
      *exception_mask = GSL_IEEE_MASK_DIVISION_BY_ZERO ;
    }
  else if (strcmp(p,"mask-overflow") == 0)
    {
      *exception_mask = GSL_IEEE_MASK_OVERFLOW ;
    }
  else if (strcmp(p,"mask-underflow") == 0)
    {
      *exception_mask = GSL_IEEE_MASK_UNDERFLOW ;
    }
  else if (strcmp(p,"trap-inexact") == 0)
    {
      *exception_mask = GSL_IEEE_TRAP_INEXACT ;
    }
  else if (strcmp(p,"trap-common") == 0)
    {
      return 0 ;
    }
  else
    {
      return 1 ;
    }

  return 0 ;
}


/*

Excited States software: KGS
Contributors: See CONTRIBUTORS.txt
Contact: kgs-contact@simtk.org

Copyright (C) 2009-2017 Stanford University

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:

This entire text, including 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, CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR
OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
IN THE SOFTWARE.

*/

#ifndef CONFIGURATION_H
#define CONFIGURATION_H

#include <list>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <vector>
#include <tuple>

#include "math/Nullspace.h"
#include "math/Eigenvalue.h"
#include "core/graph/KinGraph.h"

class Molecule;
class Selection;

/**
 * A configuration holds all DOF-values necessary to update a molecules atom-positions.
 * DOF-values are relative to whatever is stored in the Atom::reference_positions, so calling
 * Configuration* conf = new Configuration(m_protein);
 * will create a configuration where all DOF-values are 0 and which represents whatever is in
 * the reference positions of m_protein. Modifying for example
 * conf->m_dofs[2] += 0.1
 * will add 0.1 units (often radians) to the third DOF. To see the resulting structure call either
 * m_protein->setConfiguration(conf);
 * or
 * conf->updatedProtein();
 * and then access Atom::position (not Atom::m_referencePosition).
 *
 * As new configurations are generated from older ones, the field m_children and m_parent store the
 * connectivity information.
 */
class Configuration
{
 public:
  double *m_dofs;                    ///< DOF-values (relative to Atom::m_referencePosition)
  //double *m_sumProjSteps;          //TODO: What is this?

  /** Construct a configuration with all DOF-values set to 0 and no m_parent. */
  Configuration(Molecule * mol);

  /** Construct a configuration with all DOF-values set to 0 and the specified m_parent. */
  Configuration(Configuration* parent);

  ~Configuration();

  double getGlobalTorsion( int i ) ; ///< Get a global DOF-value
  double* getGlobalTorsions() ;     ///< Get global DOF-value array

  unsigned int getNumDOFs() const;

  /** Set the specified dof to the global torsion value. Convenient function for
   * calculating difference between reference torsion and val. */
  void setGlobalTorsion(int i, double val);

  Configuration* clone() const;          ///< Copy this configuration

  void Print();                          // TODO: Remove or rename to printDOFs
//  Molecule* collapseRigidBonds();
//  void identifyBiggerRigidBodies();      ///< Identify clusters
//  void readBiggerSet();                  ///< read the set of clusters, related to identifying clusters
  void projectOnCycleNullSpace (gsl_vector *to_project, gsl_vector *after_project);

  void convertAllDofsToCycleDofs( gsl_vector *cycleDofs, gsl_vector *allDofs);
  void convertCycleDofsToAllDofs( gsl_vector *allDofsAfter, gsl_vector *cycleDofs, gsl_vector *allDofsBefore = nullptr);

//  static bool compareSize(std::pair<int, unsigned int> firstEntry, std::pair<int, unsigned int> secondEntry);//TODO: What is this?

  void writeQToBfactor();

  // When the samples are generated as an expanding tree, the m_children and m_parent store the connectivity information of these nodes
  const int m_treeDepth;             ///< Depth in the exploration tree
  int m_id;                         ///< ID of configuration
  double m_vdwEnergy;               ///< van der Waals energy of configuration
  double m_deltaH;                  ///< change in enthalpy due to clash constraints
  double m_distanceToTarget;        ///< Distance to target configuration
  double m_distanceToParent;        ///< Distance to m_parent configuration
  double m_distanceToIni;           ///< Distance to initial configuration
  double m_paretoFrontDistance;
  double m_maxConstraintViolation;  //Maximum detected distance violation of h-bond constraints, Todo: maybe not necessary to keep
  double m_minCollisionFactor;      //minimum necessary clash-factor for configuration to be clash free, Todo: maybe not necessary to keep
  double m_usedClashPrevention;

//  std::map<unsigned int, Rigidbody*> m_biggerRBMap;  // <Cluster-idx, Pointer-to-cluster>
//  std::vector< std::pair<int, unsigned int> > m_sortedRBs; // < cluster-idx, cluster size>

  int m_numClusters;             ///< Number of rigid clusters (super rigid bodies)
  int m_maxIndex;                ///< Index of largest cluster
  int m_maxSize;                 ///< Size of largest cluster
  int m_clashFreeDofs;           ///< Number of clash-free dofs (for post-processing)

  Molecule * updatedMolecule();  ///< Update the atom-positions to reflect this configuration and return the molecule
  Molecule * getMolecule() const;///< Return the associated molecule
  void updateMolecule();         ///< Update the atom-positions to reflect this configuration

  /** Return the cycle jacobian. Calls computeJacobians if CycleJacobian is not up to date */
  gsl_matrix* getCycleJacobian();
  Nullspace* getNullspace();    ///< Compute the nullspace (if it wasn't already) and return it
  //Nullspace* getNullspaceligand();  ///< Compute the nullspace for ligand (if it wasn't already) and return it
  Nullspace* getNullspacenocoupling();
  void Hessianmatrixentropy(double cutoff=20.0, double coefficientvalue=1.0, double vdwenergyvalue=10000.0, Nullspace* Nu=nullptr, Molecule* mol=nullptr, bool proteinonly=false, std::string nocoupling="true");  ///< Compute the nullspace for vibrational entropy (if it wasn't already) and return it
  Eigenvalue* geteigenvalue();
  gsl_matrix* getHydrophobicJacobian();
  gsl_matrix* getHydrogenJacobian();
  gsl_matrix* getDistanceJacobian();

  void rigidityAnalysis();
  void deleteNullspace(); ///if not needed anymore, save memory

  Configuration* getParent();   ///< Access configuration that spawned this one
  std::list<Configuration*>& getChildren(); ///< Access child configurations

  double siteDOFTransfer(Selection& source, Selection& sink,gsl_matrix* baseMatrix); //
  void sortFreeEnergyModes(gsl_matrix* baseMatrix, gsl_vector* singVals, gsl_vector* returnIDs); //return index list of modes sorted by free energy
  int getNumRigidDihedralsligand();

  bool checknocoupling();
  static Nullspace* ClashAvoidingNullSpace; //TODO: Make private (or even better put in ClashAvoidingMove).
  void setrigiddofid();

 protected:

    int numligandRigidDihedrals=0; ///< Rigid dihedrals in ligand
  void updateGlobalTorsions();           ///< Update the global DOF-values (m_dofs_global field)
  double *m_dofs_global;                 ///< DOF-values in a global system (not relative to Atom::reference_position)
  Molecule * const m_molecule;           ///< The molecule related to the configuration
  Configuration * m_parent;              ///< The parent-configuration this configuration was generated from
  std::list<Configuration*> m_children;  ///< List of child-configurations

  void computeCycleJacobianAndNullSpace();
  void computeCycleJacobianentropyforall();
  void computeCycleJacobianentropy(Nullspace* Nu,std::string nocoupling);
  void computeMassmatrix();
  void computedistancematrix(Molecule* mol);
  void computeHessiancartesian(double cutoff, double coefficientvalue, double vdwenergyvalue,Molecule* mol);
  void computeJacobians();               ///< Compute non-redundant cycle jacobian and hbond-jacobian // and also HydrophobicBond-jacobian
  // Jacobian matrix of all the cycles of rigid bodies
  void computeJacobiansnocoupling();
  static gsl_matrix* CycleJacobian; // column dimension is the number of DOFs; row dimension is 5 times the number of cycles because 2 atoms on each cycle-closing edge
  //static gsl_matrix* CycleJacobianligand;// column dimension is the number of DOFS; row dimension is the number of cycles\//
  //static gsl_matrix* ClashAvoidingJacobian;
  //Nullspace* nullspaceligand;
  //static SVD* JacobianSVDligand;
  static gsl_matrix* CycleJacobiannocoupling;
  gsl_matrix* CycleJacobianentropy;// column dimension is the number of DOFS; row dimension is the number of cycles\//
  gsl_matrix* CycleJacobianentropycoupling;
  gsl_matrix* CycleJacobianentropynocoupling;
  gsl_matrix* Hessianmatrix_cartesian;
  static gsl_matrix* Massmatrix;
  static gsl_matrix* distancematrix;
  static gsl_matrix* coefficientmatrix;
  Eigenvalue* Entropyeigen;
  static gsl_matrix* HBondJacobian; // column dimension is the number of DOFS; row dimension is the number of cycles\//
  static gsl_matrix* HydrophobicBondJacobian; //column dimension is the number of DOFs; row dimension is the 5 times the number of Hydrophobic bond
  static gsl_matrix* DBondJacobian; //column dimension is the number of DOFs; row dimension is the 5 times the number of Hydrophobic bond
  static Configuration* CycleJacobianOwner;

  static SVD* JacobianSVD;
  static SVD* JacobianSVDnocoupling;
  Nullspace* nullspace;                  ///< Nullspace of hbond of this configuration
  Nullspace* nullspacenocoupling;
  Nullspace* nullspaceHydro;

};

#endif


#pragma once

#include <optional>
#include <sam.h>
#include <gsl/span>
#include "clocks.h"
#include "utils.h"

namespace mcu {

class I2cMaster {
public:
	I2cMaster(Sercom* sercom, const mcu::ClockGenerator& clock, const mcu::ClockGenerator& slowClock, unsigned frequency = 100000)
		:port{sercom->I2CM}
	{
		int sercomIndex = mcu::util::getSercomIndex(sercom);
		PM->APBCMASK.reg |= 1 << (PM_APBCMASK_SERCOM0_Pos + sercomIndex);
		clock.routeToPeripheral(GCLK_CLKCTRL_ID_SERCOM0_CORE_Val + sercomIndex);
		slowClock.routeToPeripheral(GCLK_CLKCTRL_ID_SERCOMX_SLOW_Val);

		port.CTRLA.reg = SERCOM_I2CM_CTRLA_MODE_I2C_MASTER;
		(void)port.CTRLB.reg;
		volatile uint32_t ctrla = port.CTRLA.reg;
		port.CTRLB.reg = 0;
		port.BAUD.reg = computeBaud(frequency, clock.frequency);
		while (port.STATUS.bit.SYNCBUSY);
		port.CTRLA.bit.ENABLE = true;
		while (port.STATUS.bit.SYNCBUSY);
		port.STATUS.bit.BUSSTATE = BusstateIdle;
	}

	std::optional<std::byte> readReg(uint8_t slaveAddress, uint8_t reg)
	{
		port.ADDR.reg = (slaveAddress << 1) | 0;
		while (port.INTFLAG.reg == 0);
		port.INTFLAG.reg = SERCOM_I2CM_INTFLAG_MB | SERCOM_I2CM_INTFLAG_SB;
		if (port.STATUS.reg & (StatusErrorMask | SERCOM_I2CM_STATUS_RXNACK) != 0) {
			if (port.STATUS.bit.BUSSTATE == BusstateOwner)
				port.CTRLB.reg = SERCOM_I2CM_CTRLB_CMD(CommandStop);
			return std::nullopt;
		}

		port.DATA.reg = reg;
		while (port.INTFLAG.reg == 0);
		port.INTFLAG.reg = SERCOM_I2CM_INTFLAG_MB | SERCOM_I2CM_INTFLAG_SB;
		if (port.STATUS.reg & StatusErrorMask) {
			if (port.STATUS.bit.BUSSTATE == BusstateOwner)
				port.CTRLB.reg = SERCOM_I2CM_CTRLB_CMD(CommandStop);
			return std::nullopt;
		}

		port.ADDR.reg = (slaveAddress << 1) | 1;
		while (port.INTFLAG.reg == 0);
		port.INTFLAG.reg = SERCOM_I2CM_INTFLAG_MB | SERCOM_I2CM_INTFLAG_SB;
		if (port.STATUS.reg & (StatusErrorMask | SERCOM_I2CM_STATUS_RXNACK) != 0) {
			if (port.STATUS.bit.BUSSTATE == BusstateOwner)
				port.CTRLB.reg = SERCOM_I2CM_CTRLB_CMD(CommandStop);
			return std::nullopt;
		}

		std::byte result = static_cast<std::byte>(port.DATA.reg);
		port.CTRLB.reg = SERCOM_I2CM_CTRLB_ACKACT | SERCOM_I2CM_CTRLB_CMD(CommandStop);
		return result;
	}

	bool writeReg(uint8_t slaveAddress, uint8_t reg, std::byte data)
	{
		port.ADDR.reg = (slaveAddress << 1) | 0;
		while (port.INTFLAG.reg == 0);
		port.INTFLAG.reg = SERCOM_I2CM_INTFLAG_MB | SERCOM_I2CM_INTFLAG_SB;
		if (port.STATUS.reg & (StatusErrorMask | SERCOM_I2CM_STATUS_RXNACK) != 0) {
			if (port.STATUS.bit.BUSSTATE == BusstateOwner)
				port.CTRLB.reg = SERCOM_I2CM_CTRLB_CMD(CommandStop);
			return false;
		}

		port.DATA.reg = reg;
		while (port.INTFLAG.reg == 0);
		port.INTFLAG.reg = SERCOM_I2CM_INTFLAG_MB | SERCOM_I2CM_INTFLAG_SB;
		if (port.STATUS.reg & (StatusErrorMask | SERCOM_I2CM_STATUS_RXNACK) != 0) {
			if (port.STATUS.bit.BUSSTATE == BusstateOwner)
				port.CTRLB.reg = SERCOM_I2CM_CTRLB_CMD(CommandStop);
			return false;
		}

		port.DATA.reg = static_cast<uint8_t>(data);
		while (port.INTFLAG.reg == 0);
		port.INTFLAG.reg = SERCOM_I2CM_INTFLAG_MB | SERCOM_I2CM_INTFLAG_SB;
		if (port.STATUS.reg & (StatusErrorMask) != 0) {
			if (port.STATUS.bit.BUSSTATE == BusstateOwner)
				port.CTRLB.reg = SERCOM_I2CM_CTRLB_CMD(CommandStop);
			return false;
		}

		port.CTRLB.reg = SERCOM_I2CM_CTRLB_CMD(CommandStop);
		return true;
	}

private:
	SercomI2cm& port;

	static constexpr unsigned CommandRead = 0x2;
	static constexpr unsigned CommandStop = 0x3;
	static constexpr unsigned BusstateIdle = 0x1;
	static constexpr unsigned BusstateOwner = 0x2;
	static constexpr unsigned StatusErrorMask = SERCOM_I2CM_STATUS_ARBLOST;

	static constexpr unsigned i2cRise = 215;
	static constexpr uint16_t computeBaud(unsigned desiredFrequency, unsigned coreFrequency)
	{
		//                  coreFreq - (desiredFreq * 10) - (coreFreq * desiredFreq * i2cRise)
		// BAUD + BAUDLOW = ------------------------------------------------------------------
		//                  desiredFreq
		int baudSum = (((coreFrequency - (desiredFrequency * 10) - (i2cRise * (desiredFrequency / 100) * (coreFrequency / 1000) / 10000)) * 10 + 5) / (desiredFrequency * 10));
		assert(2 <= baudSum && baudSum <= 0xff*2);

		if (baudSum % 2 == 1)
			return (baudSum / 2) | ((baudSum / 2 + 1) << 8);
		else
			return baudSum / 2;
	}
};

}


/*
GENETIC - A simple genetic algorithm.

Copyright 2014, Javier Burguete Tolosa.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

1. Redistributions of source code must retain the above copyright notice, this
	list of conditions and the following disclaimer.

2. 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.

THIS SOFTWARE IS PROVIDED BY Javier Burguete Tolosa ``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 Javier Burguete Tolosa 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.
*/

/**
 * \file evolution.c
 * \brief Source file to define evolution functions.
 * \author Javier Burguete Tolosa.
 * \copyright Copyright 2014 Javier Burguete Tolosa. All rights reserved.
 */
#define _GNU_SOURCE
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <gsl/gsl_rng.h>
//#include "config.h"
#include "bits.h"
#include "sort.h"
#include "entity.h"
#include "population.h"
#include "mutation.h"
#include "reproduction.h"
#include "adaptation.h"
#include "selection.h"
#include "evolution.h"

#define DEBUG_EVOLUTION 0       ///< Macro to debug the evolution functions.

/**
 * Function to sort the survivals of the evaluated population
 */
void
evolution_sort (Population * population)        ///< Population.
{
  unsigned int i, j, index[population->nentities];
  double objective[population->nsurvival];
  Entity entity[population->nsurvival];
#if DEBUG_EVOLUTION
  fprintf (stderr, "evolution_sort: start\n");
#endif
  index_new (population->objective, index, population->nentities);
  for (i = 0; i < population->nsurvival; ++i)
    {
      j = index[i];
      objective[i] = population->objective[j];
      entity_new (entity + i, population->genome_nbytes, i);
      memcpy (entity[i].genome, population->entity[j].genome,
              population->genome_nbytes);
    }
  for (i = 0; i < population->nsurvival; ++i)
    {
      population->objective[i] = objective[i];
      memcpy (population->entity[i].genome, entity[i].genome,
              population->genome_nbytes);
      entity_free (entity + i);
    }
#if DEBUG_EVOLUTION
  fprintf (stderr, "evolution_sort: end\n");
#endif
}

/**
 * Funtion to apply the mutation evolution.
 */
void
evolution_mutation (Population * population,    ///< Population.
                    gsl_rng * rng)      ///< GSL random numbers generator.
{
  unsigned int i;
  Entity *mother, *son;
#if DEBUG_EVOLUTION
  fprintf (stderr, "evolution_mutation: start\n");
#endif
  for (i = population->mutation_max; i > population->mutation_min;)
    {
#if DEBUG_EVOLUTION
      fprintf (stderr, "evolution_mutation: selection\n");
#endif
      selection_mutation (population, &mother, rng);
      son = population->entity + --i;
#if DEBUG_EVOLUTION
      fprintf (stderr, "evolution_mutation: mutation in %u\n", i);
#endif
      mutation (population, mother, son, rng);
    }
#if DEBUG_EVOLUTION
  fprintf (stderr, "evolution_mutation: end\n");
#endif
}

/**
 * Funtion to apply the reproduction evolution.
 */
void
evolution_reproduction (Population * population,        ///< Population.
                        gsl_rng * rng)  ///< GSL random numbers generator.
{
  unsigned int i;
  Entity *mother, *father, *son;
#if DEBUG_EVOLUTION
  fprintf (stderr, "evolution_reproduction: start\n");
#endif
  for (i = population->reproduction_max; i > population->reproduction_min;)
    {
#if DEBUG_EVOLUTION
      fprintf (stderr, "evolution_reproduction: selection\n");
#endif
      selection_reproduction (population, &mother, &father, rng);
      son = population->entity + --i;
#if DEBUG_EVOLUTION
      fprintf (stderr, "evolution_reproduction: reproduction in %u\n", i);
#endif
      reproduction (mother, father, son, population->genome_nbits, rng);
    }
#if DEBUG_EVOLUTION
  fprintf (stderr, "evolution_reproduction: end\n");
#endif
}

/**
 * Funtion to apply the adaptation evolution.
 */
void
evolution_adaptation (Population * population,  ///< Population.
                      gsl_rng * rng)    ///< GSL random numbers generator.
{
  unsigned int i;
  Entity *mother, *son;
#if DEBUG_EVOLUTION
  fprintf (stderr, "evolution_adaptation: start\n");
#endif
  for (i = population->adaptation_max; i > population->adaptation_min;)
    {
#if DEBUG_EVOLUTION
      fprintf (stderr, "evolution_adaptation: selection\n");
#endif
      selection_adaptation (population, &mother, rng);
      son = population->entity + --i;
#if DEBUG_EVOLUTION
      fprintf (stderr, "evolution_adaptation: adaptation in %u\n", i);
#endif
      adaptation (population, mother, son, rng);
    }
#if DEBUG_EVOLUTION
  fprintf (stderr, "evolution_adaptation: end\n");
#endif
}