#pragma once

#include "common.h"
#include "multibase.h"
#include "uvarint.h"
#include <gsl/gsl>
#include <map>

namespace multiformats {

    enum hash_t {
        dynamic_hash = -1,
        sha1,
        sha2_256
    };

    namespace details {

        struct hashimpl {
            hash_t      key;
            const char* name;
            uint32_t    code;
            int32_t     len;
        };

        constexpr hashimpl _HashTable[] = {
            { dynamic_hash, "dynamic_hash", 0x00,  0 },
            { sha1,         "sha1",         0x11, 20 },
            { sha2_256,     "sha2_256",     0x12, 32 },
        };

        constexpr int find_hashimpl_by_key(hash_t code) {
            for (auto i = 0; i < _countof(_HashTable); i++)
                if (_HashTable[i].key == code) return i;
            return 0;
        }

        constexpr int find_hashimpl_by_code(uint32_t code) {
            for (auto i = 0; i < _countof(_HashTable); i++)
                if (_HashTable[i].code == code) return i;
            return 0;
        }

        template <hash_t _Hash = dynamic_hash, int _Index = find_hashimpl_by_key(_Hash)>
        class hashcode_type
        {
        public:
            static_assert(_Hash != dynamic_hash, "dynamic_hash is not allowed here");
            static_assert(_Index > 0, "The hash_t is not implemented");

            constexpr hashcode_type(hash_t hash) { Expects(hash == _Hash); }

            constexpr hash_t      hash()  const { return _HashTable[_Index].key; }
            constexpr const char* name()  const { return _HashTable[_Index].name; }
            constexpr uint32_t    code()  const { return _HashTable[_Index].code; }
            constexpr int32_t     len()   const { return _HashTable[_Index].len; }
            constexpr int         index() const { return _Index; }
        };

        template <>
        class hashcode_type<dynamic_hash>
        {
        public:
            explicit constexpr hashcode_type(hash_t hash) : _index(find_hashimpl_by_key(hash)) { Expects(_index > 0); }

            const hash_t      hash()  const { return _HashTable[_index].key; }
            const char*       name()  const { return _HashTable[_index].name; }
            const uint32_t    code()  const { return _HashTable[_index].code; }
            const int32_t     len()   const { return _HashTable[_index].len; }
            const int         index() const { return _index; }
        private:
            const int _index;
        };

    }

    // A strongly typed byte buffer that contains an immutable representation of the output digest for a specific hash algorithm
    // known at compile-time.
    // The specialization with dynamic_hash is used when the hash function is not known at compile-time.

    template <hash_t _Hash = dynamic_hash>
    class digest_buffer
    {
    public:
        // Construct empty
        digest_buffer() : _hash(_Hash) {}
        digest_buffer(hash_t hash) : _hash(hash) {}

        // Construct by copying _Right
        //    - from bufferview_t
        digest_buffer(bufferview_t _Right) : _buffer(std::begin(_Right), std::end(_Right)), _hash(_Hash)
        {
            static_assert(_Hash != dynamic_hash, "dynamic_hash is not allowed here");
            Expects(!_Right.empty());
            Expects(_hash.len() < 0 || _Right.size() == _hash.len());
        }
        digest_buffer(hash_t hash, bufferview_t _Right) : _buffer(std::begin(_Right), std::end(_Right)), _hash(hash)
        {
            Expects(!_Right.empty());
            Expects(_hash.len() < 0 || _Right.size() == _hash.len());
        }

        //    - from digest_buffer<>
        digest_buffer(const digest_buffer<_Hash>& _Right) : _buffer(_Right._buffer), _hash(_Right.hash())
        {
            Expects(!_Right.empty());
        }

        template <hash_t _RightHash>
        digest_buffer(const digest_buffer<_RightHash>& _Right) : _buffer(_Right._buffer), _hash(_Right.hash())
        {
            Expects(!_Right.empty());
        }

        // Construct by moving _Right
        digest_buffer(buffer_t&& _Right) : _buffer(std::move(_Right)), _hash(_Hash)
        {
            static_assert(_Hash != dynamic_hash, "dynamic_hash is not allowed here");
        }
        digest_buffer(hash_t hash, buffer_t&& _Right) : _buffer(std::move(_Right)), _hash(hash) {}

        digest_buffer(digest_buffer<_Hash>&& _Right) : _buffer(std::move(_Right._buffer)), _hash(_Right.hash()) {}
        template <hash_t _RightHash>
        digest_buffer(digest_buffer<_RightHash>&& _Right) : _string(std::move(_Right._buffer)), _hash(_Right.hash()) {}

        // Assign by copying _Right
        // Note: an digest_buffer<> is immutable once initialized
        digest_buffer<_Hash>& operator =(bufferview_t _Right)
        {
            Expects(_buffer.empty() && !_Right.empty());
            _buffer = _Right;
            return *this;
        }

        digest_buffer<_Hash>& operator =(const digest_buffer<_Hash>& _Right)
        {
            Expects(_Right.hash() == hash());
            Expects(_buffer.empty() && !_Right.empty());
            _buffer = _Right._buffer;
            return *this;
        }

        template <hash_t _RightHash>
        digest_buffer<_Hash>& operator =(const digest_buffer<_RightHash>& _Right)
        {
            static_assert(_RightHash == _Hash || _RightHash == dynamic_hash || _Hash == dynamic_hash, "Mismatch between codes.");
            Expects(_Right.hash() == hash());
            Expects(_buffer.empty() && !_Right.empty());
            _buffer = _Right._buffer;
            return *this;
        }

        // Assign by moving _Right
        // Note: an digest_buffer<> is immutable once initialized and cannot be emptied
        digest_buffer<_Hash>& operator =(bufferview_t&& _Right)
        {
            Expects(_buffer.empty() && !_Right.empty());
            _buffer = std::move(_Right);
            return *this;
        }

        digest_buffer<_Hash>& operator =(digest_buffer<_Hash>&& _Right)
        {
            Expects(_Right.hash() == hash());
            Expects(_buffer.empty() && !_Right.empty());
            _buffer = std::move(_Right._buffer);
            return *this;
        }

        template <hash_t _RightHash>
        digest_buffer<_Hash>& operator =(digest_buffer<_RightHash>&& _Right)
        {
            static_assert(_RightHash == _Hash || _RightHash == dynamic_hash || _Hash == dynamic_hash, "Mismatch between codes.");
            Expects(_Right.hash() == hash());
            Expects(_buffer.empty() && !_Right.empty());
            _buffer = std::move(_Right._buffer);
            return *this;
        }

        auto hash()  const { return _hash.hash(); }
        auto data()  const { return _buffer; }
        auto code()  const { return _hash.code(); }

        auto empty() const { return _buffer.empty(); }
        auto size()  const { return _buffer.size(); }
        auto begin() const { return _buffer.begin(); }
        auto end()   const { return _buffer.end(); }

    private:
        const details::hashcode_type<_Hash> _hash;
        buffer_t _buffer;

        template <hash_t _FriendHash> friend class digest_buffer;
    };

    // operator==
    template <hash_t _HashLeft, hash_t _HashRight>
    bool operator==(const digest_buffer<_HashLeft>& _Left, const digest_buffer<_HashRight>& _Right)
    {
        return (_Left.hash() == _Right.hash()) && (_Left.data() == _Right.data());
    }

    // A string that contains an encoded string in base _Base of a digest with algo _Hash
    template <hash_t _Hash = dynamic_hash, base_t _Base = dynamic_base>
    class digest_string : public encoded_string<_Base>
    {
        using string_t = encoded_string<_Base>;

    public:
        // Construct empty
        digest_string() : string_t(), _hash(_Hash) {}
        digest_string(hash_t hash) : string_t(), _hash(hash) {}
        digest_string(base_t base) : string_t(base), _hash(_Hash) {}
        digest_string(hash_t hash, base_t base) : string_t(base), _hash(hash) {}

        // Construct by copying _Right
        //    - from string
        digest_string(stringview_t _Right) : string_t(_Right), _hash(_Hash)
        {
            static_assert(_Hash != dynamic_hash, "dynamic_hash is not allowed here");
            Expects(!_Right.empty());
            Expects(_hash.len() < 0 || decode(this->base(), _Right).size() == _hash.len());
        }
        digest_string(hash_t hash, stringview_t _Right) : string_t(_Right), _hash(hash)
        {
            Expects(!_Right.empty());
            Expects(_hash.len() < 0 || decode(this->base(), _Right).size() == _hash.len());
        }
        digest_string(base_t base, stringview_t _Right) : string_t(base, _Right), _hash(_Hash)
        {
            static_assert(_Hash != dynamic_hash, "dynamic_hash is not allowed here");
            Expects(!_Right.empty());
            Expects(_hash.len() < 0 || decode(this->base(), _Right).size() == _hash.len());
        }
        digest_string(hash_t hash, base_t base, stringview_t _Right) : string_t(base, _Right), _hash(hash)
        {
            Expects(!_Right.empty());
            Expects(_hash.len() < 0 || decode(this->base(), _Right).size() == _hash.len());
        }

        digest_string(const char* _Right) : digest_string(gsl::ensure_z(_Right)) {}
        digest_string(hash_t hash, const char* _Right) : digest_string(hash, gsl::ensure_z(_Right)) {}
        digest_string(base_t base, const char* _Right) : digest_string(base, gsl::ensure_z(_Right)) {}
        digest_string(hash_t hash, base_t base, const char* _Right) : digest_string(hash, base, gsl::ensure_z(_Right)) {}

        //    - from encoded_string<>
        digest_string(const encoded_string<_Base>& _Right) : string_t(_Right), _hash(_Hash)
        {
            static_assert(_Hash != dynamic_hash, "dynamic_hash is not allowed here");
            Expects(!_Right.empty());
            Expects(_hash.len() < 0 || decode(_Right).size() == _hash.len());
        }
        digest_string(hash_t hash, const encoded_string<_Base>& _Right) : string_t(_Right), _hash(hash)
        {
            Expects(!_Right.empty());
            Expects(_hash.len() < 0 || decode(_Right).size() == _hash.len());
        }

        template <base_t _RightBase>
        digest_string(const encoded_string<_RightBase>& _Right) : string_t(_Right), _hash(_Hash)
        {
            static_assert(_RightBase == _Base || _RightBase == dynamic_base || _Base == dynamic_base, "Mismatch between codes.");
            static_assert(_Hash != dynamic_hash, "dynamic_hash is not allowed here");
            Expects(!_Right.empty());
            Expects(_hash.len() < 0 || decode(_Right).size() == _hash.len());
        }
        template <base_t _RightBase>
        digest_string(hash_t hash, const encoded_string<_RightBase>& _Right) : string_t(_Right), _hash(hash)
        {
            static_assert(_RightBase == _Base || _RightBase == dynamic_base || _Base == dynamic_base, "Mismatch between codes.");
            Expects(!_Right.empty());
            Expects(_hash.len() < 0 || decode(_Right).size() == _hash.len());
        }

        //    - from digest_string<>
        digest_string(const digest_string<_Hash, _Base>& _Right) : string_t(_Right), _hash(_Right.hash())
        {
            Expects(!_Right.empty());
        }
        template <hash_t _RightHash>
        digest_string(const digest_string<_RightHash, _Base>& _Right) : string_t(_Right), _hash(_Right.hash())
        {
            static_assert(_RightHash == _Hash || _RightHash == dynamic_hash || _Hash == dynamic_hash, "Mismatch between codes.");
            Expects(!_Right.empty());
        }
        template <base_t _RightBase>
        digest_string(const digest_string<_Hash, _RightBase>& _Right) : string_t(_Right), _hash(_Right.hash())
        {
            static_assert(_RightBase == _Base || _RightBase == dynamic_base || _Base == dynamic_base, "Mismatch between codes.");
            Expects(!_Right.empty());
        }
        template <hash_t _RightHash, base_t _RightBase>
        digest_string(const digest_string<_RightHash, _RightBase>& _Right) : string_t(_Right), _hash(_Right.hash())
        {
            static_assert(_RightHash == _Hash || _RightHash == dynamic_hash || _Hash == dynamic_hash, "Mismatch between codes.");
            static_assert(_RightBase == _Base || _RightBase == dynamic_base || _Base == dynamic_base, "Mismatch between codes.");
            Expects(!_Right.empty());
        }

        // Assign by copying _Right
        // Note: an digest_string<> is immutable once initialized
        digest_string<_Hash, _Base>& operator =(stringview_t _Right)
        {
            Expects(_hash.len() < 0 || decode(this->base(), _Right).size() == _hash.len());
            *((string_t*)this) = _Right;
            return *this;
        }
        digest_string<_Hash, _Base>& operator =(const char* _Right)
        {
            return operator=(gsl::ensure_z(_Right));
        }

        digest_string<_Hash, _Base>& operator =(const encoded_string<_Base>& _Right)
        {
            Expects(_hash.len() < 0 || decode(_Right).size() == _hash.len());
            *((string_t*)this) = _Right;
            return *this;
        }
        template <base_t _RightBase>
        digest_string<_Hash, _Base>& operator =(const encoded_string<_RightBase>& _Right)
        {
            static_assert(_RightBase == _Base || _RightBase == dynamic_base || _Base == dynamic_base, "Mismatch between codes.");
            Expects(_hash.len() < 0 || decode(_Right).size() == _hash.len());
            *((string_t*)this) = _Right;
            return *this;
        }

        digest_string<_Hash, _Base>& operator =(const digest_string<_Hash, _Base>& _Right)
        {
            Expects(_Right.hash() == hash());
            *this = _Right.str();
            return *this;
        }
        template <hash_t _RightHash, base_t _RightBase>
        digest_string<_Hash, _Base>& operator =(const digest_string<_RightHash, _RightBase>& _Right)
        {
            static_assert(_RightHash == _Hash || _RightHash == dynamic_hash || _Hash == dynamic_hash, "Mismatch between codes.");
            static_assert(_RightBase == _Base || _RightBase == dynamic_base || _Base == dynamic_base, "Mismatch between codes.");
            Expects(_Right.hash() == hash());
            *this = _Right.str();
            return *this;
        }

        hash_t hash() const { return _hash.hash(); }

    private:
        const details::hashcode_type<_Hash> _hash;
    };

    //
    // Encode a digest into a _Base encoded_string
    //
    template <hash_t _Hash, base_t _Base>
    digest_string<_Hash, _Base> encode(const digest_buffer<_Hash>& digest)
    {
        Expects(!digest.empty());
        return encode<_Base>(digest);
    }

    template <hash_t _Hash>
    digest_string<_Hash> encode(base_t base, const digest_buffer<_Hash>& digest)
    {
        Expects(!digest.empty());
        return encode(base, digest);
    }

    inline digest_string<> encode(hash_t hash, base_t base, bufferview_t digest)
    {
        Expects(!digest.empty());
        return { hash, encode(base, digest) };
    }

    template <hash_t _Hash, base_t _Base>
    digest_buffer<_Hash> decode(digest_string<_Hash, _Base> digest)
    {
        return decode(static_cast<encoded_string<_Base>>(digest));
    }

    class multihash
    {
    public:
        multihash(bufferview_t mhview)
        {
            auto posIt = mhview.begin();

            // parse and check hash code
            auto code = uint32_t{};
            posIt = uvarint::decode(posIt, mhview.end(), &code);

            auto index = details::find_hashimpl_by_code(code);
            if (index == 0) throw std::invalid_argument("Failed to parse multihash buffer: wrong hash code");
            _hash = details::_HashTable[index].key;

            // parse and check size
            posIt = uvarint::decode(posIt, mhview.end(), &_size);

            if (static_cast<ptrdiff_t>(_size) != mhview.end() - posIt) throw std::invalid_argument("Failed to parse multihash buffer: wrong size");
            if (details::_HashTable[index].len > 0 && _size != details::_HashTable[index].len) throw std::invalid_argument("Failed to parse multihash buffer: wrong size");

            // Seems legit
            _data.assign(mhview.begin(), mhview.end());
        }

        hash_t       hash()   const { return _hash; }
        size_t       size()   const { return _size; }
        bufferview_t digest() const { return bufferview_t{ _data }.last(_size); }

        bufferview_t data()   const { return _data; }

    private:
        hash_t _hash;
        size_t _size;
        buffer_t _data;
    };

    // Create a multihash from a digest_buffer
    template <hash_t _Hash>
    multihash to_multihash(digest_buffer<_Hash> digest) {
        auto mh = buffer_t{};

        uvarint::encode(digest.code(), std::back_inserter(mh));
        uvarint::encode(digest.size(), std::back_inserter(mh));
        mh.insert(mh.end(), digest.begin(), digest.end());

        return { mh };
    }

    // Create a multihash from a digest_string
    template <hash_t _Hash, base_t _Base>
    multihash to_multihash(digest_string<_Hash, _Base> digest) { return to_multihash(decode(digest)); }

    // Create a multihash from a raw digest_string
    template <hash_t _Hash>
    multihash to_multihash(bufferview_t digest) { return to_multihash(digest_buffer<_Hash>{ digest }); }
    inline multihash to_multihash(hash_t hash, bufferview_t digest) { return to_multihash(digest_buffer<>{ hash, digest }); }
}


/* movstat/fill.c
 *
 * Copyright (C) 2018 Patrick Alken
 *
 * 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 <math.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_movstat.h>

/*
gsl_movstat_fill()
  Fill window for sample 'idx' from x using given end conditions

Inputs: endtype     - how to handle end points
        x           - input vector, length n
        idx         - index of center sample in window in [0,n-1]
        H           - number of samples left of center to include
        J           - number of samples right of center to include
        window      - (output) window of samples centered on x_{idx},
                      W_{idx}^{H,J}, length H + J + 1

Return: size of filled window (<= H + J + 1)
*/

size_t
gsl_movstat_fill(const gsl_movstat_end_t endtype, const gsl_vector * x, const size_t idx,
                 const size_t H, const size_t J, double * window)
{
  if (idx >= x->size)
    {
      GSL_ERROR_VAL ("window center index must be between 0 and n - 1", GSL_EDOM, 0);
    }
  else
    {
      const int n = (int) x->size;
      const int iidx = (int) idx;
      const int iH = (int) H;
      const int iJ = (int) J;
      int idx1, idx2, j;
      size_t window_size;

      if (endtype == GSL_MOVSTAT_END_TRUNCATE)
        {
          idx1 = GSL_MAX(iidx - iH, 0);
          idx2 = GSL_MIN(iidx + iJ, n - 1);
        }
      else
        {
          idx1 = iidx - iH;
          idx2 = iidx + iJ;
        }

      window_size = (size_t) (idx2 - idx1 + 1);

      /* fill sliding window */
      for (j = idx1; j <= idx2; ++j)
        {
          int widx = j - idx1;

          if (j < 0)
            {
              /* initial condition */
              if (endtype == GSL_MOVSTAT_END_PADZERO)
                window[widx] = 0.0;
              else if (endtype == GSL_MOVSTAT_END_PADVALUE)
                window[widx] = gsl_vector_get(x, 0);
            }
          else if (j >= n)
            {
              if (endtype == GSL_MOVSTAT_END_PADZERO)
                window[widx] = 0.0;
              else if (endtype == GSL_MOVSTAT_END_PADVALUE)
                window[widx] = gsl_vector_get(x, n - 1);
            }
          else
            {
              window[widx] = gsl_vector_get(x, j);
            }
        }

      return window_size;
    }
}


/**
 *
 * @file qwrapper_dpotrf.c
 *
 *  PLASMA core_blas quark wrapper
 *  PLASMA is a software package provided by Univ. of Tennessee,
 *  Univ. of California Berkeley and Univ. of Colorado Denver
 *
 * @version 2.6.0
 * @author Hatem Ltaief
 * @author Mathieu Faverge
 * @author Jakub Kurzak
 * @date 2010-11-15
 * @generated d Tue Jan  7 11:44:56 2014
 *
 **/
#include <lapacke.h>
#include "common.h"

/***************************************************************************//**
 *
 **/
void QUARK_CORE_dlauum(Quark *quark, Quark_Task_Flags *task_flags,
                       PLASMA_enum uplo, int n, int nb,
                       double *A, int lda)
{
    DAG_CORE_LAUUM;
    QUARK_Insert_Task(quark, CORE_dlauum_quark, task_flags,
        sizeof(PLASMA_enum),                &uplo,  VALUE,
        sizeof(int),                        &n,     VALUE,
        sizeof(double)*nb*nb,    A,             INOUT,
        sizeof(int),                        &lda,   VALUE,
        0);
}

/***************************************************************************//**
 *
 **/
#if defined(PLASMA_HAVE_WEAK)
#pragma weak CORE_dlauum_quark = PCORE_dlauum_quark
#define CORE_dlauum_quark PCORE_dlauum_quark
#endif
void CORE_dlauum_quark(Quark *quark)
{
    PLASMA_enum uplo;
    int N;
    double *A;
    int LDA;

    quark_unpack_args_4(quark, uplo, N, A, LDA);
    LAPACKE_dlauum_work(LAPACK_COL_MAJOR, lapack_const(uplo), N, A, LDA);
}


#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#include <assert.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#ifdef VRNA_WITH_GSL
#include <gsl/gsl_multimin.h>
#endif

#include "ViennaRNA/eval.h"
#include "ViennaRNA/fold_vars.h"
#include "ViennaRNA/constraints/hard.h"
#include "ViennaRNA/constraints/soft.h"
#include "ViennaRNA/fold.h"
#include "ViennaRNA/part_func.h"
#include "ViennaRNA/utils/basic.h"
#include "ViennaRNA/params/basic.h"
#include "ViennaRNA/perturbation_fold.h"

static void
calculate_probability_unpaired(vrna_fold_compound_t *vc,
                               double               *probability)
{
  int         length = vc->length;
  FLT_OR_DBL  *probs = vc->exp_matrices->probs;
  int         *iidx = vc->iindx;
  int         i, j;

  for (i = 0; i <= length; ++i)
    probability[i] = 1;

  for (i = 1; i <= length; ++i)
    for (j = i + 1; j <= length; ++j) {
      probability[i]  -= probs[iidx[i] - j];
      probability[j]  -= probs[iidx[i] - j];
    }
}

#if 0
static double
calculate_norm(double *vector,
               int    length)
{
  double  sum = 0;
  int     i;

  for (i = 1; i <= length; ++i)
    sum += vector[i] * vector[i];

  return sqrt(sum);
}

#endif

static void
addSoftConstraint(vrna_fold_compound_t  *vc,
                  const double          *epsilon,
                  int                   length)
{
  /* remove previous soft constraints */
  vrna_sc_init(vc);

  /* prepare vector of unpaired constraints in kcal/mol */
  FLT_OR_DBL *constraints = (FLT_OR_DBL *)vrna_alloc(sizeof(FLT_OR_DBL) * (length + 1));

  memcpy(constraints + 1, epsilon + 1, sizeof(FLT_OR_DBL) * length);

  /* add new soft constraints */
  vrna_sc_set_up(vc, (const FLT_OR_DBL *)constraints, VRNA_OPTION_DEFAULT);

  free(constraints);
}

static double
evaluate_objective_function_contribution(double value,
                                         int    objective_function)
{
  if (objective_function == VRNA_OBJECTIVE_FUNCTION_QUADRATIC)
    return value * value;

  if (objective_function == VRNA_OBJECTIVE_FUNCTION_ABSOLUTE)
    return fabs(value);

  assert(0);
  return 0;
}

static double
evaluate_perturbation_vector_score(vrna_fold_compound_t *vc,
                                   const double         *epsilon,
                                   const double         *q_prob_unpaired,
                                   double               sigma_squared,
                                   double               tau_squared,
                                   int                  objective_function)
{
  double  ret   = 0;
  double  ret2  = 0.;
  double  *p_prob_unpaired;
  int     i;
  int     length = vc->length;

  /* calculate pairing probabilty in the pertubated energy model */
  p_prob_unpaired = vrna_alloc(sizeof(double) * (length + 1));

  addSoftConstraint(vc, epsilon, length);

  vc->params->model_details.compute_bpp     = 1;
  vc->exp_params->model_details.compute_bpp = 1;

  /* get new (constrained) MFE to scale pf computations properly */
  double mfe = (double)vrna_mfe(vc, NULL);

  vrna_exp_params_rescale(vc, &mfe);

  vrna_pf(vc, NULL);

  calculate_probability_unpaired(vc, p_prob_unpaired);

  vrna_sc_remove(vc);

  for (i = 1; i <= length; ++i) {
    /* add penalty for pertubation energies */
    ret += evaluate_objective_function_contribution(epsilon[i], objective_function) / tau_squared;

    /* add penalty for mismatches between observed and predicted probabilities */
    if (q_prob_unpaired[i] >= 0) /* ignore positions with missing data */
      ret2 += evaluate_objective_function_contribution(p_prob_unpaired[i] - q_prob_unpaired[i],
                                                       objective_function) / sigma_squared;
  }

  vrna_message_info(stderr, "Score: pertubation: %g\tdiscrepancy: %g", ret, ret2);
  free(p_prob_unpaired);

  return ret + ret2;
}

static void
pairing_probabilities_from_restricted_pf(vrna_fold_compound_t *vc,
                                         const double         *epsilon,
                                         double               *prob_unpaired,
                                         double               **conditional_prob_unpaired)
{
  int length = vc->length;
  int i;

  addSoftConstraint(vc, epsilon, length);
  vc->params->model_details.compute_bpp     = 1;
  vc->exp_params->model_details.compute_bpp = 1;

  /* get new (constrained) MFE to scale pf computations properly */
  double mfe = (double)vrna_mfe(vc, NULL);

  vrna_exp_params_rescale(vc, &mfe);

  vrna_pf(vc, NULL);

  calculate_probability_unpaired(vc, prob_unpaired);

#ifdef _OPENMP
#pragma omp parallel for private(i)
#endif
  for (i = 1; i <= length; ++i) {
    vrna_fold_compound_t  *restricted_vc;
    char                  *hc_string;
    unsigned int          constraint_options = VRNA_CONSTRAINT_DB
                                               | VRNA_CONSTRAINT_DB_PIPE
                                               | VRNA_CONSTRAINT_DB_DOT
                                               | VRNA_CONSTRAINT_DB_X
                                               | VRNA_CONSTRAINT_DB_ANG_BRACK
                                               | VRNA_CONSTRAINT_DB_RND_BRACK;

    hc_string = vrna_alloc(sizeof(char) * (length + 1));
    memset(hc_string, '.', length);
    hc_string[i - 1] = 'x';

    restricted_vc = vrna_fold_compound(vc->sequence,
                                       &(vc->exp_params->model_details),
                                       VRNA_OPTION_DEFAULT);
    vrna_constraints_add(restricted_vc, hc_string, constraint_options);
    free(hc_string);

    vrna_exp_params_subst(restricted_vc, vc->exp_params);

    vrna_pf(restricted_vc, NULL);
    calculate_probability_unpaired(restricted_vc, conditional_prob_unpaired[i]);

    restricted_vc->sc = NULL;
    vrna_fold_compound_free(restricted_vc);
  }

  vrna_sc_remove(vc);
}

static void
pairing_probabilities_from_sampling(vrna_fold_compound_t  *vc,
                                    const double          *epsilon,
                                    int                   sample_size,
                                    double                *prob_unpaired,
                                    double                **conditional_prob_unpaired,
                                    unsigned int          options)
{
  char    **samples, **ptr;
  int     length, i, j;
  double  mfe;

  length = vc->length;
  addSoftConstraint(vc, epsilon, length);

  vc->params->model_details.compute_bpp     = 0;
  vc->exp_params->model_details.compute_bpp = 0;

  /* get new (constrained) MFE to scale pf computations properly */
  mfe = (double)vrna_mfe(vc, NULL);
  vrna_exp_params_rescale(vc, &mfe);

  vrna_pf(vc, NULL);

  samples = vrna_pbacktrack_num(vc,
                                (unsigned int)sample_size,
                                options);

  for (ptr = samples; (*ptr); ptr++) {
    for (i = length; i > 0; i--) {
      if ((*ptr)[i - 1] == '.') {
        ++prob_unpaired[i];

        for (j = length; j > 0; j--)
          if ((*ptr)[j - 1] == '.')
            ++conditional_prob_unpaired[i][j];
      }
    }

    free(*ptr);
  }

  free(samples);

  for (i = 1; i <= length; ++i) {
    if (prob_unpaired[i])
      for (j = 1; j <= length; ++j)
        conditional_prob_unpaired[i][j] /= prob_unpaired[i];

    prob_unpaired[i] /= sample_size;

    assert(prob_unpaired[i] >= 0 && prob_unpaired[i] <= 1);
  }

  vrna_sc_remove(vc);
}

static void
allocateProbabilityArrays(double  **unpaired,
                          double  ***conditional_unpaired,
                          int     length)
{
  int i;

  *unpaired             = vrna_alloc(sizeof(double) * (length + 1));
  *conditional_unpaired = vrna_alloc(sizeof(double *) * (length + 1));

  for (i = 1; i <= length; ++i)
    (*conditional_unpaired)[i] = vrna_alloc(sizeof(double) * (length + 1));
}

static void
freeProbabilityArrays(double  *unpaired,
                      double  **conditional_unpaired,
                      int     length)
{
  int i;

  free(unpaired);
  for (i = 1; i <= length; ++i)
    free(conditional_unpaired[i]);
  free(conditional_unpaired);
}

static void
evaluate_perturbation_vector_gradient(vrna_fold_compound_t  *vc,
                                      const double          *epsilon,
                                      const double          *q_prob_unpaired,
                                      double                sigma_squared,
                                      double                tau_squared,
                                      int                   objective_function,
                                      int                   sample_size,
                                      double                *gradient)
{
  double  *p_prob_unpaired;
  double  **p_conditional_prob_unpaired;
  int     i, mu;
  int     length  = vc->length;
  double  kT      = vc->exp_params->kT / 1000;

  allocateProbabilityArrays(&p_prob_unpaired, &p_conditional_prob_unpaired, length);

  if (sample_size > 0) {
    pairing_probabilities_from_sampling(vc,
                                        epsilon,
                                        sample_size,
                                        p_prob_unpaired,
                                        p_conditional_prob_unpaired,
                                        VRNA_PBACKTRACK_DEFAULT);
  } else if (sample_size < 0) {
    pairing_probabilities_from_sampling(vc,
                                        epsilon,
                                        -sample_size,
                                        p_prob_unpaired,
                                        p_conditional_prob_unpaired,
                                        VRNA_PBACKTRACK_NON_REDUNDANT);
  } else {
    pairing_probabilities_from_restricted_pf(vc,
                                             epsilon,
                                             p_prob_unpaired,
                                             p_conditional_prob_unpaired);
  }

  for (mu = 1; mu <= length; ++mu) {
    double sum = 0;

    if (objective_function == VRNA_OBJECTIVE_FUNCTION_QUADRATIC) {
      for (i = 1; i <= length; ++i) {
        if (q_prob_unpaired[i] < 0) /* ignore positions with missing data */
          continue;

        sum += (p_prob_unpaired[i] - q_prob_unpaired[i])
               * p_prob_unpaired[i] * (p_prob_unpaired[mu] - p_conditional_prob_unpaired[i][mu])
               / sigma_squared;
      }

      gradient[mu] = 2 * (epsilon[mu] / tau_squared + sum / kT);
    } else if (objective_function == VRNA_OBJECTIVE_FUNCTION_ABSOLUTE) {
      for (i = 1; i <= length; ++i)
        if (q_prob_unpaired[i] >= 0 && p_prob_unpaired[i] != q_prob_unpaired[i]) {
          sum += (p_prob_unpaired[i] * (p_prob_unpaired[mu] - p_conditional_prob_unpaired[i][mu])) /
                 kT
                 / sigma_squared
                 * (p_prob_unpaired[i] > q_prob_unpaired[i] ? 1. : -1.);
        }

      if (epsilon[mu])
        sum += (epsilon[mu] > 0 ? 1. : -1.) / tau_squared;

      gradient[mu] = sum;
    }
  }

  freeProbabilityArrays(p_prob_unpaired, p_conditional_prob_unpaired, length);
}

#ifdef VRNA_WITH_GSL
typedef struct parameters_gsl {
  vrna_fold_compound_t  *vc;
  const double          *q_prob_unpaired;
  double                sigma_squared;
  double                tau_squared;
  int                   objective_function;
  int                   sample_size;
} parameters_gsl;

static double
f_gsl(const gsl_vector  *x,
      void              *params)
{
  parameters_gsl *p = params;

  return evaluate_perturbation_vector_score(p->vc,
                                            x->data,
                                            p->q_prob_unpaired,
                                            p->sigma_squared,
                                            p->tau_squared,
                                            p->objective_function);
}

static void
df_gsl(const gsl_vector *x,
       void             *params,
       gsl_vector       *df)
{
  parameters_gsl *p = params;

  gsl_vector_set(df, 0, 0);
  evaluate_perturbation_vector_gradient(p->vc,
                                        x->data,
                                        p->q_prob_unpaired,
                                        p->sigma_squared,
                                        p->tau_squared,
                                        p->objective_function,
                                        p->sample_size,
                                        df->data);
}

static void
fdf_gsl(const gsl_vector  *x,
        void              *params,
        double            *f,
        gsl_vector        *g)
{
  *f = f_gsl(x, params);
  df_gsl(x, params, g);
}

#endif /* VRNA_WITH_GSL */

PUBLIC void
vrna_sc_minimize_pertubation(vrna_fold_compound_t *vc,
                             const double         *q_prob_unpaired,
                             int                  objective_function,
                             double               sigma_squared,
                             double               tau_squared,
                             int                  algorithm,
                             int                  sample_size,
                             double               *epsilon,
                             double               initialStepSize,
                             double               minStepSize,
                             double               minImprovement,
                             double               minimizerTolerance,
                             progress_callback    callback)
{
  int                                   iteration_count = 0;
  const int                             max_iterations  = 100;
  int                                   length          = vc->length;

#ifdef VRNA_WITH_GSL
  const gsl_multimin_fdfminimizer_type  *minimizer_type = 0;

  struct {
    int                                   type;
    const gsl_multimin_fdfminimizer_type  *gsl_type;
  } algorithms[] =
  { { VRNA_MINIMIZER_CONJUGATE_FR,
      gsl_multimin_fdfminimizer_conjugate_fr },
    { VRNA_MINIMIZER_CONJUGATE_PR,
      gsl_multimin_fdfminimizer_conjugate_pr },
    { VRNA_MINIMIZER_VECTOR_BFGS,
      gsl_multimin_fdfminimizer_vector_bfgs },
    { VRNA_MINIMIZER_VECTOR_BFGS2,
      gsl_multimin_fdfminimizer_vector_bfgs2 },
    { VRNA_MINIMIZER_STEEPEST_DESCENT,
      gsl_multimin_fdfminimizer_steepest_descent },
    { 0,
      NULL } };
  int i;
  for (i = 0; algorithms[i].type; ++i)
    if (algorithms[i].type == algorithm) {
      minimizer_type = algorithms[i].gsl_type;
      break;
    }

  if (minimizer_type) {
    parameters_gsl            parameters;
    gsl_multimin_function_fdf fdf;
    gsl_multimin_fdfminimizer *minimizer;
    gsl_vector                *vector;

    int                       status;

    parameters.vc                 = vc;
    parameters.q_prob_unpaired    = q_prob_unpaired;
    parameters.sigma_squared      = sigma_squared;
    parameters.tau_squared        = tau_squared;
    parameters.objective_function = objective_function;
    parameters.sample_size        = sample_size;

    fdf.n       = length + 1;
    fdf.f       = &f_gsl;
    fdf.df      = &df_gsl;
    fdf.fdf     = &fdf_gsl;
    fdf.params  = (void *)&parameters;

    minimizer = gsl_multimin_fdfminimizer_alloc(minimizer_type, length + 1);
    vector    = gsl_vector_calloc(length + 1);

    /* gsl_multimin_fdfminimizer_set(minimizer, &fdf, vector, 0.01, 1e-4); */
    gsl_multimin_fdfminimizer_set(minimizer, &fdf, vector, initialStepSize, minimizerTolerance);

    if (callback)
      callback(0, minimizer->f, minimizer->x->data);

    do {
      ++iteration_count;
      status = gsl_multimin_fdfminimizer_iterate(minimizer);

      if (callback)
        callback(iteration_count, minimizer->f, minimizer->x->data);

      if (status)
        break;

      status = gsl_multimin_test_gradient(minimizer->gradient, minimizerTolerance);
    } while (status == GSL_CONTINUE && iteration_count < max_iterations);

    memcpy(epsilon, minimizer->x->data, sizeof(double) * (length + 1));

    gsl_multimin_fdfminimizer_free(minimizer);
    gsl_vector_free(vector);

    return;
  }

#endif /* VRNA_WITH_GSL */

  double        improvement;
  const double  min_improvement = minImprovement;

  double        *new_epsilon  = vrna_alloc(sizeof(double) * (length + 1));
  double        *gradient     = vrna_alloc(sizeof(double) * (length + 1));

  double        score = evaluate_perturbation_vector_score(vc,
                                                           epsilon,
                                                           q_prob_unpaired,
                                                           sigma_squared,
                                                           tau_squared,
                                                           objective_function);

  if (callback)
    callback(0, score, epsilon);

  do {
    double  new_score;
    double  step_size;

    ++iteration_count;

    evaluate_perturbation_vector_gradient(vc,
                                          epsilon,
                                          q_prob_unpaired,
                                          sigma_squared,
                                          tau_squared,
                                          objective_function,
                                          sample_size,
                                          gradient);

    /*    step_size = 0.5 / calculate_norm(gradient, length);*/
    step_size = initialStepSize;

    do {
      int i;
      for (i = 1; i <= length; ++i)
        new_epsilon[i] = epsilon[i] - step_size * gradient[i];

      new_score = evaluate_perturbation_vector_score(vc,
                                                     new_epsilon,
                                                     q_prob_unpaired,
                                                     sigma_squared,
                                                     tau_squared,
                                                     objective_function);
      improvement = 1 - new_score / score;
      step_size   /= 2;
    } while ((improvement < min_improvement) && (step_size >= minStepSize));

    if (new_score > score)
      break;

    if (callback)
      callback(iteration_count, new_score, new_epsilon);

    score = new_score;
    memcpy(epsilon, new_epsilon, sizeof(double) * (length + 1));
  } while (improvement >= min_improvement && iteration_count < max_iterations);

  free(gradient);
  free(new_epsilon);
}


#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>

#include <gsl/gsl_integration.h>
#include <gsl/gsl_interp.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_errno.h>

#include "ccl.h"

/*
 * Spline the linear power spectrum for mu-Sigma MG cosmologies.
 * @param cosmo Cosmological parameters
 ^ @param psp The linear power spectrum to spline.
 * @param status, integer indicating the status
 */
void ccl_cosmology_spline_linpower_musigma(ccl_cosmology* cosmo, ccl_f2d_t *psp, int rescaled_mg_flag, int* status) {
  double kmin, kmax, ndecades, amin, amax, ic, sigma8, log_sigma8;
  int nk, na, s;
  double *lk = NULL, *aa = NULL, *lpk_ln = NULL, *lpk_nl = NULL;
  double norm_pk;
  double *mnu_list = NULL;
  ccl_parameters params_GR;
  params_GR.m_nu = NULL;
  params_GR.z_mgrowth = NULL;
  params_GR.df_mgrowth = NULL;
  ccl_cosmology * cosmo_GR = NULL;
  double *D_mu = NULL;
  double *D_GR = NULL;

  if (*status == 0) {
    //calculations done - now allocate CCL splines
    kmin = 2*exp(psp->lkmin);
    kmax = cosmo->spline_params.K_MAX_SPLINE;
    //Compute nk from number of decades and N_K = # k per decade
    ndecades = log10(kmax) - log10(kmin);
    nk = (int)ceil(ndecades*cosmo->spline_params.N_K);
    amin = cosmo->spline_params.A_SPLINE_MINLOG_PK;
    amax = cosmo->spline_params.A_SPLINE_MAX;
    na = cosmo->spline_params.A_SPLINE_NA_PK+cosmo->spline_params.A_SPLINE_NLOG_PK-1;

    // The lk array is initially k, but will later
    // be overwritten with log(k)
    lk = ccl_log_spacing(kmin, kmax, nk);
    if (lk == NULL) {
      *status = CCL_ERROR_MEMORY;
      ccl_cosmology_set_status_message(
          cosmo,
          "ccl_power.c: ccl_cosmology_spline_linpower_musigma(): memory allocation\n");
    }
  }

  if (*status == 0) {
    aa = ccl_linlog_spacing(
        amin, cosmo->spline_params.A_SPLINE_MIN_PK,
        amax, cosmo->spline_params.A_SPLINE_NLOG_PK,
        cosmo->spline_params.A_SPLINE_NA_PK);
    if (aa == NULL) {
      *status = CCL_ERROR_MEMORY;
      ccl_cosmology_set_status_message(
        cosmo,
        "ccl_power.c: ccl_cosmology_spline_linpower_musigma(): memory allocation\n");
    }
  }

  if (*status == 0) {
    lpk_ln = malloc(nk * na * sizeof(double));
    if (lpk_ln == NULL) {
      *status = CCL_ERROR_MEMORY;
      ccl_cosmology_set_status_message(
        cosmo,
        "ccl_power.c: ccl_cosmology_spline_linpower_musigma(): memory allocation\n");
    }
  }

  if (*status == 0) {
    lpk_nl = malloc(nk * na * sizeof(double));
    if(lpk_nl == NULL) {
      *status = CCL_ERROR_MEMORY;
      ccl_cosmology_set_status_message(
        cosmo,
        "ccl_power.c: ccl_cosmology_spline_linpower_musigma(): memory allocation\n");
    }
  }

  if (*status == 0) {
    // After this loop lk will contain log(k),
    // lpk_ln will contain log(P_lin), all in Mpc, not Mpc/h units!
    double psout_l;
    s = 0;

    // If scale-independent mu / Sigma modified gravity is in use
    // and mu ! = 0 : get the unnormalized growth factor in MG and for
    // corresponding GR case, to rescale CLASS power spectrum
    if (fabs(cosmo->params.mu_0) > 1e-14) {
      // Set up another cosmology which is exactly the same as the
      // current one but with mu_0 and Sigma_0=0, for scaling P(k)

      // Get a list of the three neutrino masses already calculated
      mnu_list = malloc(3*sizeof(double));
      if (mnu_list == NULL) {
        *status = CCL_ERROR_MEMORY;
        ccl_cosmology_set_status_message(
          cosmo,
          "ccl_power.c: ccl_cosmology_spline_linpower_musigma(): memory allocation\n");
      }

      if (*status == 0) {
        for (int i=0; i< cosmo->params.N_nu_mass; i=i+1) {
          mnu_list[i] = cosmo->params.m_nu[i];
        }
        if (cosmo->params.N_nu_mass < 3) {
          for (int j=cosmo->params.N_nu_mass; j<3; j=j+1) {
            mnu_list[j] = 0.;
          }
        }

        if (isfinite(cosmo->params.A_s)) {
          norm_pk = cosmo->params.A_s;
        }
        else if (isfinite(cosmo->params.sigma8)) {
          norm_pk = cosmo->params.sigma8;
        }
        else {
          *status = CCL_ERROR_PARAMETERS;
          strcpy(
            cosmo->status_message,
            "ccl_power.c: ccl_cosmology_spline_linpower_musigma(): neither A_s nor sigma8 defined.\n");
        }
      }

      if (*status == 0) {
        params_GR = ccl_parameters_create(
          cosmo->params.Omega_c, cosmo->params.Omega_b, cosmo->params.Omega_k,
          cosmo->params.Neff, mnu_list, cosmo->params.N_nu_mass,
          cosmo->params.w0, cosmo->params.wa, cosmo->params.h,
          norm_pk, cosmo->params.n_s,
          cosmo->params.bcm_log10Mc, cosmo->params.bcm_etab,
          cosmo->params.bcm_ks, 0., 0., cosmo->params.nz_mgrowth,
          cosmo->params.z_mgrowth, cosmo->params.df_mgrowth, status);

        if (*status) {
          *status = CCL_ERROR_PARAMETERS;
          strcpy(
            cosmo->status_message,
            "ccl_power.c: ccl_cosmology_spline_linpower_musigma(): could not make MG params.\n");
        }
      }

      if (*status == 0) {
        cosmo_GR = ccl_cosmology_create(params_GR, cosmo->config);
        D_mu = malloc(na * sizeof(double));
        D_GR = malloc(na * sizeof(double));

        if (cosmo_GR == NULL || D_mu == NULL || D_GR == NULL) {
          *status = CCL_ERROR_MEMORY;
          ccl_cosmology_set_status_message(
            cosmo,
            "ccl_power.c: ccl_cosmology_spline_linpower_musigma(): memory allocation\n");
        }
      }

      if (*status == 0) {
        ccl_cosmology_compute_growth(cosmo_GR, status);

        if (*status) {
          *status = CCL_ERROR_PARAMETERS;
          strcpy(
            cosmo->status_message,
            "ccl_power.c: ccl_cosmology_spline_linpower_musigma(): could not init GR growth.\n");
        }
      }

      if (*status == 0) {
        for (int i=0; i<na; i++) {
          D_mu[i] = ccl_growth_factor_unnorm(cosmo, aa[i], status);
          D_GR[i] = ccl_growth_factor_unnorm(cosmo_GR, aa[i], status);
        }

        if (*status) {
          *status = CCL_ERROR_PARAMETERS;
          strcpy(
            cosmo->status_message,
            "ccl_power.c: ccl_cosmology_spline_linpower_musigma(): could not make MG and GR growth.\n");
        }
      }

      if (*status == 0) {
        for (int i=0; i<nk; i++) {
          lk[i] = log(lk[i]);
          for (int j = 0; j < na; j++) {
            //The 2D interpolation routines access the function values pk_{k_ia_j} with the following ordering:
            //pk_ij = pk[j*N_k + i]
            //with i = 0,...,N_k-1 and j = 0,...,N_a-1.
            psout_l = ccl_f2d_t_eval(psp, lk[i], aa[j], cosmo, status);
	    if (rescaled_mg_flag == 0) {
            lpk_ln[j*nk+i] = log(psout_l) ;
	    }
	    else {
            lpk_ln[j*nk+i] = log(psout_l) + 2 * log(D_mu[j]) - 2 * log(D_GR[j]);
	    }

          }
        }
      }
    }
    else {
      // This is the normal GR case.
      for (int i=0; i<nk; i++) {
        lk[i] = log(lk[i]);
        for (int j = 0; j<na; j++) {
          //The 2D interpolation routines access the function values pk_{k_ia_j} with the following ordering:
          //pk_ij = pk[j*N_k + i]
          //with i = 0,...,N_k-1 and j = 0,...,N_a-1.
          psout_l = ccl_f2d_t_eval(psp, lk[i], aa[j], cosmo, status);
          lpk_ln[j*nk+i] = log(psout_l);
        }
      }
    }
  }

  if (*status == 0) {
    cosmo->data.p_lin = ccl_f2d_t_new(
      na, aa, nk, lk, lpk_ln, NULL, NULL, 0,
      1, 2, ccl_f2d_cclgrowth, 1, NULL, 0, 2,
      ccl_f2d_3,status);
  }

  // if desried, renomalize to a given sigma8
  if (isfinite(cosmo->params.sigma8) && (!isfinite(cosmo->params.A_s))) {
    if (*status == 0) {
      cosmo->computed_linear_power = true;
      sigma8 = ccl_sigma8(cosmo, status);
      cosmo->computed_linear_power = false;
    }

    if (*status == 0) {
      // Calculate normalization factor using computed value of sigma8, then
      // recompute P(k, a) using this normalization
      log_sigma8 = 2*(log(cosmo->params.sigma8) - log(sigma8));
      for(int j = 0; j<na*nk; j++)
        lpk_ln[j] += log_sigma8;
    }

    if (*status == 0) {
      // Free the previous P(k,a) spline, and allocate a new one to store the
      // properly-normalized P(k,a)
      ccl_f2d_t_free(cosmo->data.p_lin);
      cosmo->data.p_lin = ccl_f2d_t_new(
        na, aa, nk, lk, lpk_ln, NULL, NULL, 0,
        1, 2, ccl_f2d_cclgrowth, 1, NULL, 0, 2,
        ccl_f2d_3,status);
    }
  }

  free(D_mu);
  free(D_GR);
  free(mnu_list);
  ccl_parameters_free(&params_GR);
  ccl_cosmology_free(cosmo_GR);
  free(lk);
  free(aa);
  free(lpk_nl);
  free(lpk_ln);
}


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

#ifndef SIM_PHYSICS_WIND_H_
#define SIM_PHYSICS_WIND_H_

#include <gsl/gsl_vector.h>

#include <string>

#include "common/c_math/mat3.h"
#include "common/c_math/vec3.h"
#include "common/macros.h"

// Functions related to wind shear and turbulence.
//
// Many of these functions define a "mean wind frame" denoted by
// the "mw" subscript.  This should not be confused with the
// wind frame defined in sim/physics/aero_frame.h, which related to
// the apparent wind at a body.
//
// The origin of the mean wind frame lies at ground level directly
// below the ground station origin.  The axes are defined by:
//   x: Points along the predominate wind vector.
//   y: Completes a right-handed frame.
//   z: Points up.
// These are chosen to be consistent with NREL's TurbSim software
// and various IEC standards.

namespace sim {

namespace physics {

namespace wind {

double CalcWindShear(double height_agl, double ref_height_agl,
                     double ref_wind_speed, double wind_shear_exponent);

class FrozenTurbulenceWindDatabase {
  friend class FrozenTurbulenceWindDatabaseTest;

 public:
  FrozenTurbulenceWindDatabase(double t0, double y0,
                               const std::string &filename);
  ~FrozenTurbulenceWindDatabase() {
    gsl_vector_free(u_);
    gsl_vector_free(v_);
    gsl_vector_free(w_);
  }
  void CalcWind(double t, const Vec3 &pos_mw, Vec3 *wind_mw) const;

 private:
  // Time offset [s] to apply.  At t = 0, positions whose x coordinate
  // (in the mw frame) is zero will fetch the database entry given at
  // time t0_.
  double t0_;
  // Y offset [m] to apply from the center of the wind database.
  double y0_;
  // Mean wind speed [m/s] that sets the rate at which the database
  // is "advected" along the mean wind coordinate x axis.
  double mean_wind_speed_;
  // Number [#] of t, y, and z grid points.
  int32_t num_t_, num_y_, num_z_;
  // Duration [s], width [m], and height [m] of the grid defining
  // the database.
  double duration_, width_, height_;
  // Vectors storing the components of the wind velocity [m/s].  Each
  // vector has num_t * num_y * num_z elements.  The u, v, and w
  // components point along the x, y, and z axes of the mean wind
  // coordinate system.
  gsl_vector *u_;
  gsl_vector *v_;
  gsl_vector *w_;

  DISALLOW_COPY_AND_ASSIGN(FrozenTurbulenceWindDatabase);
};

enum class IecTurbulenceCategory { kA, kB, kC };

enum class IecWindTurbineClass { kI, kII, kIII };

class IecWindModel {
 public:
  IecWindModel(double hub_height_agl__, double rotor_diameter,
               double hub_wind_speed, IecTurbulenceCategory turbulence_category,
               IecWindTurbineClass wind_turbine_class);

  double hub_height_agl() const { return hub_height_agl_; }
  double ntm_sigma_1() const { return ntm_sigma_1_; }
  double ewm_sigma_1() const { return ewm_sigma_1_; }
  double etm_sigma_1() const { return etm_sigma_1_; }

  // IEC 61400-1 Design situations.

  // IEC 61400-1 Sec. 6.3.1.2 -- Normal Wind Profile Model (NWP).
  double CalcNormalWindProfile(double height_agl) const;

  // IEC 61400-1 Sec. 6.3.2.1 -- Extreme wind speed model (EWM).
  double CalcExtremeWindSpeed1YearRecurrence(double height_agl,
                                             bool is_turbulent) const;
  double CalcExtremeWindSpeed50YearRecurrence(double height_agl,
                                              bool is_turbulent) const;

  // IEC 61400-1 Sec. 6.3.2.2 -- Extreme Operating Gust (EOG).
  double CalcExtremeOperatingGust(double t, double height_agl) const;

  // IEC 61400-1 Sec. 6.3.2.4 -- Extreme direction change (EDC).
  double CalcExtremeDirectionChange(double t) const;

  // IEC 61400-1 Sec. 6.3.2.5 -- Extreme coherent gust with direction
  // change (ECD).
  double CalcExtremeCoherentGustWithDirectionChange(
      double t, double height_agl, double *wind_direction) const;

  // IEC 61400-1 Sec. 6.3.2.6 -- Extreme wind shear (EWS).
  double CalcExtremeWindShearVertical(double t, double height_agl) const;
  double CalcExtremeWindShearHorizontal(double t, double height_agl,
                                        double y) const;

 private:
  // Hub height above ground level [m].
  const double hub_height_agl_;

  // Rotor diameter [m].
  const double rotor_diameter_;

  // Nominal wind speed [m/s] at hub height.
  const double hub_wind_speed_;

  // Reference wind speed [m/s], see IEC 61400-1 Sec. 6.2.
  const double v_ref_;

  // Expected value of the turbulence intensity [#] at 15 m/s.
  const double I_ref_;

  // Longitudinal turbulence scale parameter [#].
  const double lambda_1_;

  // Normal turbulence model (NTM) standard deviation [m/s].
  const double ntm_sigma_1_;

  // Extreme wind model (EWM) standard deviation [m/s].
  const double ewm_sigma_1_;

  // Extreme turbulence model (ETM) standard deviation [m/s].
  const double etm_sigma_1_;
};

}  // namespace wind

}  // namespace physics

}  // namespace sim

#endif  // SIM_PHYSICS_WIND_H_


#include <pygsl/solver.h>
#include <gsl/gsl_multiroots.h>

static PyObject * module = NULL;

/* accessor methods */
#define _SOLVER_METH(solver, name) \
static PyObject*  \
PyGSL_multiroot_ ## solver ## _ ## name(PyGSL_solver *self, PyObject *args) \
{ return PyGSL_solver_ret_vec(self, args, (ret_vec)gsl_multiroot_## solver ## _ ## name); }

#define FSOLVER(name)   _SOLVER_METH(fsolver, name)
#define FDFSOLVER(name) _SOLVER_METH(fdfsolver, name)
#define BSOLVER_METH(name) _SOLVER_METH(fsolver, name) _SOLVER_METH(fdfsolver, name)
BSOLVER_METH(root)
BSOLVER_METH(f)
BSOLVER_METH(dx)

static PyObject*
PyGSL_multiroot_fsolver_test_residual(PyGSL_solver *self, PyObject *args)
{
     int flag;
     double epsabs;
     gsl_multiroot_fsolver *s = self->solver;
     if(!PyArg_ParseTuple(args, "d", &epsabs))
	  return NULL;
     flag = gsl_multiroot_test_residual(s->f, epsabs);
     return PyGSL_ERROR_FLAG_TO_PYINT(flag);
}
static PyObject*
PyGSL_multiroot_fdfsolver_test_residual(PyGSL_solver *self, PyObject *args)
{
     int flag;
     double epsabs;
     gsl_multiroot_fdfsolver *s = self->solver;
     if(!PyArg_ParseTuple(args, "d", &epsabs))
	  return NULL;
     flag = gsl_multiroot_test_residual(s->f, epsabs);
     return PyGSL_ERROR_FLAG_TO_PYINT(flag);
}

static PyObject*
PyGSL_multiroot_fsolver_test_delta(PyGSL_solver *self, PyObject *args)
{
     int flag;
     double epsabs, epsrel;
     gsl_multiroot_fsolver *s = self->solver;
     if(!PyArg_ParseTuple(args, "dd", &epsabs, &epsrel))
	  return NULL;
     flag = gsl_multiroot_test_delta(s->dx, s->x, epsabs, epsrel);
     return PyGSL_ERROR_FLAG_TO_PYINT(flag);
}
static PyObject*
PyGSL_multiroot_fdfsolver_test_delta(PyGSL_solver *self, PyObject *args)
{
     int flag;
     double epsabs, epsrel;
     gsl_multiroot_fdfsolver *s = self->solver;
     if(!PyArg_ParseTuple(args, "dd", &epsabs, &epsrel))
	  return NULL;
     flag = gsl_multiroot_test_delta(s->dx, s->x, epsabs, epsrel);
     return PyGSL_ERROR_FLAG_TO_PYINT(flag);
}
int
PyGSL_multiroot_function_wrap(const gsl_vector *x, void *params, gsl_vector *f)
{
     int flag;
     PyGSL_solver *p = (PyGSL_solver *) params;
     FUNC_MESS_BEGIN();
     if(PyGSL_DEBUG_LEVEL() > 2){
       gsl_vector_fprintf(stderr, x, "x = %d");
     }
     flag =  PyGSL_function_wrap_Op_On(x, f, p->cbs[0], p->args, x->size,
				       x->size, __FUNCTION__);
     FUNC_MESS_END();
     return flag;
}

int
PyGSL_multiroot_function_wrap_df(const gsl_vector *x, void *params, gsl_matrix *J)
{
     int flag;
     PyGSL_solver *p = (PyGSL_solver *) params;
     FUNC_MESS_BEGIN();
     flag =  PyGSL_function_wrap_Op_Opn(x, J, p->cbs[1], p->args, x->size, x->size,
					__FUNCTION__);
     FUNC_MESS_END();
     return flag;
}

int
PyGSL_multiroot_function_wrap_fdf(const gsl_vector *x, void *params, gsl_vector *f, gsl_matrix *J)
{
     int flag;
     PyGSL_solver *p = (PyGSL_solver *) params;
     FUNC_MESS_BEGIN();
     flag = PyGSL_function_wrap_Op_On_Opn(x, f, J, p->cbs[2], p->args, x->size, x->size,
					  __FUNCTION__);
     FUNC_MESS_END();
     return flag;
}

PyObject *
PyGSL_multiroot_fsolver_set(PyGSL_solver *self, PyObject *pyargs, PyObject *kw)
{

     gsl_multiroot_function * c_sys;
     struct pygsl_solver_n_set info = {0, NULL, (set_m_t) gsl_multiroot_fsolver_set};
     PyObject * tmp;

     FUNC_MESS_BEGIN();
     if(self->c_sys == NULL){
	  if((c_sys = calloc(1, sizeof(gsl_multiroot_function))) == NULL){
	       PyGSL_ERROR_NULL("Could not allocate the memory for the c_sys", GSL_ENOMEM);
	  }
	  c_sys->n =self->problem_dimensions[0];
	  c_sys->f =PyGSL_multiroot_function_wrap;
	  c_sys->params=(void*)self;
	  info.c_sys = c_sys;
     }else{
	  info.c_sys = self->c_sys;
     }
     tmp =  PyGSL_solver_n_set(self, pyargs, kw, &info);
     FUNC_MESS_END();
     return tmp;
}

PyObject *
PyGSL_multiroot_fdfsolver_set(PyGSL_solver *self, PyObject *pyargs, PyObject *kw)
{

     gsl_multiroot_function_fdf * c_sys;
     struct pygsl_solver_n_set info = {1, NULL, (set_m_t)gsl_multiroot_fdfsolver_set};
     PyObject * tmp;

     FUNC_MESS_BEGIN();
     if(self->c_sys == NULL){
	  if((c_sys = calloc(1, sizeof(gsl_multiroot_function_fdf))) == NULL){
	       PyGSL_ERROR_NULL("Could not allocate the memory for the c_sys", GSL_ENOMEM);
	  }
	  c_sys->n=self->problem_dimensions[0];
	  c_sys->f  = PyGSL_multiroot_function_wrap;
	  c_sys->df = PyGSL_multiroot_function_wrap_df;
	  c_sys->fdf = PyGSL_multiroot_function_wrap_fdf;
	  c_sys->params=(void*)self;
	  info.c_sys = c_sys;
     }else{
	  info.c_sys = self->c_sys;
     }
     tmp = PyGSL_solver_n_set(self, pyargs, kw, &info);
     FUNC_MESS_END();
     return tmp;
}

static PyMethodDef PyGSL_multiroot_fmethods[] = {
     {"dx",   (PyCFunction)PyGSL_multiroot_fsolver_dx,   METH_NOARGS, NULL},
     {"f",    (PyCFunction)PyGSL_multiroot_fsolver_f,    METH_NOARGS, NULL},
     {"root", (PyCFunction)PyGSL_multiroot_fsolver_root, METH_NOARGS, NULL},
     {"set",  (PyCFunction)PyGSL_multiroot_fsolver_set,  METH_VARARGS|METH_KEYWORDS, NULL},
     {"test_delta",    (PyCFunction)PyGSL_multiroot_fsolver_test_delta, METH_VARARGS, NULL},
     {"test_residual", (PyCFunction)PyGSL_multiroot_fsolver_test_residual, METH_VARARGS, NULL},
     {NULL, NULL, 0, NULL}
};

static PyMethodDef PyGSL_multiroot_fdfmethods[] = {
     {"dx",            (PyCFunction)PyGSL_multiroot_fdfsolver_dx,   METH_NOARGS, NULL},
     {"f",             (PyCFunction)PyGSL_multiroot_fdfsolver_f,    METH_NOARGS, NULL},
     {"root",          (PyCFunction)PyGSL_multiroot_fdfsolver_root, METH_NOARGS, NULL},
     {"set",           (PyCFunction)PyGSL_multiroot_fdfsolver_set,  METH_VARARGS|METH_KEYWORDS, NULL},
     {"test_delta",    (PyCFunction)PyGSL_multiroot_fdfsolver_test_delta, METH_VARARGS, NULL},
     {"test_residual", (PyCFunction)PyGSL_multiroot_fdfsolver_test_residual, METH_VARARGS, NULL},
     {NULL, NULL, 0, NULL}
};

static const char multiroot_f_type_name[] = "F-MultiRootSolver";
static const char multiroot_fdf_type_name[] = "FdF-MultiRootSolver";

const struct _SolverStatic
multiroot_solver_f   = {{(void_m_t) gsl_multiroot_fsolver_free,
			/* gsl_multiroot_fsolver_restart */  (void_m_t) NULL,
			(name_m_t) gsl_multiroot_fsolver_name,
			 (int_m_t) gsl_multiroot_fsolver_iterate},
			1, PyGSL_multiroot_fmethods, multiroot_f_type_name},
multiroot_solver_fdf = {{(void_m_t) gsl_multiroot_fdfsolver_free,
			 /* gsl_multiroot_fdfsolver_restart (void_m_t) */ NULL,
			 (name_m_t) gsl_multiroot_fdfsolver_name,
			 (int_m_t)  gsl_multiroot_fdfsolver_iterate},
			3, PyGSL_multiroot_fdfmethods, multiroot_fdf_type_name};

static PyObject*
PyGSL_multiroot_f_init(PyObject *self, PyObject *args,
		      const gsl_multiroot_fsolver_type * type)
{

     PyObject *tmp=NULL;
     solver_alloc_struct s = {type, (void_an_t) gsl_multiroot_fsolver_alloc,
			      &multiroot_solver_f};
     FUNC_MESS_BEGIN();
     tmp = PyGSL_solver_dn_init(self, args, &s, 1);
     FUNC_MESS_END();
     return tmp;
}

static PyObject*
PyGSL_multiroot_fdf_init(PyObject *self, PyObject *args,
		      const gsl_multiroot_fdfsolver_type * type)
{

     PyObject *tmp=NULL;
     solver_alloc_struct s = {type, (void_an_t) gsl_multiroot_fdfsolver_alloc,
			      &multiroot_solver_fdf};
     FUNC_MESS_BEGIN();
     tmp = PyGSL_solver_dn_init(self, args, &s, 1);
     FUNC_MESS_END();
     return tmp;
}

#define AMROOTS_F(name)                                                  \
static PyObject* PyGSL_multiroot_init_ ## name (PyObject *self, PyObject *args)\
{                                                                             \
     PyObject *tmp = NULL;                                                    \
     FUNC_MESS_BEGIN();                                                       \
     tmp = PyGSL_multiroot_f_init(self, args,  gsl_multiroot_fsolver_ ## name); \
     if (tmp == NULL){                                                        \
	  PyGSL_add_traceback(module, __FILE__, __FUNCTION__, __LINE__); \
     }                                                                        \
     FUNC_MESS_END();                                                         \
     return tmp;                                                              \
}

#define AMROOTS_FDF(name)                                                  \
static PyObject* PyGSL_multiroot_init_ ## name (PyObject *self, PyObject *args)\
{                                                                             \
     PyObject *tmp = NULL;                                                    \
     FUNC_MESS_BEGIN();                                                       \
     tmp = PyGSL_multiroot_fdf_init(self, args,  gsl_multiroot_fdfsolver_ ## name); \
     if (tmp == NULL){                                                        \
	  PyGSL_add_traceback(module, __FILE__, __FUNCTION__, __LINE__); \
     }                                                                        \
     FUNC_MESS_END();                                                         \
     return tmp;                                                              \
}

AMROOTS_F(dnewton)
AMROOTS_F(broyden)
AMROOTS_F(hybrid)
AMROOTS_F(hybrids)

AMROOTS_FDF(newton)
AMROOTS_FDF(gnewton)
AMROOTS_FDF(hybridj)
AMROOTS_FDF(hybridsj)

static PyObject *
PyGSL_multiroot_test_delta(PyObject * self, PyObject * args)
{
     return PyGSL_solver_vvdd_i(self, args, gsl_multiroot_test_delta);
}

static PyObject *
PyGSL_multiroot_test_residual(PyObject * self, PyObject * args)
{
     return PyGSL_solver_vd_i(self, args, gsl_multiroot_test_residual);
}

static PyMethodDef mMethods[] = {
     /* solvers */
     {"dnewton" ,      PyGSL_multiroot_init_dnewton,  METH_VARARGS, NULL},
     {"broyden" ,      PyGSL_multiroot_init_broyden,  METH_VARARGS, NULL},
     {"hybrid"  ,      PyGSL_multiroot_init_hybrid ,  METH_VARARGS, NULL},
     {"hybrids" ,      PyGSL_multiroot_init_hybrids,  METH_VARARGS, NULL},
     {"newton"  ,      PyGSL_multiroot_init_newton ,  METH_VARARGS, NULL},
     {"gnewton" ,      PyGSL_multiroot_init_gnewton,  METH_VARARGS, NULL},
     {"hybridj" ,      PyGSL_multiroot_init_hybridj,  METH_VARARGS, NULL},
     {"hybridsj",      PyGSL_multiroot_init_hybridsj, METH_VARARGS, NULL},
     /* mutliroot funcs */
     {"test_delta",    PyGSL_multiroot_test_delta,     METH_VARARGS, NULL},
     {"test_residual", PyGSL_multiroot_test_residual,  METH_VARARGS, NULL},
     {NULL, NULL, 0, NULL}
};

static const char PyGSL_multiroot_module_doc[] = "XXX Missing\n";

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

     m=Py_InitModule("multiroot", mMethods);
     module = m;
     assert(m);

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

     dict = PyModule_GetDict(m);
     if(!dict)
	  goto fail;

     if (!(item = PyString_FromString((char*)PyGSL_multiroot_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_math.h>
#include <gsl/gsl_cblas.h>
#include "cblas.h"

void
cblas_ccopy (const int N, const void *X, const int incX, void *Y,
             const int incY)
{
#define BASE float
#include "source_copy_c.h"
#undef BASE
}


#include "jfftw_complex_nd_Plan.h"
#include <fftw.h>

/*
 * Class:     jfftw_complex_nd_Plan
 * Method:    createPlan
 * Signature: ([III)V
 */
JNIEXPORT void JNICALL Java_jfftw_complex_nd_Plan_createPlan( JNIEnv *env, jobject obj, jintArray dim, jint dir, jint flags )
{
	jclass clazz;
	jfieldID id;
	jbyteArray arr;
	unsigned char* carr;
	int rank;
	int *cdim;

	if( sizeof( jdouble ) != sizeof( fftw_real ) )
	{
		(*env)->ThrowNew( env, (*env)->FindClass( env, "java/lang/RuntimeException" ), "jdouble and fftw_real are incompatible" );
		return;
	}

	clazz = (*env)->GetObjectClass( env, obj );
	id    = (*env)->GetFieldID( env, clazz, "plan", "[B" );
	arr   = (*env)->NewByteArray( env, sizeof( fftwnd_plan ) );
	carr  = (*env)->GetByteArrayElements( env, arr, 0 );
	rank  = (*env)->GetArrayLength( env, dim );
	cdim  = (*env)->GetIntArrayElements( env, dim, 0 );

	(*env)->MonitorEnter( env, (*env)->FindClass( env, "jfftw/Plan" ) );

	*(fftwnd_plan*)carr = fftwnd_create_plan( rank, cdim, dir, flags );

	(*env)->MonitorExit( env, (*env)->FindClass( env, "jfftw/Plan" ) );

	(*env)->ReleaseIntArrayElements( env, dim, cdim, 0 );
	(*env)->ReleaseByteArrayElements( env, arr, carr, 0 );
	(*env)->SetObjectField( env, obj, id, arr );
}
/*
 * Class:     jfftw_complex_nd_Plan
 * Method:    createPlanSpecific
 * Signature: ([III[DI[DI)V
 */
JNIEXPORT void JNICALL Java_jfftw_complex_nd_Plan_createPlanSpecific( JNIEnv *env, jobject obj, jintArray dim, jint dir, jint flags, jdoubleArray in, jint idist, jdoubleArray out, jint odist )
{
	jclass clazz;
	jfieldID id;
	jbyteArray arr;
	unsigned char* carr;
	int rank;
	int *cdim;
	double *cin, *cout;

	if( sizeof( jdouble ) != sizeof( fftw_real ) )
	{
		(*env)->ThrowNew( env, (*env)->FindClass( env, "java/lang/RuntimeException" ), "jdouble and fftw_real are incompatible" );
		return;
	}

	clazz = (*env)->GetObjectClass( env, obj );
	id    = (*env)->GetFieldID( env, clazz, "plan", "[B" );
	arr   = (*env)->NewByteArray( env, sizeof( fftwnd_plan ) );
	carr  = (*env)->GetByteArrayElements( env, arr, 0 );
	rank  = (*env)->GetArrayLength( env, dim );
	cdim  = (*env)->GetIntArrayElements( env, dim, 0 );
	cin   = (*env)->GetDoubleArrayElements( env, in, 0 );
	cout  = (*env)->GetDoubleArrayElements( env, out, 0 );

	(*env)->MonitorEnter( env, (*env)->FindClass( env, "jfftw/Plan" ) );

	*(fftwnd_plan*)carr = fftwnd_create_plan_specific( rank, cdim, dir, flags, (fftw_complex*)cin, idist, (fftw_complex*)cout, odist );

	(*env)->MonitorExit( env, (*env)->FindClass( env, "jfftw/Plan" ) );

	(*env)->ReleaseDoubleArrayElements( env, in, cin, 0 );
	(*env)->ReleaseDoubleArrayElements( env, out, cout, 0 );
	(*env)->ReleaseByteArrayElements( env, arr, carr, 0 );
	(*env)->SetObjectField( env, obj, id, arr );
}
/*
 * Class:     jfftw_complex_nd_Plan
 * Method:    destroyPlan
 * Signature: ()V
 */
JNIEXPORT void JNICALL Java_jfftw_complex_nd_Plan_destroyPlan( JNIEnv* env, jobject obj )
{
	jclass clazz = (*env)->GetObjectClass( env, obj );
	jfieldID id = (*env)->GetFieldID( env, clazz, "plan", "[B" );
	jbyteArray arr = (jbyteArray)(*env)->GetObjectField( env, obj, id );
	unsigned char* carr = (*env)->GetByteArrayElements( env, arr, 0 );

	fftwnd_destroy_plan( *(fftwnd_plan*)carr );

	(*env)->ReleaseByteArrayElements( env, arr, carr, 0 );
	(*env)->SetObjectField( env, obj, id, NULL );
}
/*
 * Class:     jfftw_complex_nd_Plan
 * Method:    transform
 * Signature: ([D)[D
 */
JNIEXPORT jdoubleArray JNICALL Java_jfftw_complex_nd_Plan_transform___3D( JNIEnv* env, jobject obj, jdoubleArray in )
{
	jdouble *cin, *cout;
	jdoubleArray out;
	int i;

	jclass clazz = (*env)->GetObjectClass( env, obj );
	jfieldID id = (*env)->GetFieldID( env, clazz, "plan", "[B" );
	jbyteArray arr = (jbyteArray)(*env)->GetObjectField( env, obj, id );
	unsigned char* carr = (*env)->GetByteArrayElements( env, arr, 0 );
	fftwnd_plan plan = *(fftwnd_plan*)carr;
	int length = 1;
	for( i = 0; i < plan->rank; ++i ) length *= plan->n[i];
	if( length * 2 != (*env)->GetArrayLength( env, in ) )
	{
		(*env)->ThrowNew( env, (*env)->FindClass( env, "java/lang/IndexOutOfBoundsException" ), "the Plan was created for a different length" );
		(*env)->ReleaseByteArrayElements( env, arr, carr, 0 );
		return NULL;
	}

	cin = (*env)->GetDoubleArrayElements( env, in, 0 );

	if( plan->rank > 0 && ! plan->plans[0]->flags & FFTW_THREADSAFE )
	{
		// synchronization
		(*env)->MonitorEnter( env, obj );
	}

	if( plan->is_in_place )
	{
		out = in;

		fftwnd_one( plan, (fftw_complex*)cin, NULL );
	}
	else
	{
		out = (*env)->NewDoubleArray( env, length * 2 );
		cout = (*env)->GetDoubleArrayElements( env, out, 0 );

		fftwnd_one( plan, (fftw_complex*)cin, (fftw_complex*)cout );

		(*env)->ReleaseDoubleArrayElements( env, out, cout, 0 );
	}

	if( plan->rank > 0 && ! plan->plans[0]->flags & FFTW_THREADSAFE )
	{
		// synchronization
		(*env)->MonitorExit( env, obj );
	}

	(*env)->ReleaseDoubleArrayElements( env, in, cin, 0 );
	(*env)->ReleaseByteArrayElements( env, arr, carr, 0 );
	return out;
}
/*
 * Class:     jfftw_complex_nd_Plan
 * Method:    transform
 * Signature: (I[DII[DII)V
 */
JNIEXPORT void JNICALL Java_jfftw_complex_nd_Plan_transform__I_3DII_3DII( JNIEnv *env, jobject obj, jint howmany, jdoubleArray in, jint istride, jint idist, jdoubleArray out, jint ostride, jint odist )
{
	jdouble *cin, *cout;
	int i;

	jclass clazz = (*env)->GetObjectClass( env, obj );
	jfieldID id = (*env)->GetFieldID( env, clazz, "plan", "[B" );
	jbyteArray arr = (jbyteArray)(*env)->GetObjectField( env, obj, id );
	unsigned char* carr = (*env)->GetByteArrayElements( env, arr, 0 );
	fftwnd_plan plan = *(fftwnd_plan*)carr;
	int length = 1;
	for( i = 0; i < plan->rank; ++i ) length *= plan->plans[i]->n;
	if( (howmany - 1) * idist * 2 + length * istride * 2 != (*env)->GetArrayLength( env, in ) )
	{
		(*env)->ThrowNew( env, (*env)->FindClass( env, "java/lang/IndexOutOfBoundsException" ), "the Plan was created for a different length (in)" );
		(*env)->ReleaseByteArrayElements( env, arr, carr, 0 );
		return;
	}
	if( (howmany - 1) * odist * 2 + length * ostride * 2 != (*env)->GetArrayLength( env, out ) )
	{
		(*env)->ThrowNew( env, (*env)->FindClass( env, "java/lang/IndexOutOfBoundsException" ), "the Plan was created for a different length (out)" );
		(*env)->ReleaseByteArrayElements( env, arr, carr, 0 );
		return;
	}

	cin = (*env)->GetDoubleArrayElements( env, in, 0 );
	cout = (*env)->GetDoubleArrayElements( env, out, 0 );

	if( plan->rank > 0 && ! plan->plans[0]->flags & FFTW_THREADSAFE )
	{
		// synchronization
		(*env)->MonitorEnter( env, obj );
	}

	fftwnd( plan, howmany, (fftw_complex*)cin, istride, idist, (fftw_complex*)cout, ostride, odist );

	if( plan->rank > 0 && ! plan->plans[0]->flags & FFTW_THREADSAFE )
	{
		// synchronization
		(*env)->MonitorExit( env, obj );
	}

	(*env)->ReleaseByteArrayElements( env, arr, carr, 0 );
	(*env)->ReleaseDoubleArrayElements( env, in, cin, 0 );
	(*env)->ReleaseDoubleArrayElements( env, out, cout, 0 );
}


/* itersolve.c
 *
 * Copyright (C) 2014 Patrick Alken
 *
 * 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 <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_spmatrix.h>
#include <gsl/gsl_spblas.h>
#include <gsl/gsl_splinalg.h>

gsl_splinalg_itersolve *
gsl_splinalg_itersolve_alloc(const gsl_splinalg_itersolve_type *T,
                             const size_t n, const size_t m)
{
  gsl_splinalg_itersolve *w;

  w = calloc(1, sizeof(gsl_splinalg_itersolve));
  if (w == NULL)
    {
      GSL_ERROR_NULL("failed to allocate space for itersolve struct",
                     GSL_ENOMEM);
    }

  w->type = T;
  w->normr = 0.0;

  w->state = w->type->alloc(n, m);
  if (w->state == NULL)
    {
      gsl_splinalg_itersolve_free(w);
      GSL_ERROR_NULL("failed to allocate space for itersolve state",
                     GSL_ENOMEM);
    }

  return w;
} /* gsl_splinalg_itersolve_alloc() */

void
gsl_splinalg_itersolve_free(gsl_splinalg_itersolve *w)
{
  RETURN_IF_NULL(w);

  if (w->state)
    w->type->free(w->state);

  free(w);
}

const char *
gsl_splinalg_itersolve_name(const gsl_splinalg_itersolve *w)
{
  return w->type->name;
}

int
gsl_splinalg_itersolve_iterate(const gsl_spmatrix *A, const gsl_vector *b,
                               const double tol, gsl_vector *x,
                               gsl_splinalg_itersolve *w)
{
  int status = w->type->iterate(A, b, tol, x, w->state);

  /* store current residual */
  w->normr = w->type->normr(w->state);

  return status;
}

double
gsl_splinalg_itersolve_normr(const gsl_splinalg_itersolve *w)
{
  return w->normr;
}


//c++ network  author : liqi
//Nangjing University of Posts and Telecommunications
//date 2017.5.21,20:27
#ifndef NETWORK_H
#define NETWORK_H
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/highgui/highgui.hpp"
#include <algorithm>
#include <stdlib.h>
#include <memory.h>
#include <fstream>
#include <cstring>
#include <cblas.h>
#include <string>
#include <math.h>
#include "pBox.h"
using namespace cv;

void addbias(struct pBox *pbox, mydataFmt *pbias);
void image2Matrix(const Mat &image, const struct pBox *pbox);
void featurePad(const pBox *pbox, const pBox *outpBox, const int pad);
void feature2Matrix(const pBox *pbox, pBox *Matrix, const Weight *weight);
void maxPooling(const pBox *pbox, pBox *Matrix, int kernelSize, int stride);
void relu(struct pBox *pbox, mydataFmt *pbias);
void prelu(struct pBox *pbox, mydataFmt *pbias, mydataFmt *prelu_gmma);
void convolution(const Weight *weight, const pBox *pbox, pBox *outpBox, const struct pBox *matrix);
void fullconnect(const Weight *weight, const pBox *pbox, pBox *outpBox);
void readData(string filename, long dataNumber[], mydataFmt *pTeam[]);
long initConvAndFc(struct Weight *weight, int schannel, int lchannel, int kersize, int stride, int pad);
void initpRelu(struct pRelu *prelu, int width);
void softmax(const struct pBox *pbox);

void image2MatrixInit(Mat &image, struct pBox *pbox);
void featurePadInit(const pBox *pbox, pBox *outpBox, const int pad);
void maxPoolingInit(const pBox *pbox, pBox *Matrix, int kernelSize, int stride);
void feature2MatrixInit(const pBox *pbox, pBox *Matrix, const Weight *weight);
void convolutionInit(const Weight *weight, const pBox *pbox, pBox *outpBox, const struct pBox *matrix);
void fullconnectInit(const Weight *weight, pBox *outpBox);

bool cmpScore(struct orderScore lsh, struct orderScore rsh);
void nms(vector<struct Bbox> &boundingBox_, vector<struct orderScore> &bboxScore_, const float overlap_threshold, string modelname = "Union");
void refineAndSquareBbox(vector<struct Bbox> &vecBbox, const int &height, const int &width);

#endif


#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <math.h>
#include <float.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include "common.h"
#include "graph.h"
#include "utils.h"

void graph_init(graph *g, int vertices)
{
    g->num_v = vertices;
    g->num_e = (int*)malloc(vertices * sizeof(int));
    g->capacity = (int*)malloc(vertices * sizeof(int));
    g->adj_list = (int**)malloc(vertices * sizeof(int*));
    g->w_list = (double**)malloc(vertices * sizeof(double*));

    int i = 0;
    for (; i < vertices; ++i)
    {
        g->num_e[i] = 0;
        g->capacity[i] = GRAPH_INIT_CAPACITY;
        g->adj_list[i] = (int*)malloc(GRAPH_INIT_CAPACITY * sizeof(int));
        g->w_list[i] = (double*)malloc(GRAPH_INIT_CAPACITY * sizeof(double));
    }
}

ORIGIN_INLINE int graph_edges(const graph *g)
{
    const int num_v = g->num_v;
    int sum = 0;
    int v = 0;
    for (; v < num_v; ++v)
    {
        sum += g->num_e[v];
    }
    return sum;
}

ORIGIN_INLINE int graph_proper_edges(const graph *g)
{
    const int num_v = g->num_v;
    int sum = 0;
    int v = 0;
    for (; v < num_v; ++v)
    {
        int e = 0;
        for (; e < g->num_e[v]; ++e)
        {
            if (g->adj_list[v][e] != v)
            {
                ++sum;
            }
        }
    }
    return sum;
}

ORIGIN_INLINE int graph_loops(const graph *g)
{
    const int num_v = g->num_v;
    int sum = 0;
    int v = 0;
    for (; v < num_v; ++v)
    {
        int e = 0;
        for (; e < g->num_e[v]; ++e)
        {
            if (g->adj_list[v][e] == v)
            {
                ++sum;
            }
        }
    }
    return sum;
}

ORIGIN_INLINE int graph_outdegree(const graph *g, int u)
{
    return g->num_e[u];
}

ORIGIN_INLINE int graph_indegree(const graph *g, int u)
{
    const int num_v = g->num_v;
    int sum = 0;
    int v = 0;
    for (; v < num_v; ++v)
    {
        int e = 0;
        for (; e < g->num_e[v]; ++e)
        {
            if (g->adj_list[v][e] == u)
            {
                ++sum;
            }
        }
    }
    return sum;
}

ORIGIN_INLINE int graph_is_balanced(const graph *g)
{
    const int num_v = g->num_v;
    int u = 0;
    for (; u < num_v; ++u)
    {
        if (graph_indegree(g, u) != graph_outdegree(g, u))
        {
            return FALSE;
        }
    }
    return TRUE;
}

ORIGIN_INLINE void graph_add_edge(graph *g, int u, int v, double weight)
{
    if (g->num_e[u] == g->capacity[u])
    {
        graph_grow_lists(g, u);
    }
    g->adj_list[u][g->num_e[u]] = v;
    g->w_list[u][g->num_e[u]] = weight;
    g->num_e[u]++;
}

void graph_add_sym_edges(graph *g, int u, int v, double weight)
{
    graph_add_edge(g, u, v, weight);
    graph_add_edge(g, v, u, weight);
}

int graph_rmv_edge(graph *g, int u, int v)
{
    const int num_e = g->num_e[u];
    int e = 0;
    for (; e < num_e; ++e)
    {
        if (g->adj_list[u][e] == v)
        {
            g->num_e[u]--;
            int f = e;
            for (; f < num_e - 1; ++f)
            {
                g->adj_list[u][f] = g->adj_list[u][f+1];
                g->w_list[u][f] = g->w_list[u][f+1];
            }
            return TRUE;
        }
    }
    return FALSE;
}

int graph_rmv_sym_edges(graph *g, int u, int v)
{
    return (graph_rmv_edge(g, u, v) && graph_rmv_edge(g, v, u));
}

int graph_has_edge(graph *g, int u, int v)
{
    const int num_e = g->num_e[u];
    int e = 0;
    for (; e < num_e; ++e)
    {
        if (g->adj_list[u][e] == v)
        {
            return TRUE;
        }
    }
    return FALSE;
}

int graph_strongly_connected(const graph *g)
{
    const int num_v = g->num_v;
    int *group = (int*)malloc(num_v * sizeof(int));

    int u = 0;
    for (; u < num_v; ++u)
    {
        int v = 0;
        for (; v < num_v; ++v)
        {
            group[v] = FALSE;
        }

        group[u] = TRUE;
        graph_test_cc(g, group, u); // Call recursive function

        for (v = 0; v < num_v; ++v)
        {
            if (group[v] == FALSE)
            {
                free(group);
                return FALSE;
            }
        }
    }
    free(group);
    return TRUE;
}

void graph_svg(const graph *g, double *x, double *y, double size, double offset, FILE *out)
{
    fprintf(out, "<svg xmlns=\"http://www.w3.org/2000/svg\" version=\"1.1\">");
    const int num_v = g->num_v;
    for (int i = 0; i < num_v; ++i)
    {
        const int num_e = g->num_e[i];
        for (int j = 0; j < num_e; ++j)
        {
            const int e = g->adj_list[i][j];
            fprintf(out, "  <line x1=\"%f\" y1=\"%f\" x2=\"%f\" y2=\"%f\" style=\"stroke:rgb(0,0,0);stroke-width:2\"/>\n", offset + x[i] * size, offset + y[i] * size, offset + x[e] * size, offset + y[e] * size);
        }
    }
    for (int i = 0; i < num_v; ++i)
    {
        fprintf(out, "  <circle id=\"%d\" cx=\"%f\" cy=\"%f\" r=\"8\" fill=\"rgb(0,0,0)\"/>\n", i, offset + x[i] * size, offset + y[i] * size);
    }
    fprintf(out, "</svg>");
}

void graph_svg_abun(const graph *g, double *x, double *y, double size, double offset, double *abun, int color, FILE *out)
{
    fprintf(out, "<svg xmlns=\"http://www.w3.org/2000/svg\" version=\"1.1\">");
    const int num_v = g->num_v;
    for (int i = 0; i < num_v; ++i)
    {
        const int num_e = g->num_e[i];
        for (int j = 0; j < num_e; ++j)
        {
            const int e = g->adj_list[i][j];
            fprintf(out, "  <line x1=\"%f\" y1=\"%f\" x2=\"%f\" y2=\"%f\" style=\"stroke:rgb(0,0,0);stroke-width:2\"/>\n", offset + x[i] * size, offset + y[i] * size, offset + x[e] * size, offset + y[e] * size);
        }
    }
    int clr[3] = {0, 0, 0};
    for (int i = 0; i < num_v; ++i)
    {
        clr[color] = (int)(abun[i] * 255);
        fprintf(out, "  <circle id=\"%d\" cx=\"%f\" cy=\"%f\" r=\"8\" stroke=\"black\" stroke-width=\"1\" fill=\"rgb(%d,%d,%d)\"/>\n", i, offset + x[i] * size, offset + y[i] * size, clr[0], clr[1], clr[2]);
    }
    fprintf(out, "</svg>");
}

void graph_graphml(const graph *g, FILE *out, unsigned int id)
{
    const int num_v = g->num_v;
    fprintf(out, "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n");
    fprintf(out, "<graphml xmlns=\"http://graphml.graphdrawing.org/xmlns\"");
    fprintf(out, " xmlns:xsi=\"http://www.w3.org/2001/XMLSchema-instance\"");
    fprintf(out, " xsi:schemaLocation=\"http://graphml.graphdrawing.org/xmlns http://graphml.graphdrawing.org/xmlns/1.0/graphml.xsd\">\n");

    fprintf(out, "  <graph id=\"%u\" edgedefault=\"directed\">\n", id);
    for (int i = 0; i < num_v; ++i)
    {
        fprintf(out, "    <node id=\"v%d\"/>\n", i);
    }
    for (int i = 0; i < num_v; ++i)
    {
        for (int e = 0; e < g->num_e[i]; ++e)
        {
            fprintf(out, "    <edge source=\"v%d\" target=\"v%d\"/>\n", i, g->adj_list[i][e]);
        }
    }
    fprintf(out, "  </graph>\n");
    fprintf(out, "</graphml>\n");
}

void graph_print(const graph *g, FILE *out)
{
    for (int i = 0; i < g->num_v; ++i)
    {
        fprintf(out, "%5d -> ", i);
        for (int e = 0; e < g->num_e[i]; ++e)
        {
            fprintf(out, "%d ", g->adj_list[i][e]);
        }
        fprintf(out, "\n");
    }
}

void graph_free(graph *g)
{
    const int num_v = g->num_v;
    for (int i = 0; i < num_v; ++i)
    {
        free(g->adj_list[i]);
        free(g->w_list[i]);
    }
    free(g->adj_list);
    g->adj_list = NULL;
    free(g->w_list);
    g->w_list = NULL;
    free(g->num_e);
    g->num_e = NULL;
    free(g->capacity);
    g->capacity = NULL;
}

void graph_get_rgg(graph *g, int vertices, double r, double *x, double *y, gsl_rng *rng)
{
    graph_init(g, vertices);

    for (int i = 0; i < vertices; ++i)
    {
        x[i] = gsl_rng_uniform(rng);
        y[i] = gsl_rng_uniform(rng);
    }
    double d;
    for (int i = 0; i < vertices; ++i)
    {
        for (int j = 0; j < vertices; ++j)
        {
            const double a = x[i] - x[j];
            const double b = y[i] - y[j];
            d = hypot(a, b);
            if (d < r)
            {
                graph_add_edge(g, i, j, r - d);
            }
        }
    }
}

void graph_get_crgg(graph *g, int vertices, double r, double *x, double *y, gsl_rng *rng)
{
    graph_init(g, vertices);
    graph_get_rgg(g, vertices, r, x, y, rng);

    while (graph_strongly_connected(g) == FALSE)
    {
        graph_free(g);
        graph_get_rgg(g, vertices, r, x, y, rng);
    }
}

void graph_get_rec_rgg(graph *g, int vertices, double width, double r, double *x, double *y, gsl_rng *rng)
{
    graph_init(g, vertices);

    const double length = 1.0 / width; // A = l * w so l = 1 / w

    for (int i = 0; i < vertices; ++i)
    {
        x[i] = gsl_rng_uniform(rng) * length;
        y[i] = gsl_rng_uniform(rng) * width;
    }
    double d;
    for (int i = 0; i < vertices; ++i)
    {
        for (int j = 0; j < vertices; ++j)
        {
            const double a = x[i] - x[j];
            const double b = y[i] - y[j];
            d = hypot(a, b);
            if (d < r)
            {
                graph_add_edge(g, i, j, r - d);
            }
        }
    }
}

void graph_get_rec_crgg(graph *g, int vertices, double width, double r, double *x, double *y, gsl_rng *rng)
{
    graph_init(g, vertices);
    graph_get_rec_rgg(g, vertices, width, r, x, y, rng);

    while (graph_strongly_connected(g) == FALSE)
    {
        graph_free(g);
        graph_get_rec_rgg(g, vertices, width, r, x, y, rng);
    }
}

void graph_get_complete(graph *g, int vertices)
{
    graph_init(g, vertices);

    for (int i = 0; i < vertices; ++i)
    {
        for (int j = 0; j < vertices; ++j)
        {
            graph_add_edge(g, i, j, 1.0);
        }
    }
}

void graph_get_circle(graph *g, int vertices)
{
    graph_init(g, vertices);

    graph_add_edge(g, vertices - 1, vertices - 1, 1.0);
    graph_add_sym_edges(g, 0, vertices - 1, 1.0);

    for (int u = 0; u < vertices - 1; ++u)
    {
        graph_add_edge(g, u, u, 1.0);
        graph_add_sym_edges(g, u, u + 1, 1.0);
    }
}

void graph_get_star(graph *g, int vertices)
{
    graph_init(g, vertices);

    for (int u = 0; u < vertices; ++u)
    {
        graph_add_edge(g, u, u, 1.0);
        graph_add_sym_edges(g, u, 0, 1.0);
    }
}

///////////////////////////////////////////////////////////////
// 'Private' functions

void graph_test_cc(const graph *g, int *group, int u)
{
    const int num_e = g->num_e[u];
    int e = 0;
    for (; e < num_e; ++e)
    {
        const int head = g->adj_list[u][e];
        if (head != u && group[head] == FALSE)
        {
            group[head] = TRUE;
            graph_test_cc(g, group, head);
        }
    }
}

// Grow the edge list of some vertex v.
void graph_grow_lists(graph *g, int u)
{
    g->capacity[u] <<= 1;

    int *tmp_adj = (int*)malloc(g->capacity[u] * sizeof(int));
    double *tmp_w = (double*)malloc(g->capacity[u] * sizeof(double));

    const int num_e = g->num_e[u];
    int e = 0;
    for (; e < num_e; ++e)
    {
        tmp_adj[e] = g->adj_list[u][e];
        tmp_w[e] = g->w_list[u][e];
    }
    int *swap_adj = g->adj_list[u];
    double *swap_w = g->w_list[u];

    g->adj_list[u] = tmp_adj;
    g->w_list[u] = tmp_w;

    free(swap_adj);
    free(swap_w);
}