/* rng/gsl_rng.h
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 James Theiler, 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 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., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

#ifndef __GSL_RNG_H__
#define __GSL_RNG_H__
#include <stdlib.h>
#include <gsl/gsl_errno.h>

#undef __BEGIN_DECLS
#undef __END_DECLS
#ifdef __cplusplus
# define __BEGIN_DECLS extern "C" {
# define __END_DECLS }
#else
# define __BEGIN_DECLS /* empty */
# define __END_DECLS /* empty */
#endif

__BEGIN_DECLS

typedef struct
  {
    const char *name;
    unsigned long int max;
    unsigned long int min;
    size_t size;
    void (*set) (void *state, unsigned long int seed);
    unsigned long int (*get) (void *state);
    double (*get_double) (void *state);
  }
gsl_rng_type;

typedef struct
  {
    const gsl_rng_type * type;
    void *state;
  }
gsl_rng;

/* These structs also need to appear in default.c so you can select
   them via the environment variable GSL_RNG_TYPE */

extern const gsl_rng_type *gsl_rng_cmrg;
extern const gsl_rng_type *gsl_rng_gfsr4;
extern const gsl_rng_type *gsl_rng_minstd;
extern const gsl_rng_type *gsl_rng_mrg;
extern const gsl_rng_type *gsl_rng_mt19937;
extern const gsl_rng_type *gsl_rng_mt19937_1998;
extern const gsl_rng_type *gsl_rng_r250;
extern const gsl_rng_type *gsl_rng_ran0;
extern const gsl_rng_type *gsl_rng_ran1;
extern const gsl_rng_type *gsl_rng_ran2;
extern const gsl_rng_type *gsl_rng_ran3;
extern const gsl_rng_type *gsl_rng_rand48;
extern const gsl_rng_type *gsl_rng_rand;
extern const gsl_rng_type *gsl_rng_random128_bsd;
extern const gsl_rng_type *gsl_rng_random128_glibc2;
extern const gsl_rng_type *gsl_rng_random128_libc5;
extern const gsl_rng_type *gsl_rng_random256_bsd;
extern const gsl_rng_type *gsl_rng_random256_glibc2;
extern const gsl_rng_type *gsl_rng_random256_libc5;
extern const gsl_rng_type *gsl_rng_random32_bsd;
extern const gsl_rng_type *gsl_rng_random32_glibc2;
extern const gsl_rng_type *gsl_rng_random32_libc5;
extern const gsl_rng_type *gsl_rng_random64_bsd;
extern const gsl_rng_type *gsl_rng_random64_glibc2;
extern const gsl_rng_type *gsl_rng_random64_libc5;
extern const gsl_rng_type *gsl_rng_random8_bsd;
extern const gsl_rng_type *gsl_rng_random8_glibc2;
extern const gsl_rng_type *gsl_rng_random8_libc5;
extern const gsl_rng_type *gsl_rng_random_bsd;
extern const gsl_rng_type *gsl_rng_random_glibc2;
extern const gsl_rng_type *gsl_rng_random_libc5;
extern const gsl_rng_type *gsl_rng_randu;
extern const gsl_rng_type *gsl_rng_ranf;
extern const gsl_rng_type *gsl_rng_ranlux389;
extern const gsl_rng_type *gsl_rng_ranlux;
extern const gsl_rng_type *gsl_rng_ranlxd1;
extern const gsl_rng_type *gsl_rng_ranlxd2;
extern const gsl_rng_type *gsl_rng_ranlxs0;
extern const gsl_rng_type *gsl_rng_ranlxs1;
extern const gsl_rng_type *gsl_rng_ranlxs2;
extern const gsl_rng_type *gsl_rng_ranmar;
extern const gsl_rng_type *gsl_rng_slatec;
extern const gsl_rng_type *gsl_rng_taus;
extern const gsl_rng_type *gsl_rng_transputer;
extern const gsl_rng_type *gsl_rng_tt800;
extern const gsl_rng_type *gsl_rng_uni32;
extern const gsl_rng_type *gsl_rng_uni;
extern const gsl_rng_type *gsl_rng_vax;
extern const gsl_rng_type *gsl_rng_zuf;

const gsl_rng_type ** gsl_rng_types_setup(void);

extern const gsl_rng_type *gsl_rng_default;
extern unsigned long int gsl_rng_default_seed;

gsl_rng *gsl_rng_alloc (const gsl_rng_type * T);
int gsl_rng_memcpy (gsl_rng * dest, const gsl_rng * src);
gsl_rng *gsl_rng_clone (const gsl_rng * r);

void gsl_rng_free (gsl_rng * r);

void gsl_rng_set (const gsl_rng * r, unsigned long int seed);
unsigned long int gsl_rng_max (const gsl_rng * r);
unsigned long int gsl_rng_min (const gsl_rng * r);
const char *gsl_rng_name (const gsl_rng * r);
size_t gsl_rng_size (const gsl_rng * r);
void * gsl_rng_state (const gsl_rng * r);

void gsl_rng_print_state (const gsl_rng * r);

const gsl_rng_type * gsl_rng_env_setup (void);

unsigned long int gsl_rng_get (const gsl_rng * r);
double gsl_rng_uniform (const gsl_rng * r);
double gsl_rng_uniform_pos (const gsl_rng * r);
unsigned long int gsl_rng_uniform_int (const gsl_rng * r, unsigned long int n);

#ifdef HAVE_INLINE
extern inline unsigned long int gsl_rng_get (const gsl_rng * r);

extern inline unsigned long int
gsl_rng_get (const gsl_rng * r)
{
  return (r->type->get) (r->state);
}

extern inline double gsl_rng_uniform (const gsl_rng * r);

extern inline double
gsl_rng_uniform (const gsl_rng * r)
{
  return (r->type->get_double) (r->state);
}

extern inline double gsl_rng_uniform_pos (const gsl_rng * r);

extern inline double
gsl_rng_uniform_pos (const gsl_rng * r)
{
  double x ;
  do
    {
      x = (r->type->get_double) (r->state) ;
    }
  while (x == 0) ;

  return x ;
}

extern inline unsigned long int gsl_rng_uniform_int (const gsl_rng * r, unsigned long int n);

extern inline unsigned long int
gsl_rng_uniform_int (const gsl_rng * r, unsigned long int n)
{
  unsigned long int offset = r->type->min;
  unsigned long int range = r->type->max - offset;
  unsigned long int scale = range / n;
  unsigned long int k;

  if (n > range)
    {
      GSL_ERROR_VAL ("n exceeds maximum value of generator",
			GSL_EINVAL, 0) ;
    }

  do
    {
      k = (((r->type->get) (r->state)) - offset) / scale;
    }
  while (k >= n);

  return k;
}
#endif /* HAVE_INLINE */

__END_DECLS

#endif /* __GSL_RNG_H__ */


#ifndef AMICI_SUNDIALS_MATRIX_WRAPPER_H
#define AMICI_SUNDIALS_MATRIX_WRAPPER_H

#include <sunmatrix/sunmatrix_band.h>   // SUNMatrix_Band
#include <sunmatrix/sunmatrix_dense.h>  // SUNMatrix_Dense
#include <sunmatrix/sunmatrix_sparse.h> // SUNMatrix_Sparse

#include <gsl/gsl-lite.hpp>

#include <vector>

#include "amici/vector.h"

namespace amici {

/**
 * @brief A RAII wrapper for SUNMatrix structs.
 *
 * This can create dense, sparse, or banded matrices using the respective
 * constructor.
 */
class SUNMatrixWrapper {
  public:
    SUNMatrixWrapper() = default;

    /**
     * @brief Create sparse matrix. See SUNSparseMatrix in sunmatrix_sparse.h
     * @param M Number of rows
     * @param N Number of columns
     * @param NNZ Number of nonzeros
     * @param sparsetype Sparse type
     */
    SUNMatrixWrapper(int M, int N, int NNZ, int sparsetype);

    /**
     * @brief Create dense matrix. See SUNDenseMatrix in sunmatrix_dense.h
     * @param M Number of rows
     * @param N Number of columns
     */
    SUNMatrixWrapper(int M, int N);

    /**
     * @brief Create banded matrix. See SUNBandMatrix in sunmatrix_band.h
     * @param M Number of rows and columns
     * @param ubw Upper bandwidth
     * @param lbw Lower bandwidth
     */
    SUNMatrixWrapper(int M, int ubw, int lbw);

    /**
     * @brief Create sparse matrix from dense or banded matrix. See
     * SUNSparseFromDenseMatrix and SUNSparseFromBandMatrix in
     * sunmatrix_sparse.h
     * @param A Wrapper for dense matrix
     * @param droptol tolerance for dropping entries
     * @param sparsetype Sparse type
     */
    SUNMatrixWrapper(const SUNMatrixWrapper &A, realtype droptol,
                     int sparsetype);

    /**
     * @brief Wrap existing SUNMatrix
     * @param mat
     */
    explicit SUNMatrixWrapper(SUNMatrix mat);

    ~SUNMatrixWrapper();

    /**
     * @brief Copy constructor
     * @param other
     */
    SUNMatrixWrapper(const SUNMatrixWrapper &other);

    /**
     * @brief Move constructor
     * @param other
     */
    SUNMatrixWrapper(SUNMatrixWrapper &&other);

    /**
     * @brief Copy assignment
     * @param other
     * @return
     */
    SUNMatrixWrapper &operator=(const SUNMatrixWrapper &other);

    /**
     * @brief Move assignment
     * @param other
     * @return
     */
    SUNMatrixWrapper &operator=(SUNMatrixWrapper &&other);

    /**
     * @brief Access raw data
     * @return raw data pointer
     */
    realtype *data() const;

    /**
     * @brief Get the wrapped SUNMatrix
     * @return SlsMat
     */
    SUNMatrix get() const;

    /**
     * @brief Get the number of rows
     * @return number
     */
    sunindextype rows() const;

    /**
     * @brief Get the number of columns
     * @return number
     */
    sunindextype columns() const;

    /**
     * @brief Get the number of non-zero elements (sparse matrices only)
     * @return number
     */
    sunindextype nonzeros() const;

    /**
     * @brief Get the index values of a sparse matrix
     * @return index array
     */
    sunindextype *indexvals() const;

    /**
     * @brief Get the index pointers of a sparse matrix
     * @return index array
     */
    sunindextype *indexptrs() const;

    /**
     * @brief Get the type of sparse matrix
     * @return index array
     */
    int sparsetype() const;

    /**
     * @brief reset data to zeroes
     */
    void reset();

    /**
     * @brief multiply with a scalar (in-place)
     * @param a scalar value to multiply matrix
     */
    void scale(realtype a);

    /**
     * @brief N_Vector interface for multiply
     * @param c output vector, may already contain values
     * @param b multiplication vector
     */
    void multiply(N_Vector c, const_N_Vector b) const;

    /**
     * @brief Perform matrix vector multiplication c += A*b
     * @param c output vector, may already contain values
     * @param b multiplication vector
     */
    void multiply(gsl::span<realtype> c, gsl::span<const realtype> b) const;

    /**
     * @brief Perform reordered matrix vector multiplication c += A[:,cols]*b
     * @param c output vector, may already contain values
     * @param b multiplication vector
     * @param cols int vector for column reordering
     * @param transpose bool transpose A before multiplication
     */
    void multiply(N_Vector c,
                  const N_Vector b,
                  gsl::span <const int> cols,
                  bool transpose) const;

    /**
     * @brief Perform reordered matrix vector multiplication c += A[:,cols]*b
     * @param c output vector, may already contain values
     * @param b multiplication vector
     * @param cols int vector for column reordering
     * @param transpose bool transpose A before multiplication
     */
    void multiply(gsl::span<realtype> c,
                  gsl::span<const realtype> b,
                  gsl::span <const int> cols,
                  bool transpose) const;

    /**
     * @brief Perform matrix matrix multiplication
              C[:, :] += A * B
              for sparse A, B, C
     * @param C output matrix, may already contain values
     * @param B multiplication matrix
     */
    void sparse_multiply(SUNMatrixWrapper *C,
                         SUNMatrixWrapper *B) const;

    /**
     * @brief Set to 0.0
     */
    void zero();

  private:
    void update_ptrs();

    /**
     * @brief CSC matrix to which all methods are applied
     */
    SUNMatrix matrix_ {nullptr};
    realtype *data_ptr_ {nullptr};
    sunindextype *indexptrs_ptr_ {nullptr};
    sunindextype *indexvals_ptr_ {nullptr};
};

} // namespace amici

namespace gsl {
/**
 * @brief Create span from SUNMatrix
 * @param nv
 * @return
 */
inline span<realtype> make_span(SUNMatrix m)
{
    switch (SUNMatGetID(m)) {
    case SUNMATRIX_DENSE:
        return span<realtype>(SM_DATA_D(m), SM_LDATA_D(m));
    case SUNMATRIX_SPARSE:
        return span<realtype>(SM_DATA_S(m), SM_NNZ_S(m));
    default:
        throw amici::AmiException("Unimplemented SUNMatrix type for make_span");
    }
}
} // namespace gsl

#endif // AMICI_SUNDIALS_MATRIX_WRAPPER_H


#pragma once

#include <memory>
#include <iterator>
#include <cstddef>
#include <gsl/gsl>

namespace dr {

/// \brief round `s` up to the nearest multiple of n
template<typename T>
T round_up(T s, unsigned int n) { return ((s + n - 1) / n) * n; }

template<typename T, typename Allocator = std::allocator<T>>
struct gap_buffer {
  using value_type      = T;
  using allocator_type  = Allocator;
  using size_type       = std::size_t;
  using difference_type = ptrdiff_t;
  using reference       = value_type&;
  using const_reference = const value_type&;
  using pointer         = typename std::allocator_traits<Allocator>::pointer;
  using const_pointer   = typename std::allocator_traits<Allocator>::const_pointer;

  struct const_iterator;

  struct iterator {
    using self_type         = iterator;
    using container_type    = gap_buffer;

    using value_type        = container_type::value_type;
    using difference_type   = container_type::difference_type;
    using reference         = container_type::reference;
    using pointer           = container_type::pointer;
    using iterator_category = std::random_access_iterator_tag;

    explicit iterator(gap_buffer* container = nullptr, difference_type offset = 0)
        : container(container), offset(offset) { }

    operator const_iterator() const {
      return const_iterator(container, offset);
    }

    reference operator [](difference_type i) const {
      return (*container)[offset + i];
    }

    reference operator *() const {
      return (*container)[offset];
    }

    pointer operator ->() const {
      return &(*container)[offset];
    }

    self_type& operator ++() {
      offset++;
      return *this;
    }

    self_type operator ++(int) {
      self_type retval = *this;
      this->operator ++();
      return retval;
    }

    self_type& operator --() {
      offset--;
      return *this;
    }

    self_type operator --(int) {
      self_type retval = *this;
      this->operator --();
      return retval;
    }

    bool operator ==(const self_type& other) const {
      return container == other.container && offset == other.offset;
    }

    bool operator !=(const self_type& other) const {
      return !(*this == other);
    }

    bool operator <(const self_type& other) const {
      Expects(container == other.container);
      return offset < other.offset;
    }

    bool operator >(const self_type& other) const {
      return other < *this;
    }

    bool operator <=(const self_type& other) const {
      return !(other < *this);
    }

    bool operator >=(const self_type& other) const {
      return !(*this < other);
    }

    self_type& operator +=(difference_type n) {
      offset += n;
      return *this;
    }

    friend
    self_type operator +(self_type it, difference_type n) {
      it.offset += n;
      return it;
    }

    friend
    self_type operator +(difference_type n, self_type it) {
      it.offset += n;
      return it;
    }

    self_type& operator -=(difference_type n) {
      offset -= n;
      return *this;
    }

    self_type operator -(difference_type n) const {
      return self_type(container, offset - n);
    }

    difference_type operator -(const self_type& other) const {
      Expects(container == other.container);
      return offset - other.offset;
    }

    friend class gap_buffer;

  private:
    gap_buffer* container;
    difference_type offset;
  };

  // Here we repeat ourselves, that is, DRY principle is violated.
  // We can get rid of the repetition with CRTP. But let's keep it
  // as is.
  struct const_iterator {
    using self_type         = const_iterator;
    using container_type    = gap_buffer;

    using value_type        = container_type::value_type;
    using difference_type   = container_type::difference_type;
    using reference         = container_type::const_reference;
    using pointer           = container_type::const_pointer;
    using iterator_category = std::random_access_iterator_tag;

    explicit const_iterator(const gap_buffer* container = nullptr, difference_type offset = 0)
        : container(container),
          offset(offset) { }

    reference operator [](difference_type i) const {
      return (*container)[offset + i];
    }

    reference operator *() const {
      return (*container)[offset];
    }

    pointer operator ->() const {
      return &(*container)[offset];
    }

    self_type& operator ++() {
      offset++;
      return *this;
    }

    self_type operator ++(int) {
      self_type retval = *this;
      this->operator ++();
      return retval;
    }

    self_type& operator --() {
      offset--;
      return *this;
    }

    self_type operator --(int) {
      self_type retval = *this;
      this->operator --();
      return retval;
    }

    bool operator ==(const self_type& other) const {
      return container == other.container && offset == other.offset;
    }

    bool operator !=(const self_type& other) const {
      return !(*this == other);
    }

    bool operator <(const self_type& other) const {
      Expects(container == other.container);
      return offset < other.offset;
    }

    bool operator >(const self_type& other) const {
      return other < *this;
    }

    bool operator <=(const self_type& other) const {
      return !(other < *this);
    }

    bool operator >=(const self_type& other) const {
      return !(*this < other);
    }

    self_type& operator +=(difference_type n) {
      offset += n;
      return *this;
    }

    friend
    self_type operator +(self_type it, difference_type n) {
      it.offset += n;
      return it;
    }

    friend
    self_type operator +(difference_type n, self_type it) {
      it.offset += n;
      return it;
    }

    self_type& operator -=(difference_type n) {
      offset -= n;
      return *this;
    }

    self_type operator -(difference_type n) const {
      return self_type(container, offset - n);
    }

    difference_type operator -(const self_type& other) const {
      Expects(container == other.container);
      return offset - other.offset;
    }

    friend class gap_buffer;

  private:
    const gap_buffer* container;
    difference_type offset;
  };

  using reverse_iterator       = std::reverse_iterator<iterator>;
  using const_reverse_iterator = std::reverse_iterator<const_iterator>;

private:
  static constexpr float incremental_factor = 0.2;
  static constexpr size_type default_size = 8;
  static constexpr size_type alignment = 8;

public:
  explicit gap_buffer(size_type count = default_size) {
    if (count == 0) {
      start = finish = gap_start = nullptr;
      gap_size = 0;
    }
    else {
      count = round_up(count, alignment);

      start = allocate_and_construct(count);
      finish = start + count;
      gap_start = start;
      gap_size = count;
    }
  }

  gap_buffer(size_type count, const T& value)
      : gap_buffer(count) {
    for (size_type i = 0; i < count; ++i) push_back(value);
  }

  template<typename InputIt>
  gap_buffer(InputIt first, InputIt last) {
    difference_type n = std::distance(first, last);
    size_type len = round_up(std::max(default_size, size_type(n)), alignment);

    start = data_allocator.allocate(len);
    finish = start + len;

    int except_flag = 0;
    try {
      gap_start = std::uninitialized_copy(first, last, start);
      except_flag = 1;
      std::uninitialized_default_construct(gap_start, finish);
    }
    catch (...) {
      switch (except_flag) {
      case 1:
        std::destroy(start, start + n);
        // FALL THROUGH
      case 0:
        data_allocator.deallocate(start, finish - start);
      default:;// DO NOTHING
      }
      throw;
    }

    gap_size = len - n;
  }

  gap_buffer(const gap_buffer& rhs)
      : gap_buffer(rhs.begin(), rhs.end()) { }

  gap_buffer(gap_buffer&& rhs) noexcept
      : gap_buffer(0) { swap(rhs); }

  gap_buffer(std::initializer_list<T> ilist)
      : gap_buffer(ilist.begin(), ilist.end()) { }

  gap_buffer& operator =(const gap_buffer& rhs) {
    gap_buffer temp(rhs);
    swap(temp);
    return *this;
  }

  gap_buffer& operator =(gap_buffer&& rhs) noexcept {
    swap(rhs);
    return *this;
  }

  ~gap_buffer() {
    destroy_and_deallocate(start, finish);
    start = finish = gap_start = nullptr;
    gap_size = 0;
  }

  void swap(gap_buffer& rhs) {
    using std::swap;
    swap(start, rhs.start);
    swap(finish, rhs.finish);
    swap(gap_start, rhs.gap_start);
    swap(gap_size, rhs.gap_size);
  }

  void assign(size_type count, const T& value) { *this = gap_buffer(count, value); }

  template<typename InputIt>
  void assign(InputIt first, InputIt last) { *this = gap_buffer(first, last); }

  void assign(std::initializer_list<T> ilist) { *this = gap_buffer(ilist); }

  allocator_type get_allocator() const { return data_allocator; }

  // ------ basis START HERE ------

  const_reference operator [](size_type pos) const {
    if (start + pos < gap_start) return *(start + pos);
    else return *(start + gap_size + pos);
  }

  iterator erase(const_iterator first, const_iterator last) {
    Expects(first.container == this && last.container == this);
    difference_type num_to_erase = std::distance(first, last);
    relocate_gap(first.offset);
    std::fill_n(gap_start + gap_size, num_to_erase, T{});
    gap_size += num_to_erase;
    return iterator(this, first.offset);
  }

  /// \return iterator to the first inserted element
  template<class InputIt>
  iterator insert(const_iterator pos, InputIt first, InputIt last) {
    Expects(this == pos.container && pos <= end());

    difference_type num_to_insert = std::distance(first, last);
    if (gap_size >= num_to_insert) {
      relocate_gap(pos.offset);

      std::copy(first, last, gap_start);
      gap_start += num_to_insert;
      gap_size -= num_to_insert;
      return iterator(this, pos.offset);
    }
    else {
      size_type old_size = size();
      size_type old_capacity = capacity();
      auto default_delta = size_type(old_capacity * incremental_factor);
      size_type delta = round_up(std::max(default_delta, num_to_insert - gap_size), alignment);
      size_type new_capacity = std::max(old_capacity + delta, default_size);

      gap_buffer temp(new_capacity);

      relocate_gap(pos.offset);
      auto cursor = std::copy(start, gap_start, temp.start);
      cursor = std::copy(first, last, cursor);
      std::copy(gap_start + gap_size, finish, cursor);

      swap(temp);

      gap_start = start + old_size + num_to_insert;
      gap_size = finish - gap_start;

      return iterator(this, pos.offset);
    }
  }

  void reserve(size_type new_cap = 0) {
    if (capacity() >= new_cap) return;
    if (new_cap > max_size()) throw std::length_error("new_cap should be less than max_size()");

    size_type old_size = size();
    size_type new_capacity = round_up(new_cap, alignment);

    gap_buffer temp(new_capacity);
    auto cursor = std::copy(start, gap_start, temp.start);
    std::copy(gap_start + gap_size, finish, cursor);
    swap(temp);

    gap_start = start + old_size;
    gap_size = finish - gap_start;
  }

  size_type size() const noexcept { return finish - start - gap_size; }
  size_type max_size() const noexcept { return size_type(1 << 31); }
  size_type capacity() const noexcept { return finish - start; }

  // ------ basis END HERE ------

  reference operator [](size_type pos) {
    return const_cast<reference>(
        static_cast<const gap_buffer&>(*this)[pos]
    );
  }

  const_reference at(size_type pos) const {
    if (pos >= size()) throw std::out_of_range("index out of range");
    return (*this)[pos];
  }

  reference at(size_type pos) {
    return const_cast<reference>(
        static_cast<const gap_buffer&>(*this).at(pos)
    );
  }

  const_reference front() const { return (*this)[0]; }
  reference front() {
    return const_cast<reference>(
        static_cast<const gap_buffer&>(*this).front()
    );
  }

  const_reference back() const { return (*this)[size() - 1]; }
  reference back() {
    return const_cast<reference>(
        static_cast<const gap_buffer&>(*this).back()
    );
  }

  T* data() noexcept  = delete;
  const T* data() const noexcept = delete;

  [[nodiscard]] bool empty() const noexcept { return size() == 0; }

  void shrink_to_fit() {
    gap_buffer temp(begin(), end());
    swap(temp);
  }

  void clear() { erase(begin(), end()); }

  void resize(size_type count, const value_type& value = value_type{}) {
    if (count < size())
      erase(begin() + count, end());
    else
      insert(end(), count - size(), value);
  }

  iterator insert(const_iterator pos, const T& value) {
    return insert(pos, &value, &value + 1);
  }

  iterator insert(const_iterator pos, T&& value) {
    return insert(pos, std::make_move_iterator(&value), std::make_move_iterator(&value + 1));
  }

  iterator insert(const_iterator pos, size_type count, const T& value) {
    for (size_type i = 0; i < count; ++i)
      pos = insert(pos, value);
    return iterator(this, pos.offset);
  }

  iterator insert(const_iterator pos, std::initializer_list<T> ilist) {
    return insert(pos, ilist.begin(), ilist.end());
  }

  void erase(const_iterator pos) { erase(pos, pos + 1); }

  template<typename ... Args>
  iterator emplace(const_iterator pos, Args&& ... args) {
    return insert(pos, T(std::forward<Args>(args)...));
  }

  template<typename ... Args>
  reference emplace_back(Args&& ... args) {
    return *emplace(end(), std::forward<Args>(args)...);
  }

  void push_back(const T& value) { insert(end(), &value, &value + 1); }
  void push_back(T&& value) { insert(end(), std::make_move_iterator(&value), std::make_move_iterator(&value + 1)); }
  void pop_back() { erase(end() - 1); }

  const_iterator begin() const noexcept { return const_iterator(this); }
  const_iterator cbegin() const noexcept { return begin(); }

  const_iterator end() const noexcept { return const_iterator(this, size()); }
  const_iterator cend() const noexcept { return end(); }

  iterator begin() noexcept { return iterator(this); }
  iterator end() noexcept { return iterator(this, size()); }

  const_reverse_iterator rbegin() const noexcept { return const_reverse_iterator(end()); }
  const_reverse_iterator crbegin() const noexcept { return rbegin(); }

  const_reverse_iterator rend() const noexcept { return const_reverse_iterator(begin()); }
  const_reverse_iterator crend() const noexcept { return rend(); }

  reverse_iterator rbegin() noexcept { return reverse_iterator(end()); }
  reverse_iterator rend() noexcept { return reverse_iterator(begin()); }

  friend
  bool operator ==(const gap_buffer& lhs, const gap_buffer& rhs) {
    return std::equal(lhs.begin(), lhs.end(), rhs.begin(), rhs.end());
  }

  friend
  bool operator !=(const gap_buffer& lhs, const gap_buffer& rhs) {
    return !(lhs == rhs);
  }

  friend
  bool operator <(const gap_buffer& lhs, const gap_buffer& rhs) {
    auto[left, right] = std::mismatch(lhs.begin(), lhs.end(), rhs.begin(), rhs.end());

    if (right == rhs.end()) return false;
    else if (left == lhs.end()) return true;
    else return *left < *right;
  }

  friend
  bool operator >(const gap_buffer& lhs, const gap_buffer& rhs) {
    return rhs < lhs;
  }

  friend
  bool operator <=(const gap_buffer& lhs, const gap_buffer& rhs) {
    return !(rhs < lhs);
  }

  friend
  bool operator >=(const gap_buffer& lhs, const gap_buffer& rhs) {
    return !(lhs < rhs);
  }

  // additional
  template<typename InputIt>
  void append(InputIt first, InputIt last) {
    insert(end(), first, last);
  }

  void append(const T& value) { push_back(value); }

  void append(T&& value) { push_back(std::move(value)); }

  template<typename InputIt>
  void replace(const_iterator f1, const_iterator l1, InputIt f2, InputIt l2) {
    auto cursor = erase(f1, l1);
    insert(cursor, f2, l2);
  }

  template<typename InputIt>
  void replace(const_iterator pos, InputIt first, InputIt last) {
    replace(pos, pos + 1, first, last);
  }

  gap_buffer substr(const_iterator first, const_iterator last) const {
    return substr_impl<gap_buffer>(first, last);
  }

protected:

  template<typename U>
  U substr_impl(const_iterator first, const_iterator last) const {
    return U(first, last);
  }

  void relocate_gap(difference_type offset) {
    if (gap_start != start + offset) {
      if (gap_start < start + offset)
        std::move(gap_start /**/ + gap_size,
                  start + offset + gap_size,
                  gap_start);
      else
        std::move_backward(start + offset,
                           gap_start,
                           gap_start + gap_size);

      gap_start = start + offset;
    }
  }

  pointer allocate_and_construct(size_type n) {
    pointer result = data_allocator.allocate(n);
    std::uninitialized_default_construct_n(result, n);
    return result;
  }

  void destroy_and_deallocate(pointer start, pointer finish) {
    std::destroy(start, finish);
    if (start)
      data_allocator.deallocate(start, finish - start);
  }

private:
  Allocator data_allocator;

  pointer start;
  pointer finish;
  pointer gap_start;
  size_type gap_size;
};

template<typename T, typename Allocator>
void swap(gap_buffer<T, Allocator>& lhs, gap_buffer<T, Allocator>& rhs) {
  lhs.swap(rhs);
}

}


#ifndef LIB_INCLUDE_TICK_ARRAY_VECTOR_OPERATIONS_H_
#define LIB_INCLUDE_TICK_ARRAY_VECTOR_OPERATIONS_H_

// License: BSD 3 clause

#include <numeric>

#include <algorithm>
#include <atomic>
#include <type_traits>

#include "promote.h"
#include "tick/base/defs.h"

namespace tick {
namespace detail {

template <typename T>
struct DLL_PUBLIC vector_operations_unoptimized {
  template <typename K>
  tick::promote_t<K> sum(const ulong n, const T *x) const;

  template <typename K>
  typename std::enable_if<std::is_same<T, std::atomic<K>>::value, T>::type dot(
      const ulong n, const T *x, const K *y) const;

  template <typename K>
  typename std::enable_if<std::is_same<T, std::atomic<K>>::value, K>::type dot(
      const ulong n, const K *x, const T *y) const;

  template <typename K>
  typename std::enable_if<!std::is_same<T, std::atomic<K>>::value, T>::type dot(
      const ulong n, const T *x, const K *y) const;

  template <typename K>
  typename std::enable_if<std::is_same<T, std::atomic<K>>::value>::type scale(
      const ulong n, const K alpha, T *x) const;

  template <typename K>
  typename std::enable_if<!std::is_same<T, std::atomic<K>>::value>::type scale(
      const ulong n, const K alpha, T *x) const;

  template <typename K>
  typename std::enable_if<std::is_same<T, std::atomic<K>>::value>::type set(
      const ulong n, const K alpha, T *x) const;

  template <typename K>
  typename std::enable_if<!std::is_same<T, std::atomic<K>>::value>::type set(
      const ulong n, const K alpha, T *x) const;

  template <typename K, typename Y>
  typename std::enable_if<std::is_same<T, std::atomic<K>>::value &&
                          !std::is_same<Y, std::atomic<K>>::value>::type
  mult_incr(const uint64_t n, const K alpha, const Y *x, T *y) const;

  template <typename K, typename Y>
  typename std::enable_if<std::is_same<Y, std::atomic<K>>::value &&
                          !std::is_same<T, std::atomic<K>>::value>::type
  mult_incr(const uint64_t n, const K alpha, const Y *x, T *y) const;

  template <typename K, typename Y>
  typename std::enable_if<std::is_same<T, std::atomic<K>>::value &&
                          std::is_same<Y, std::atomic<K>>::value>::type
  mult_incr(const uint64_t n, const K alpha, const Y *x, T *y) const;

  template <typename K, typename Y>
  typename std::enable_if<std::is_same<T, K>::value &&
                          std::is_same<Y, K>::value>::type
  mult_incr(const uint64_t n, const K alpha, const Y *x, T *y) const;
};

}  // namespace detail
}  // namespace tick

#if !defined(TICK_CBLAS_AVAILABLE)

namespace tick {

template <typename T>
using vector_operations = detail::vector_operations_unoptimized<T>;

}  // namespace tick

#else  // if defined(TICK_CBLAS_AVAILABLE)

// Find available blas distribution
#if defined(TICK_USE_MKL)

#include <mkl.h>

#elif defined(__APPLE__)

#include <Accelerate/Accelerate.h>

// TODO(svp) Disabling this feature until we find a good way to determine if
// ATLAS is actually available #define XDATA_CATLAS_AVAILABLE

#else

extern "C" {
#include <cblas.h>
}

#endif  // defined(__APPLE__)

#include "tick/array/vector/cblas.hpp"

#endif  // if !defined(TICK_CBLAS_AVAILABLE)

#include "tick/array/vector/un_optimized.hpp"

#endif  // LIB_INCLUDE_TICK_ARRAY_VECTOR_OPERATIONS_H_


#ifndef S3DCV_DEMO_DEMO_UTILS_H
#define S3DCV_DEMO_DEMO_UTILS_H

#include <s3d/cv/disparity/disparity_analyzer_stan.h>

#include <gsl/gsl>

class BadNumberOfArgumentsException {};
class FileNotFoundException {};

/**
 * Creates dataset loader from program input arguments.
 *
 * First input argument must be the dataset folder, e.g. 'Arch'
 * Second input argument must be the dataset path e.g. '/home/you/dataset'
 *
 */
inline DatasetLoaderVSG createDatasetLoader(int argc, char* argv[]) {
  auto input_args = gsl::span<char*>(argv, argc);
  if (input_args.size() != 3) {
    throw BadNumberOfArgumentsException{};
  }
  const std::string datasetName = input_args[1];
  const std::string datasetPath = input_args[2];

  return DatasetLoaderVSG{datasetName, datasetPath};
}

inline s3d::robust::Lmeds<s3d::StanFundamentalMatrixSolver, s3d::SampsonDistanceFunction> createRansac(
        s3d::Size imageSize) {
  int h = imageSize.getHeight();
  int w = imageSize.getWidth();

  s3d::robust::Parameters params;
  params.nbTrials = 500;
  params.distanceThreshold = 0.01 * sqrt(h * h + w * w);

  return s3d::DisparityAnalyzerSTAN::RansacAlgorithmSTAN(params);
}

#endif //S3DCV_DEMO_DEMO_UTILS_H


/*
* -----------------------------------------------------------------
*  In Situ Adaptive Tabulation Library --- isat_lib.c
*  Version: 1.6180
*  Date: Nov 15, 2010
* -----------------------------------------------------------------
*  Programmer: Americo Barbosa da Cunha Junior
*              americo.cunhajr@gmail.com
* -----------------------------------------------------------------
*  Copyright (c) 2010 by Americo Barbosa da Cunha Junior
*
*  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.
*
*  A copy of the GNU General Public License is available in
*  LICENSE.txt or http://www.gnu.org/licenses/.
* -----------------------------------------------------------------
*  This is the implementation file of a library
*  to work with ISAT algorithm.
* -----------------------------------------------------------------
*/

#include <stdlib.h>
#include <math.h>
#include <time.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>

#include "../include/thrm_lib.h"
#include "../include/ell_lib.h"
#include "../include/ode_lib.h"
#include "../include/isat_lib.h"

/*
*------------------------------------------------------------
*   isat_alloc
*
*   This function alocates memory for a struct isat.
*
*   Output:
*   isat - pointer to a struct isat
*
*   last update: Jul 13, 2010
*------------------------------------------------------------
*/

isat_wrk *isat_alloc()
{
    /* memory allocation for isat_wrk */
    isat_wrk *isat = NULL;
    isat = (isat_wrk *) malloc(sizeof(isat_wrk));
    if ( isat == NULL )
        return NULL;

    /* setting isat_wrk elements equal NULL and 0.0 */
    isat->root     = NULL;
    isat->lf       = 0;
    isat->nd       = 0;
    isat->add      = 0;
    isat->grw      = 0;
    isat->rtv      = 0;
    isat->dev      = 0;
    isat->hgt      = 0;
    isat->max_lf   = 0;
    isat->time_add = 0.0;
    isat->time_grw = 0.0;
    isat->time_rtv = 0.0;
    isat->time_dev = 0.0;

    return isat;
}
/*------------------------------------------------------------*/

/*
*------------------------------------------------------------
*   isat_free
*
*   This function frees the memory used by a struct isat.
*
*   Input:
*   isat - pointer to a struct isat
*
*   last update: May 10, 2009
*------------------------------------------------------------
*/

void isat_free(void **isat_bl)
{
    isat_wrk *isat = NULL;

    /* checking if isat_bl is NULL */
    if ( *isat_bl == NULL )
        return;

    isat = (isat_wrk *) (*isat_bl);

    /* releasing allocated memory by isat_wrk elements */
    if( isat->root != NULL )
    {
        bst_node_free((void **)&(isat->root));
        isat->root = NULL;
    }

    /* releasing allocated memory by isat_wrk */
    free(*isat_bl);
    *isat_bl = NULL;

    return;
}
/*------------------------------------------------------------*/

/*
*------------------------------------------------------------
*   isat_set
*
*   This function initiates ISAT binary search tree.
*
*   Input:
*   isat - pointer to a struct isat
*
*   last update: May 13, 2009
*------------------------------------------------------------
*/

int isat_set(isat_wrk *isat)
{
    /* checking if isat is NULL */
    if ( isat == NULL )
        return GSL_EINVAL;

    /* memory allocation for binary search tree root */
    if ( isat->root == NULL )
    {
        isat->root = bst_node_alloc();
        if ( isat->root == NULL )
        {
            free(isat);
            isat = NULL;
            return GSL_EINVAL;
        }
    }

    return GSL_SUCCESS;
}
/*------------------------------------------------------------*/

/*
*------------------------------------------------------------
*   isat_statistics
*
*   This function prints on the screen informations
*   about the isat workspace.
*
*   Input:
*   isat - pointer to a struct isat
*
*   last update: Jul 13, 2010
*------------------------------------------------------------
*/

void isat_statistics(isat_wrk *isat)
{
    double time_add;
    double time_grw;
    double time_rtv;
    double time_dev;

    time_add = (double) (isat->time_add / CLOCKS_PER_SEC) / isat->add;
    time_grw = (double) (isat->time_grw / CLOCKS_PER_SEC) / isat->grw;
    time_rtv = (double) (isat->time_rtv / CLOCKS_PER_SEC) / isat->rtv;
    time_dev = (double) (isat->time_dev / CLOCKS_PER_SEC) / isat->dev;

    printf("\n ISAT statistics:");
    printf("\n # of adds         = %d",   isat->add);
    printf("\n # of grows        = %d",   isat->grw);
    printf("\n # of retrieves    = %d",   isat->rtv);
    printf("\n # of dir. eval.   = %d",   isat->dev);
    printf("\n # of leaves       = %d",   isat->lf);
    printf("\n # of nodes        = %d",   isat->nd);
    printf("\n tree height       = %d\n", isat->hgt);

    printf("\n average values for CPU time (s):");
    printf("\n add: %+.6e",time_add);
    printf("\n grw: %+.6e",time_grw);
    printf("\n rtv: %+.6e",time_rtv);
    printf("\n dev: %+.6e",time_dev);

    return;
}
/*------------------------------------------------------------*/

/*
* -----------------------------------------------------------------
*   isat_input
*
*   This function receives ISAT parameters.
*
*   Input:
*   max_lf - maximum value of tree leaves
*   etol   - ISAT error tolerance
*   n0     - factor to multiply by unit roundoff
*
*   Output:
*   success or error
*
*   last update: Feb 25, 2010
* -----------------------------------------------------------------
*/

int isat_input(unsigned int *max_lf,double *etol,double *n0)
{
    printf("\n Input ISAT parameters:\n");

    printf("\n maximum of tree leaves:");
    scanf("%d", max_lf);
    printf("\n %d\n", *max_lf);
    if( *max_lf <= 0 )
	GSL_ERROR(" max_lf must be a positive integer (max_lf > 0)",GSL_EINVAL);

    printf("\n ISAT error tolerance:");
    scanf("%lf", etol);
    printf("\n %+.1e\n", *etol);
    if( *etol < 0.0 )
	GSL_ERROR(" etol must be grather than zero (etol > 0.0)",GSL_EINVAL);

    printf("\n Unit roundoff multiple:");
    scanf("%lf", n0);
    printf("\n %+.1e\n", *n0);
    if( *n0 < 0.0 )
	GSL_ERROR(" n0 must be grather than zero (n0 > 0.0)",GSL_EINVAL);

    return GSL_SUCCESS;
}
/*----------------------------------------------------------------*/

/*
*------------------------------------------------------------
*   isat_eoa_matrix
*
*   This function computes the EOA matrix in Cholesky form.
*
*   Input:
*   A    - gradient matrix
*   n0   - factor to multiply by unit roundoff
*   etol - error tolerance
*
*   Output:
*   L    - EOA Cholesky matrix
*
*   last update: Feb 19, 2010
*------------------------------------------------------------
*/

void isat_eoa_mtrx(gsl_matrix *A,double etol,double n0,
						gsl_matrix *L)
{
    unsigned int i;
    double  eps_max   = 0.5;
    double  eps_min   = etol/(n0*DBL_EPSILON);
    gsl_matrix *Aetol = NULL;
    gsl_matrix *V     = NULL;
    gsl_vector *sig   = NULL;

    /* memory allocation */
    Aetol = gsl_matrix_calloc(A->size2,A->size2);
    V     = gsl_matrix_calloc(A->size2,A->size2);
    sig   = gsl_vector_calloc(A->size2);

    /* Aetol := A */
    gsl_matrix_memcpy(Aetol,A);

    /* Aetol := (1/etol).A */
    gsl_matrix_scale (Aetol,1.0/etol);

    /* Aetol = U*sig*V^T */
    ell_psd2eig(Aetol,V,sig);

    /* eliminating small and large singular values */
    for ( i = 0; i < sig->size; i++ )
        sig->data[i] = GSL_MIN( GSL_MAX(sig->data[i],eps_max), eps_min);

    /* V*sig^2*V^T = L*L^T */
    ell_eig2chol(V,sig,L);

    /* releasing allocated memory */
    gsl_vector_free(sig);
    gsl_matrix_free(V);
    gsl_matrix_free(Aetol);
    sig   = NULL;
    V     = NULL;
    Aetol = NULL;

    return;
}
/*------------------------------------------------------------*/

/*
*------------------------------------------------------------
*   isat_lerror
*
*   This function computes the local error defined as
*
*   eps = 2-norm(Rl(phi)- R(phi)) where
*
*   Rl(phi) = R(phi0) + A*(phi-phi0).
*
*   Input:
*   Rphi  - reaction mapping of phi
*   Rphi0 - reaction mapping of phi0
*   A     - mapping gradient matrix
*   phi   - query composition
*   phi0  - initial composition
*
*   Output:
*   eps   - local error
*
*   last update: Feb 19, 2010
*------------------------------------------------------------
*/

double isat_lerror(gsl_vector *Rphi,gsl_vector *Rphi0,
		gsl_matrix *A,gsl_vector *phi,gsl_vector *phi0)
{
    double eps;
    gsl_vector *Rlphi = NULL;

    /* memory allocation for Rlphi */
    Rlphi = gsl_vector_calloc(phi->size);

    /* Rlphi := R(phi0) + A*(phi-phi0) */
    linear_approx(phi,phi0,Rphi0,A,Rlphi);

    /* Rlphi := Rl(phi) - R(phi) */
    gsl_vector_sub(Rlphi,Rphi);

    /* eps := 2-norm(Rl(phi) - R(phi)) */
    eps = gsl_blas_dnrm2(Rlphi);

    /* releasing allocated memory */
    gsl_vector_free(Rlphi);
    Rlphi = NULL;

    return eps;
}
/*------------------------------------------------------------*/

/*
*------------------------------------------------------------
*   isat4
*
*   This function executes the 4-th version of the
*   in situ adaptive tabulation algorithm.
*
*
*   Input:
*   isat      - isat workspace
*   thrm_data - thermochemistry workspace
*   etol      - error tolerance
*   n0        - factor to multiply by unit roundoff
*   t0        - initial time
*   delta_t   - time step
*   phi       - query composition
*   A         - mapping gradient matrix
*   L         - EOA Cholesky matrix
*
*   Output:
*   Rphi      - reaction mapping
*   success or error
*
*   last update: Nov 15, 2010
*------------------------------------------------------------
*/

int isat4(isat_wrk *isat,void *thrm_data,void *cvode_mem,
	    double etol,double n0,double t0,double delta_t,
    gsl_vector *phi,gsl_matrix *A,gsl_matrix *L,gsl_vector *Rphi)
{
    clock_t cpu_start = clock();

    /* ISAT first step */
    if( isat->lf == 0 )
    {
        int flag;
        bst_leaf *first_leaf = NULL;

	/* memory allocation for first_leaf */
        first_leaf = bst_leaf_alloc();
	if ( first_leaf == NULL )
                return GSL_ENOMEM;

        /* performing direct integration */
        flag = odesolver_reinit(thrm_eqs,thrm_data,t0,
                                        ATOL,RTOL,phi,cvode_mem);
        if ( flag != GSL_SUCCESS )
            return flag;

        flag = odesolver(cvode_mem,delta_t,Rphi);
        if ( flag != GSL_SUCCESS )
            return flag;

        /* computing the mapping gradient matrix */
        flag = gradient(thrm_eqs,thrm_data,cvode_mem,t0,delta_t,
                                                ATOL,RTOL,phi,Rphi,A);
        if ( flag != GSL_SUCCESS )
            return flag;

        /* computing EOA Cholesky matrix */
        isat_eoa_mtrx(A,etol,n0,L);

        /* setting first leaf elements */
        bst_leaf_set(phi,Rphi,A,L,first_leaf);
        isat->root->r_leaf = first_leaf;

        /* updating leaves and height counters */
        isat->lf++;
        isat->hgt = bst_height(isat->root);

        return GSL_SUCCESS;
    }

    /* ISAT second and following steps */
    int side, flag;
    double dot         = 0.0;
    bst_leaf *end_leaf = NULL;
    bst_node *end_node = NULL;

    /* searching for the near composition in binary search tree */
    if( isat->lf > 1 )
	side = bst_search(isat->root,phi,&end_node,&end_leaf);
    else
    {
        end_node = isat->root;
        end_leaf = isat->root->r_leaf;
        side     = RIGHT;
    }

    /* checking if the near composition is inside EOA */
    dot = ell_pt_in(phi,end_leaf->phi,end_leaf->L);
    if( gsl_fcmp(dot,1.0,ATOL) < 1 )
    {
        /* computing linear approximation */
        linear_approx(phi,end_leaf->phi,end_leaf->Rphi,end_leaf->A,Rphi);

        /* updating counters */
        isat->rtv++;
	isat->time_rtv += clock() - cpu_start;

        return GSL_SUCCESS;
    }
    else
    {
        double lerror = 0.0;

        /* performing direct integration */
        flag = odesolver_reinit(thrm_eqs,thrm_data,t0,
                                        ATOL,RTOL,phi,cvode_mem);
        if ( flag != GSL_SUCCESS )
            return flag;

        flag = odesolver(cvode_mem,delta_t,Rphi);
        if ( flag != GSL_SUCCESS )
            return flag;

        /* computing ISAT local error */
        lerror = isat_lerror(Rphi,end_leaf->Rphi,
                            end_leaf->A,phi,end_leaf->phi);

        /* checking if lerror is greater than etol */
        if( gsl_fcmp(lerror,etol,ATOL) < 1 )
        {
            /* growing the EOA */
            ell_pt_modify(phi,end_leaf->phi,end_leaf->L);

            /* updating counters */
            isat->grw++;
	    isat->time_grw += clock() - cpu_start;

            return GSL_SUCCESS;
        }
        else
        {
	    /* checking if the maximum # of leaves was excced */
	    if( isat->lf > isat->max_lf )
	    {
	        /*  updating counter */
	        isat->dev++;
		isat->time_dev +=  clock() - cpu_start;

		return GSL_SUCCESS;
	    }

            bst_leaf *new_leaf = NULL;

	    /* memory allocation */
            new_leaf = bst_leaf_alloc();
            if ( new_leaf == NULL )
                return GSL_ENOMEM;

            /* computing the mapping gradient matrix */
            flag = gradient(thrm_eqs,thrm_data,cvode_mem,t0,delta_t,
                                                ATOL,RTOL,phi,Rphi,A);
            if ( flag != GSL_SUCCESS )
		return flag;

            /* computing EOA Cholesky matrix */
            isat_eoa_mtrx(A,etol,n0,L);

            /* setting new_leaf elements */
            bst_leaf_set(phi,Rphi,A,L,new_leaf);

            /* cheking if binary search tree has more than one leaf */
            if( isat->lf > 1 )
            {
                bst_node *new_node = NULL;

		/* memory allocation for new node */
                new_node = bst_node_alloc();
                if ( new_node == NULL )
                    return GSL_ENOMEM;

		/* setting the leaves of the new node */
                bst_node_set(end_leaf,new_leaf,new_node);

		/* adding the new node to the tree */
                bst_node_add(side,end_node,new_node);
            }
	    else
                bst_node_set(end_leaf,new_leaf,end_node);

            /* updating counters */
            isat->add++;
            isat->lf++;
            isat->nd++;
            isat->hgt = bst_height(isat->root);
	    isat->time_add += clock() - cpu_start;

            return GSL_SUCCESS;
        }
    }
}
/*------------------------------------------------------------*/