/*=============================================================================
    Copyright (c) 2004 Angus Leeming

    Distributed under the Boost Software License, Version 1.0.
    (See accompanying file LICENSE_1_0.txt or copy at
    http://www.boost.org/LICENSE_1_0.txt)
=============================================================================*/
#if defined PHOENIX_LIMIT && PHOENIX_LIMIT < 6
#undef PHOENIX_LIMIT
#endif

#ifndef PHOENIX_LIMIT
#define PHOENIX_LIMIT 6
#endif

#define YAC_SINGLE_COMPILATION_UNIT

#include "yac_gnuplot_grammar.hpp"
#include "yac_skip_grammar.hpp"
#include "yac_virtual_machine.hpp"

#include <boost/spirit/iterator/file_iterator.hpp>
#include <boost/math/special_functions/acosh.hpp>
#include <boost/math/special_functions/asinh.hpp>
#include <boost/math/special_functions/atanh.hpp>

#include <iostream>
#include <sstream>
#include <string>

using std::cerr;
using std::cin;
using std::cout;
using std::endl;
using std::string;

namespace spirit = boost::spirit;

namespace {

int    bitwise_and(int a, int b) { return a & b; }
int    bitwise_or(int a, int b) { return a | b; }
int    bitwise_xor(int a, int b) { return a ^ b; }
bool   logical_and(bool a, bool b) { return a && b; }
double logical_not(double a) { return !a; }
double equal(double a, double b) { return a == b; }
double not_equal(double a, double b) { return a != b; }
double less(double a, double b) { return a < b; }
double greater(double a, double b) { return a > b; }
double less_equal(double a, double b) { return a <= b; }
double greater_equal(double a, double b) { return a >= b; }
int    shift_left(int a, int b) { return a << b; }
int    shift_right(int a, int b) { return a >> b; }
double add(double a, double b) { return a + b; }
double subtract(double a, double b) { return a - b; }
double multiply(double a, double b) { return a * b; }
double divide(double a, double b) { return a / b; }
int    mod(int a, int b) { return a % b; }
double negate(double a) { return -a; }
double sgn(double a) { return a >= 0.0 ? 1.0 : -1.0; }

typedef int(* ifunc2_ptr_t)(int, int);
typedef bool(* bfunc2_ptr_t)(bool, bool);
typedef double(* dfunc2_ptr_t)(double, double);
typedef double(* dfunc1_ptr_t)(double);

} // namespace anon

namespace yac {

boost::shared_ptr<function> make_func(string const & name, dfunc1_ptr_t ptr)
{
    return boost::shared_ptr<function>(new function1<double>(name, ptr));
}

boost::shared_ptr<function> make_func(string const & name, dfunc2_ptr_t ptr)
{
    return boost::shared_ptr<function>(new function2<double>(name, ptr));
}

boost::shared_ptr<function> make_func(string const & name, bfunc2_ptr_t ptr)
{
    return boost::shared_ptr<function>(new function2<bool>(name, ptr));
}

boost::shared_ptr<function> make_func(string const & name, ifunc2_ptr_t ptr)
{
    return boost::shared_ptr<function>(new function2<int>(name, ptr));
}

class controller {
public:
  controller(std::vector<double> X)
    : x_vector(X)
    {
        char const * const keywords[] = {
	  "x",
            "return",
            "pi", "e",
            "abs", "acos", "acosh", "asin", "asinh", "atan",
            "atanh", "besj0", "besj1", "besy0", "besy1", "ceil",
            "cos", "cosh",
#if defined(__GNUC__)
            "erf", "erfc",
#endif
            "exp", "floor", "log", "log10", "sgn", "sin", "sinh",
            "sqrt", "tan", "tanh", "atan2",
        };
        std::size_t const keywords_size =
            sizeof(keywords) / sizeof(keywords[0]);

        for (std::size_t i = 0; i != keywords_size; ++i)
                name_grammar::reserved_keywords.add(keywords[i]);

        vm.global_vars.add
            ("pi", 3.1415926536)
            ("e",  2.7182818285);

        vm.funcs.add
            ("bitwise_and##2",   make_func("bitwise_and#",   &::bitwise_and))
            ("bitwise_or##2",    make_func("bitwise_or#",    &::bitwise_or))
            ("bitwise_xor##2",   make_func("bitwise_xor#",   &::bitwise_xor))
            ("logical_and##2",   make_func("logical_and#",   &::logical_and))
            ("logical_not##1",   make_func("logical_not#",   &::logical_not))
            ("equal##2",         make_func("equal#",         &::equal))
            ("not_equal##2",     make_func("not_equal#",     &::not_equal))
            ("less##2",          make_func("less#",          &::less))
            ("greater##2",       make_func("greater#",       &::greater))
            ("less_equal##2",    make_func("less_equal#",    &::less_equal))
            ("greater_equal##2", make_func("greater_equal#", &::greater_equal))
            ("shift_left##2",    make_func("shift_left#",    &::shift_left))
            ("shift_right##2",   make_func("shift_right#",   &::shift_right))
            ("add##2",           make_func("add#",           &::add))
            ("subtract##2",      make_func("subtract#",      &::subtract))
            ("multiply##2",      make_func("multiply#",      &::multiply))
            ("divide##2",        make_func("divide#",        &::divide))
            ("mod##2",           make_func("mod#",           &::mod))
            ("negate##1",        make_func("negate#",        &::negate))
            ("abs#1",   make_func("abs",   &std::abs))
            ("acos#1",  make_func("acos",  &std::acos))
            ("asin#1",  make_func("asin",  &std::asin))
            ("atan#1",  make_func("atan",  &std::atan))
            ("acosh#1", make_func("acosh", &boost::math::acosh))
            ("asinh#1", make_func("asinh", &boost::math::asinh))
            ("atanh#1", make_func("atanh", &boost::math::atanh))
            ("besj0#1", make_func("besj0", &j0))
            ("besj1#1", make_func("besj1", &j1))
            ("besy0#1", make_func("besy0", &y0))
            ("besy1#1", make_func("besy1", &y1))
            ("ceil#1",  make_func("ceil",  &std::ceil))
            ("cos#1",   make_func("cos",   &std::cos))
            ("cosh#1",  make_func("cosh",  &std::cosh))
#if defined(__GNUC__)
            ("erf#1",   make_func("erf",   &erf))
            ("erfc#1",  make_func("erfc",  &erfc))
#endif
            ("exp#1",   make_func("exp",   &std::exp))
            ("floor#1", make_func("floor", &std::floor))
            ("log#1",   make_func("log",   &std::log))
            ("log10#1", make_func("log10", &std::log10))
            ("sgn#1",   make_func("sgn",   &sgn))
            ("sin#1",   make_func("sin",   &std::sin))
            ("sinh#1",  make_func("sinh",  &std::sinh))
            ("sqrt#1",  make_func("sqrt",  &std::sqrt))
            ("tan#1",   make_func("tan",   &std::tan))
            ("tanh#1",  make_func("tanh",  &std::tanh))
            ("atan2#2", make_func("atan2", &std::atan2));
    }

    template <typename ItT>
    std::vector<double> parse(ItT first, ItT last)
    {
        using phoenix::arg1;
        using phoenix::var;

        typedef spirit::parse_info<ItT> parse_info_t;

        gnuplot_grammar calculator(vm.funcs, vm.global_vars);

        parse_info_t info = spirit::parse(first, last,
                                          calculator[var(vm.stk) = arg1],
                                          skip_grammar());

        if (info.full) {
	  //evaluate(vm.stk, x_vector.at(0));
	  std::vector<double> result = vector_eval(vm.stk, x_vector);
	  return result;
        } else {
            cerr << "-------------------------\n"
                 << "Parsing failed\n"
                 << "-------------------------\n";
	    return std::vector<double> (x_vector.size(), 0.0);
        }
    }
private:
    virtual_machine vm;
  std::vector<double> x_vector;
};

} // namespace yac

////////////////////////////////////////////////////////////////////////////
//
//  What follows below is glue to handle data input in different ways,
//  from file, as piped input from stdin or interactive input.
//
////////////////////////////////////////////////////////////////////////////
namespace {

char const * prompt()
{
    return "yac> ";
}

int usage()
{
    cerr << "Usage: yac [Input mode]\n"
         << "\tInput mode\n"
         << "\t-i, --interactive    Data input interactively\n"
         << "\t-f, --file FILE      Data input from FILE\n"
         << "\t-p, --piped          Data piped through stdin\n";
    return 0;
}

enum input_mode {
    file_mode,
    piped_mode,
    interactive_mode,
    unrecognized_mode
};

input_mode get_input_mode(string const & mode_arg)
{
    if (mode_arg == "-f" || mode_arg == "--file")
        return file_mode;
    if (mode_arg == "-p" || mode_arg == "--piped")
        return piped_mode;
    if (mode_arg == "-i" || mode_arg == "--interactive")
        return interactive_mode;
    return unrecognized_mode;
}

} // namespace anon

int main(int argc, char * argv[])
{
    if (argc < 2)
        return usage();

    input_mode const mode = get_input_mode(argv[1]);
    if (mode == unrecognized_mode)
        return usage();

    if (mode == file_mode) {
        if (argc != 3)
            return usage();
    } else if (argc != 2)
        return usage();

    unsigned int n = 20;
    std::vector<double> X(n+1, 0.0);
    for (unsigned int i=0; i<=n; ++i) {
      X.at(i) = (double)i/n;
    }
    yac::controller calc(X);

    switch (mode) {
    case file_mode:
    {
        char const * const file = argv[2];
        spirit::file_iterator<> file_it(file);

        if (!file_it) {
                cerr << "Unable to open file '" << file << "'!"
                     << endl;
                return 1;
        }

	std::vector<double> result = calc.parse(file_it, file_it.make_end());
	for (unsigned int i=0; i<result.size();++i) {
	  std::cerr << result.at(i) << std::endl;
	}
        break;
    }
    case piped_mode:
    {
        string data;
        std::ostringstream ss;
        while (getline(cin, data))
            ss << data << '\n';
        data = ss.str();

        calc.parse(data.begin(), data.end());
        break;
    }
    case interactive_mode:
    {
        typedef spirit::parse_info<string::const_iterator> parse_info_t;
        using spirit::space_p;

        cout << "/////////////////////////////////////////////////////////\n\n"
             << "\t\tYet Another Calculator...\n\n"
             << "/////////////////////////////////////////////////////////\n\n"
             << "Type an expression...or [q or Q] to quit\n\n";

        string data;
        string buffer;
        cout << prompt();
        while (getline(cin, buffer)) {
            // getline strips the final '\n', so add it back.
            buffer += '\n';

            // Quit?
            // Find the first non-whitespace character.
            parse_info_t info
                = spirit::parse(buffer.begin(), buffer.end(), *space_p);

            if (info.full) {
                // Nothing but whitespace
                if (data.empty())
                    cout << prompt();
                continue;
            } else {
                string::size_type const first = info.stop - buffer.begin();
                if (buffer[first] == 'q' || buffer[first] == 'Q')
                    break;
            }

            // Continuation?
            // Find the last non-whitespace character.
            string const rbuffer = string(buffer.rbegin(), buffer.rend());
            // Cannot fail because we're testing the same data as above.
            info = spirit::parse(rbuffer.begin(), rbuffer.end(), *space_p);
            string::size_type const last = rbuffer.end() - info.stop - 1;

            data += buffer;
            if (buffer[last] == '\\')
                continue;

            // The data has been generated, so now get on and parse it properly.
            calc.parse(data.begin(), data.end());
            data.clear();
            cout << prompt();
        }
        break;
    }
    case unrecognized_mode:
        break;
    }

    return 0;
}


/*======================================================================
Copyright 2019 Google LLC

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You mplaay obtain a copy of the License at

    https://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.
======================================================================*/

#include <cmath>

#include <Eigen/Dense>
#include "PCV_Types.h"

#include "Filter.h"

using namespace Eigen;

// New zero-size filter to be Allocated by creation of specific filter type
Filter::Filter()
  : size_(0),order_(0),a_(NULL),b_(NULL),f_(NULL),r_(NULL)
{ }

// New filter is a copy of an existing filter
Filter::Filter(Filter const &src)
{
  Allocate(src.size_,src.order_);
  operator=(src);
}

Filter &
Filter::operator=(Filter const &src)
{
  (*a_) = *src.a_;
  (*b_) = *src.b_;
  for(int i=0; i<order_+1 ; i++ )
  { *f_[i] = *src.f_[i];
    *r_[i] = *src.r_[i];
  }
  return (*this);
}

void
Filter::Allocate( int size,  int order)
// Allocates memory for filter constants and aging the data
{
  if( size_>0 ) // PURGE DATA, IF CURRENTLY HAS DATA
    Purge();

  size_ = size;
  order_ = order;

  a_ = new VectorXd(order_+1);
  a_->setZero(order+1);
  b_ = new VectorXd(order_+1);
  b_->setZero(order+1);

  f_ = new VectorXd* [order_+1];
  r_ = new VectorXd* [order_+1];
  for(int i=0; i<order_+1 ; i++ )
  { f_[i] = new VectorXd(size_);
    f_[i]->setZero(size_);
    r_[i] = new VectorXd(size_);
    r_[i]->setZero(size_);
  }
}

void
Filter::Purge()
{
  delete a_;
  delete b_;
  for(int i=0; i<order_+1 ; i++ )
  { delete f_[i];
    delete r_[i];
  }
  delete [] f_;
  delete [] r_;

  size_ = 0;
  order_ = 0;
}

Filter::~Filter()
// Destructor
{
  Purge();
}

VectorXd
Filter::Filt(VectorXd const &in)
// Use this after assigning a filter type to filter the data
{
  int i;

  if( order_ == 0 )  // COPY AND BAIL OUT
  { return in;
  }

  *r_[0] = in;  // NEED THIS ASSIGNMENT FOR AGEING TOO
  *f_[0] = *r_[0] * (*b_)[0];

  for( i=1; i<order_+1 ; i++) // NOTE: a_[0] NOT USED. (NORMALIZED)
  {
    *f_[0] +=   *r_[i] * (*b_)[i] - *f_[i] * (*a_)[i];
  }
  // AGE THE VALUES
  for( i=0; i<order_ ; i++)
  { *f_[i+1] = *f_[i];
    *r_[i+1] = *r_[i];
  }
  return *f_[0];
}

void
Filter::Z_NumDen(VectorXd const &r0,
                 VectorXd const &z_num, VectorXd const &z_den,
                 VectorXd const &f0 )
// general digital transfer function: z_num/z_den
{
  Allocate( r0.size(), z_den.size()-1 );

  // COPY COEFF VECTORS
  for(int i=0; i<z_num.size(); i++)
    (*b_)[i] = z_num[i];  // FOR order(num) <= order(den)
  *a_ = z_den; // COPY DENOMINATOR

  for(int i=1; i<order_+1; i++)
  { *r_[i] = r0; //Store initial value
    *f_[i] = f0; //Store initial value
  }
}

void
Filter::LowPass(VectorXd const &r0, float sampFreq, float cutOffFreq)
// w/(s+w)
{
  Allocate( r0.size(), 1 );
  float T = 1.0/sampFreq;

  float w = M_2_PI*cutOffFreq;
  w = (2/T)*tan(w*T/2);  // WARPING TO MATCH CUTOFF FREQ

  float p = (T*w/2)+1;
  float m = (T*w/2)-1;

  (*b_)[0] =  (T*w/2)/p;
  (*b_)[1] =  (*b_)[0];
  (*a_)[0] =  1;
  (*a_)[1] =  m/p;

  *r_[1] = r0; //Store initial value
  *f_[1] = r0; //Store initial value
}

void
Filter::D_LowPass(VectorXd const &r0, float sampFreq, float cutOffFreq)
// (s*w)/(s+w)
{
  Allocate( r0.size(), 1 );
  float T = 1.0/sampFreq;

  float w = M_2_PI*cutOffFreq;
  w = (2/T)*tan(w*T/2);  // WARPING TO MATCH CUTOFF FREQ

  (*b_)[0] =  2*w/(w*T+2);
  (*b_)[1] = -(*b_)[0];
  (*a_)[0] =  1;
  (*a_)[1] =  (w*T-2)/(w*T+2);

  *r_[1] = r0;       //Store initial value
   f_[1]->setZero(); //Store initial value
}

void
Filter::D(VectorXd const &r0, float sampFreq)
//  s
{
  Allocate( r0.size(), 1 );
  float T = 1.0/sampFreq;

  (*b_)[0] =  1.0/T;
  (*b_)[1] = -(*b_)[0];
  (*a_)[0] =  1;
  (*a_)[1] =  0;

  *r_[1] = r0;       //Store initial value
   f_[1]->setZero(); //Store initial value
}

void
Filter::LeadLag(VectorXd const &r0, float sampFreq,
                     float f1, float f2, float f3 )
// [w2*w3/w1](s+w1)/[(s+w2)(s+w3)]
{
  Allocate( r0.size(), 2 );
  float T = 1.0/sampFreq;

  float w1 = M_2_PI * f1;
  w1 = (2/T)*tan(w1*T/2);  // WARPING TO MATCH FREQ
  float w2 = M_2_PI * f2;
  w2 = (2/T)*tan(w2*T/2);  // WARPING TO MATCH FREQ
  float w3 = M_2_PI * f3;
  w3 = (2/T)*tan(w3*T/2);  // WARPING TO MATCH FREQ

  float g0 = (w2*w3/w1); // UNITY DC GAIN
  float p2 = T*w2/2 + 1;
  float m2 = T*w2/2 - 1;
  float p3 = T*w3/2 + 1;
  float m3 = T*w3/2 - 1;
  float pp = p2*p3;
  float  c = g0*T/2/pp;

  (*b_)[0] =  c*(w1+1);
  (*b_)[1] =  c*w1;
  (*b_)[2] = -c;
  (*a_)[0] =  1;
  (*a_)[1] =  (m2*p3+p2*m3)/pp;
  (*a_)[2] =  m2*m3/pp;

  *r_[1] = r0; //Store initial value
  *r_[2] = r0; //Store initial value
  *f_[1] = r0; //Store initial value
  *f_[2] = r0; //Store initial value
}

void
Filter::Unity()   // COPIES INPUT TO OUTPUT
{
  Allocate( 1, 0 );
  (*b_)[0] = 1;
  (*a_)[0] = 1;
}


// Copyright Yamaha 2021
// MIT License
// https://github.com/yamaha-bps/cbr_math/blob/master/LICENSE

#ifndef CBR_MATH__INTERP__PIECEWISE_POLY_HPP_
#define CBR_MATH__INTERP__PIECEWISE_POLY_HPP_

#include <Eigen/Dense>

#include <algorithm>
#include <functional>
#include <array>
#include <cstdint>
#include <exception>
#include <type_traits>
#include <utility>
#include <vector>
#include <tuple>

#include "cbr_math/math.hpp"
#include "cbr_math/eigen_traits.hpp"

// Forward declare YAML::convert
namespace YAML
{
template<typename T>
struct convert;
}  // namespace YAML

namespace cbr
{

namespace pp_details
{

class PiecewisePolyBase
{
protected:
  struct Extremities
  {
    Extremities() = default;
    Extremities(const Extremities &) = default;
    Extremities(Extremities &&) = default;
    Extremities & operator=(const Extremities &) = default;
    Extremities & operator=(Extremities &&) = default;
    ~Extremities() = default;

    template<typename T1, typename T2, typename T3, typename T4>
    Extremities(T1 && a, T2 && b, T3 && c, T4 && d)
    : val0{std::forward<T1>(a)},
      der0{std::forward<T2>(b)},
      valEnd{std::forward<T3>(c)},
      derEnd{std::forward<T4>(d)}
    {}

    double val0{};
    double der0{};
    double valEnd{};
    double derEnd{};
  };

public:
  enum class EXTRAP : uint16_t
  {
    CLAMP,
    LINEAR,
    YOLO
  };

  // Search method
  enum class SEARCH : uint16_t
  {
    LINEAR_FORWARD,
    LINEAR_REVERSE,
    LINEAR_HINT,
    BINARY
  };

public:
  template<typename T = double>
  using col_t = Eigen::Matrix<T, Eigen::Dynamic, 1>;
  using row_t = Eigen::Matrix<double, 1, Eigen::Dynamic>;
  using matrix_t = Eigen::MatrixXd;

  PiecewisePolyBase() = delete;
  PiecewisePolyBase(const PiecewisePolyBase &) = default;
  PiecewisePolyBase(PiecewisePolyBase &&) = default;
  PiecewisePolyBase & operator=(const PiecewisePolyBase &) = default;
  PiecewisePolyBase & operator=(PiecewisePolyBase &&) = default;
  ~PiecewisePolyBase() = default;

  template<typename T1>
  PiecewisePolyBase(
    T1 && breakPoints,
    EXTRAP extrap,
    SEARCH search)
  : extrap_{extrap},
    search_{search},
    breakPoints_{std::forward<T1>(breakPoints)},
    length_{static_cast<std::size_t>(breakPoints_.cols())},
    x0_{breakPoints_.cols() > 0 ? breakPoints_[0] : 0.},
    xEnd_{breakPoints_.cols() > 0 ? breakPoints_[breakPoints_.cols() - 1] : 1.},
    hintSlope_{static_cast<double>(length_ - 1) / (xEnd_ - x0_)}
  {
    if (breakPoints_.cols() < 2) {
      throw std::invalid_argument(
              "Not enought break points to construct a PiecewisePoly, need at least 2.");
    }

    auto i2 = std::adjacent_find(
      breakPoints_.data(),
      breakPoints_.data() + breakPoints_.cols(),
      std::greater_equal<double>());

    if (i2 != (breakPoints_.data() + breakPoints_.cols())) {
      throw std::invalid_argument("PiecewisePoly's break points vector must be stricly sorted.");
    }
  }

  void set_search(const SEARCH search) noexcept
  {
    search_ = search;
  }

  void set_extrap(const EXTRAP extrap) noexcept
  {
    extrap_ = extrap;
  }

  const SEARCH & get_search() const noexcept
  {
    return search_;
  }

  const EXTRAP & get_extrap() const noexcept
  {
    return extrap_;
  }

  const row_t & breakPoints() const noexcept
  {
    return breakPoints_;
  }

  const double & lb() const noexcept
  {
    return x0_;
  }

  const double & ub() const noexcept
  {
    return xEnd_;
  }

  auto bounds() const noexcept
  {
    return std::pair<const double &, const double &>(lb(), ub());
  }

  const std::size_t & length() const noexcept
  {
    return length_;
  }

protected:
  std::pair<std::size_t, int8_t>
  findPosition(const double x) const
  {
    if (x < x0_) {
      return std::pair<std::size_t, int8_t>(0, -1);
    }

    if (x >= xEnd_) {
      return std::pair<std::size_t, int8_t>(0, 1);
    }

    std::size_t idx = 0;
    std::size_t flag = 0;

    if (length_ > 2) {
      switch (search_) {
        case SEARCH::LINEAR_FORWARD: {
            idx = 0;
            if (!findPositionLinear(true, x, idx)) {
              throw std::runtime_error("Couldn't find break point. Should never happen!");
            }
            break;
          }

        case SEARCH::LINEAR_REVERSE: {
            idx = length_ - 2;
            if (!findPositionLinear(false, x, idx)) {
              throw std::runtime_error("Couldn't find break point. Should never happen!");
            }
            break;
          }

        case SEARCH::LINEAR_HINT: {
            int64_t idxTmp = lround(hintSlope_ * (x - x0_));
            idx = static_cast<std::size_t>(idxTmp);
            if (idxTmp < 0 || idx > (length_ - 1)) {
              throw std::runtime_error("Point outside break points range. Should never happen!");
            }

            if (breakPoints_[static_cast<Eigen::Index>(idx)] >= x) {
              if (idx == length_ - 1) {
                idx--;
              }
              if (!findPositionLinear(false, x, idx)) {
                throw std::runtime_error("Couldn't find break point. Should never happen!");
              }
            } else {
              if (!findPositionLinear(true, x, idx)) {
                throw std::runtime_error("Couldn't find break point. Should never happen!");
              }
            }
            break;
          }

        case SEARCH::BINARY: {
            idx = 0;
            if (!findPositionBinary(x, idx)) {
              throw std::runtime_error("Couldn't find break point. Should never happen!");
            }
            break;
          }

        default: {
            throw std::runtime_error("Unkown search method. Should never happen!");
          }
      }
    }

    return std::pair<std::size_t, int8_t>(idx, flag);
  }

  /**
   * @brief Find index idx s.t. breakPts[idx] <= x < breakPts[idx+1] via linear search
   *
   * @param[in] forward search forward from starting index
   * @param[in] x
   * @param[in, out] idx starting index
   */
  bool findPositionLinear(
    const bool forward,
    const double x,
    std::size_t & idx) const noexcept
  {
    if (idx > (length_ - 2)) {
      return false;
    }

    if (forward) {
      for (; idx <= (length_ - 2); idx++) {
        if (breakPoints_[static_cast<Eigen::Index>(idx)] <= x &&
          x < breakPoints_[static_cast<Eigen::Index>(idx + 1)])
        {
          return true;
        }
      }
    } else {
      if (breakPoints_[0] <= x && x < breakPoints_[1]) {
        idx = 0;
        return true;
      }
      for (; idx > 0; idx--) {
        if (breakPoints_[static_cast<Eigen::Index>(idx)] <= x &&
          x < breakPoints_[static_cast<Eigen::Index>(idx + 1)])
        {
          return true;
        }
      }
    }

    return false;
  }

  /**
   * @brief Find index idx s.t. breakPts[idx] <= x < breakPts[idx+1] via binary search
   *
   * @param[in] x
   * @paran[out] idx
   */
  bool findPositionBinary(
    const double x,
    std::size_t & idx) const noexcept
  {
    if (length_ < 2 || x < breakPoints_[0] ||
      x >= breakPoints_[static_cast<Eigen::Index>(length_ - 1)])
    {
      return false;
    }

    std::size_t low{0}, high{length_ - 1};

    while (low < high) {
      const double frac = (x - breakPoints_[static_cast<Eigen::Index>(low)]) /
        (breakPoints_[static_cast<Eigen::Index>(high)] -
        breakPoints_[static_cast<Eigen::Index>(low)]);
      idx = low + static_cast<std::size_t>(frac * static_cast<double>(high - low));
      if (x < breakPoints_[static_cast<Eigen::Index>(idx)]) {
        high = idx;
      } else if (x >= breakPoints_[static_cast<Eigen::Index>(idx) + 1]) {
        low = idx + 1;
      } else {
        break;  // we have breakPoints_[idx] <= t < breakPoints_[idx+1]
      }
    }

    return true;
  }

  template<typename T>
  T evalPoly(
    const T x,
    const std::size_t pieceIdx,
    const matrix_t & coefList,
    const std::size_t order) const
  {
    T val =
      static_cast<T>(coefList(
        static_cast<Eigen::Index>(order - 1),
        static_cast<Eigen::Index>(pieceIdx)));
    const auto & x0 = breakPoints_[static_cast<Eigen::Index>(pieceIdx)];
    for (std::size_t i = 1; i < order; i++) {
      val +=
        static_cast<T>(coefList(
          static_cast<Eigen::Index>(order - 1 - i),
          static_cast<Eigen::Index>(pieceIdx))) * powFast<T, std::size_t>(
        x - static_cast<T>(x0), i);
    }
    return val;
  }

  template<typename T>
  T evalPolyDer(
    const T x,
    const std::size_t pieceIdx,
    const std::size_t n,
    const matrix_t & coefList,
    const std::size_t order) const
  {
    if (n == 0) {
      return evalPoly(x, pieceIdx, coefList, order);

    } else {
      T val{0.};

      if (n < order) {
        const auto & x0 = breakPoints_[static_cast<Eigen::Index>(pieceIdx)];
        for (std::size_t i = n; i < order; i++) {
          val +=
            static_cast<T>(factorial(i, i - n + 1)) * static_cast<T>(coefList(
              static_cast<Eigen::Index>(order - 1 - i),
              static_cast<Eigen::Index>(pieceIdx))) *
            powFast<T, std::size_t>(
            x - static_cast<T>(x0),
            i - n);
        }
      }

      return val;
    }
  }

  template<typename T>
  T extrapPoly(
    const int8_t direction,
    const T x,
    const std::size_t n,
    const matrix_t & coefList,
    const std::size_t order,
    const Extremities & ex) const
  {
    if (direction == 1) {
      switch (extrap_) {
        case EXTRAP::CLAMP: {
            if (n == 0) {
              return static_cast<T>(ex.valEnd);
            }
            return T{0.};
          }

        case EXTRAP::LINEAR: {
            if (n == 0) {
              return static_cast<T>(ex.valEnd + ex.derEnd * (x - xEnd_));
            }
            if (n == 1) {
              return static_cast<T>(ex.derEnd);
            }
            return T{0.};
          }

        case EXTRAP::YOLO: {
            if (n == 0) {
              return evalPoly(x, length_ - 2, coefList, order);
            }
            return evalPolyDer(x, length_ - 2, n, coefList, order);
          }

        default: {
            throw std::runtime_error("Unkown extrapolation method. Should never happen!");
          }
      }
    } else if (direction == -1) {
      switch (extrap_) {
        case EXTRAP::CLAMP: {
            if (n == 0) {
              return static_cast<T>(ex.val0);
            }
            return T{0.};
          }
        case EXTRAP::LINEAR: {
            if (n == 0) {
              return static_cast<T>(ex.val0 + ex.der0 * (x - x0_));
            }
            if (n == 1) {
              return static_cast<T>(ex.der0);
            }
            return T{0.};
          }

        case EXTRAP::YOLO: {
            if (n == 0) {
              return evalPoly(x, 0, coefList, order);
            }
            return evalPolyDer(x, 0, n, coefList, order);
          }

        default: {
            throw std::runtime_error("Unkown extrapolation method. Should never happen!");
          }
      }
    } else {
      throw std::runtime_error("Invalid extrapolation direction. Should never happen!");
    }
  }

protected:
  EXTRAP extrap_{};
  SEARCH search_{};
  row_t breakPoints_{};
  std::size_t length_{};
  double x0_{};
  double xEnd_{};
  double hintSlope_{};
};

}  // namespace pp_details

/***************************************************************************
 * \brief Class for a scalar piecewise polynomial with scalar image
 ***************************************************************************/
class PiecewisePoly : public pp_details::PiecewisePolyBase
{
public:
  PiecewisePoly() = delete;
  PiecewisePoly(const PiecewisePoly &) = default;
  PiecewisePoly(PiecewisePoly &&) = default;
  PiecewisePoly & operator=(const PiecewisePoly &) = default;
  PiecewisePoly & operator=(PiecewisePoly &&) = default;
  ~PiecewisePoly() = default;

  /**
   * @brief Scalar-valued piecewise polynomial
   *
   * @tparam T1 PiecewisePoly::row_t
   * @tparam T2 PiecewisePoly::matrix_t
   * @param breakPoints range breakpoints of shape 1xN
   * @param coefList polynomial coefficients of shape DxN
   * @param extrap extrapolation method [CLAMP/LINEAR/YOLO]
   * @param search search method [LINEAR_FORWARD/LINEAR_REVERSE/LINEAR_HINT/BINARY]
   *
   * The polynomial is s.t.
   *
   * p(x) = p_i(t - breakPoints[i])   if breakPoints[i] <= t < breakPoints[i+1]
   *
   *   where p_i is the polynimial
   *
   * p_i(t) = coefList[0, i] + coefList[1, i] * t + ... + coefList[D-1, i] * t^{D-1}
   *
   * For values outside of the range [breakPoints[0], breakPoints[breakPoints.size() - 1]]
   * the value is extrapolated using the given method.
   */
  template<typename T1, typename T2>
  PiecewisePoly(
    T1 && breakPoints,
    T2 && coefList,
    EXTRAP extrap = EXTRAP::CLAMP,
    SEARCH search = SEARCH::LINEAR_HINT)
  : pp_details::PiecewisePolyBase(
      std::forward<T1>(breakPoints),
      extrap,
      search),
    coefList_{std::forward<T2>(coefList)},
    order_{static_cast<std::size_t>(coefList_.rows())}
  {
    if (breakPoints_.cols() != (coefList_.cols() + 1)) {
      throw std::invalid_argument(
              "PiecewisePoly coefficient matrix must have "
              "one less column that the number of break points.");
    }

    if (coefList_.rows() < 1) {
      throw std::invalid_argument("PiecewisePoly coefficient matrix must have a least 1 row.");
    }

    extremities_.val0 = evalPoly<double>(breakPoints_[0], 0, coefList_, order_);
    extremities_.der0 = evalPolyDer<double>(breakPoints_[0], 0, 1, coefList_, order_);
    extremities_.valEnd = evalPoly<double>(
      breakPoints_[static_cast<Eigen::Index>(length_ - 1)], length_ - 2, coefList_, order_);
    extremities_.derEnd = evalPolyDer<double>(
      breakPoints_[static_cast<Eigen::Index>(length_ - 1)], length_ - 2, 1, coefList_,
      order_);
  }

  template<typename T = double>
  T val(const T x) const
  {
    const auto [pieceIdx, flag] = findPosition(static_cast<double>(x));
    if (flag == 0) {
      return evalPoly<T>(x, pieceIdx, coefList_, order_);
    }
    return extrapPoly<T>(flag, x, 0, coefList_, order_, extremities_);
  }

  template<typename Derived>
  auto vals(const Eigen::MatrixBase<Derived> & xs) const
  {
    Eigen::Matrix<typename Derived::Scalar, Derived::RowsAtCompileTime,
      Derived::ColsAtCompileTime> out(xs.rows(), xs.cols());
    for (Eigen::Index i = 0; i < xs.rows(); i++) {
      for (Eigen::Index j = 0; j < xs.cols(); j++) {
        out(i, j) = val<typename Derived::Scalar>(xs(i, j));
      }
    }
    return out;
  }

  template<typename T, std::size_t N>
  auto vals(const std::array<T, N> & xs) const
  {
    static_assert(N > 0, "Input must be of size > 0.");
    std::array<T, N> out;
    for (size_t i = 0; i < N; i++) {
      out[i] = val<T>(xs[i]);
    }
    return out;
  }

  template<typename T>
  auto vals(const std::vector<T> & xs) const
  {
    std::vector<T> out;
    out.reserve(xs.size());
    for (const auto & x : xs) {
      out.push_back(val<T>(x));
    }
    return out;
  }

  template<typename T = double>
  T der(const T x, const std::size_t n = 1) const
  {
    const auto [pieceIdx, flag] = findPosition(static_cast<double>(x));
    if (flag == 0) {
      return evalPolyDer<T>(x, pieceIdx, n, coefList_, order_);
    }
    return extrapPoly<T>(flag, x, n, coefList_, order_, extremities_);
  }

  template<typename Derived>
  auto ders(const Eigen::MatrixBase<Derived> & xs, const std::size_t n = 1) const
  {
    Eigen::Matrix<typename Derived::Scalar, Derived::RowsAtCompileTime,
      Derived::ColsAtCompileTime> out(xs.rows(), xs.cols());
    for (Eigen::Index i = 0; i < xs.rows(); i++) {
      for (Eigen::Index j = 0; j < xs.cols(); j++) {
        out(i, j) = der<typename Derived::Scalar>(xs(i, j), n);
      }
    }
    return out;
  }

  template<typename T, std::size_t N>
  auto ders(const std::array<T, N> & xs, const std::size_t n = 1) const
  {
    static_assert(N > 0, "Input must be of size > 0.");

    std::array<T, N> out;
    for (size_t i = 0; i < N; i++) {
      out[i] = der<T>(xs[i], n);
    }
    return out;
  }

  template<typename T>
  auto ders(const std::vector<T> & xs, const std::size_t n = 1) const
  {
    std::vector<T> out;
    out.reserve(xs.size());
    for (const auto & x : xs) {
      out.push_back(der<T>(x, n));
    }
    return out;
  }

  const matrix_t & coeffs() const noexcept
  {
    return coefList_;
  }

  const std::size_t & order() const noexcept
  {
    return order_;
  }

protected:
  matrix_t coefList_{};
  std::size_t order_{};
  Extremities extremities_{};

  template<typename T>
  friend class YAML::convert;
};

/**************************************************************************
 * \brief Class for a scalar piecewise polynomial with multivalued image
 **************************************************************************/
class PiecewisePolyND : public pp_details::PiecewisePolyBase
{
public:
  PiecewisePolyND() = delete;
  PiecewisePolyND(const PiecewisePolyND &) = default;
  PiecewisePolyND(PiecewisePolyND &&) = default;
  PiecewisePolyND & operator=(const PiecewisePolyND &) = default;
  PiecewisePolyND & operator=(PiecewisePolyND &&) = default;
  ~PiecewisePolyND() = default;

  /**
   * @brief Vector-valued piecewise polynomial
   *
   * @tparam T1 PiecewisePoly::row_t
   * @tparam T2 container_t<PiecewisePoly::matrix_t>
   * @param breakPoints range breakpoints of shape 1xN
   * @param coefLists polynomial coefficients of shape MxDxN
   * @param extrap extrapolation method [CLAMP/LINEAR/YOLO]
   * @param search search method [LINEAR_FORWARD/LINEAR_REVERSE/LINEAR_HINT/BINARY]
   *
   * The polynomial is s.t.
   *
   * p(x) = p_i(t - breakPoints[i])   if breakPoints[i] <= t < breakPoints[i+1]
   *
   *   where p_i is the polynimial
   *
   * p_i[m](t) = coefList[m, 0, i] + coefList[m, 1, i] * t + ... + coefList[m, D-1, i] * t^{D-1}
   *
   * For values outside of the range [breakPoints[0], breakPoints[breakPoints.size() - 1]]
   * the value is extrapolated using the given method.
   */
  template<typename T1, typename T2>
  PiecewisePolyND(
    T1 && breakPoints,
    T2 && coefLists,
    EXTRAP extrap = EXTRAP::CLAMP,
    SEARCH search = SEARCH::LINEAR_HINT)
  : pp_details::PiecewisePolyBase(
      std::forward<T1>(breakPoints),
      extrap,
      search),
    coefLists_{std::forward<T2>(coefLists)},
    N_{coefLists_.size()}
  {
    if (N_ < 1) {
      throw std::invalid_argument("coefList size must be > 0.");
    }

    orders_.reserve(N_);
    extremities_.reserve(N_);

    for (const auto & coefList : coefLists_) {
      if (breakPoints_.cols() != (coefList.cols() + 1)) {
        throw std::invalid_argument(
                "PiecewisePoly coefficient matrix must have "
                "one less column that the number of break points.");
      }

      if (coefList.rows() < 1) {
        throw std::invalid_argument("PiecewisePoly coefficient matrix must have a least 1 row.");
      }

      orders_.push_back(static_cast<std::size_t>(coefList.rows()));

      extremities_.emplace_back(
        evalPoly<double>(breakPoints_[0], 0, coefList, orders_.back()),
        evalPolyDer<double>(breakPoints_[0], 0, 1, coefList, orders_.back()),
        evalPoly<double>(
          breakPoints_[static_cast<Eigen::Index>(length_ - 1)], length_ - 2,
          coefList, orders_.back()),
        evalPolyDer<double>(
          breakPoints_[static_cast<Eigen::Index>(length_ - 1)], length_ - 2, 1,
          coefList, orders_.back())
      );
    }
  }

  template<typename T = double>
  col_t<T> val(const T x) const
  {
    col_t<T> out(N_);

    const auto [pieceIdx, flag] = findPosition(static_cast<double>(x));
    if (flag == 0) {
      for (std::size_t i = 0; i < N_; i++) {
        out[static_cast<Eigen::Index>(i)] = evalPoly<T>(x, pieceIdx, coefLists_[i], orders_[i]);
      }
    } else {
      for (std::size_t i = 0; i < N_; i++) {
        out[static_cast<Eigen::Index>(i)] = extrapPoly<T>(
          flag, x, 0, coefLists_[i], orders_[i],
          extremities_[i]);
      }
    }

    return out;
  }

  template<typename Derived>
  auto vals(const Eigen::MatrixBase<Derived> & xs) const
  {
    static_assert(Derived::IsVectorAtCompileTime, "Input must be a vector.");

    Eigen::Matrix<typename Derived::Scalar,
      Eigen::Dynamic, Derived::SizeAtCompileTime> out(N_, xs.size());

    for (Eigen::Index i = 0; i < xs.size(); i++) {
      out.col(i) = val<typename Derived::Scalar>(xs[i]);
    }

    return out;
  }

  template<typename T, std::size_t N>
  auto vals(const std::array<T, N> & xs) const
  {
    static_assert(N > 0, "Input must be of size > 0.");
    std::array<col_t<T>, N> out;
    for (size_t i = 0; i < N; i++) {
      out[i] = val<T>(xs[i]);
    }
    return out;
  }

  template<typename T>
  auto vals(const std::vector<T> & xs) const
  {
    std::vector<col_t<T>> out;
    out.reserve(xs.size());
    for (const auto & x : xs) {
      out.push_back(val<T>(x));
    }
    return out;
  }

  template<typename T = double>
  col_t<T> der(const T x, const std::size_t n = 1) const
  {
    col_t<T> out(N_);

    const auto [pieceIdx, flag] = findPosition(static_cast<double>(x));
    if (flag == 0) {
      for (std::size_t i = 0; i < N_; i++) {
        out[static_cast<Eigen::Index>(i)] =
          evalPolyDer<T>(x, pieceIdx, n, coefLists_[i], orders_[i]);
      }
    } else {
      for (std::size_t i = 0; i < N_; i++) {
        out[static_cast<Eigen::Index>(i)] = extrapPoly<T>(
          flag, x, n, coefLists_[i], orders_[i],
          extremities_[i]);
      }
    }

    return out;
  }

  template<typename Derived>
  auto ders(const Eigen::MatrixBase<Derived> & xs, const std::size_t n = 1) const
  {
    static_assert(Derived::IsVectorAtCompileTime, "Input must be a vector.");

    Eigen::Matrix<typename Derived::Scalar,
      Eigen::Dynamic, Derived::SizeAtCompileTime> out(N_, xs.size());

    for (Eigen::Index i = 0; i < xs.size(); i++) {
      out.col(i) = der<typename Derived::Scalar>(xs[i], n);
    }

    return out;
  }

  template<typename T, std::size_t N>
  auto ders(const std::array<T, N> & xs, const std::size_t n = 1) const
  {
    static_assert(N > 0, "Input must be of size > 0.");

    std::array<col_t<T>, N> out;
    for (std::size_t i = 0; i < N; i++) {
      out[i] = der<T>(xs[i], n);
    }
    return out;
  }

  template<typename T>
  auto ders(const std::vector<T> & xs, const std::size_t n = 1) const
  {
    std::vector<col_t<T>> out;
    out.reserve(xs.size());
    for (const auto & x : xs) {
      out.push_back(der<T>(x, n));
    }
    return out;
  }

  const std::size_t & dimension() const noexcept
  {
    return N_;
  }

  const matrix_t & coeffs(const std::size_t i) const
  {
    if (i >= N_) {
      throw std::out_of_range(
              "coeffs was call with an index greater than the dimension of the polynomial.");
    }
    return coefLists_[i];
  }

  const std::size_t & order(const std::size_t i) const
  {
    if (i <= N_) {
      throw std::out_of_range(
              "getCoeffs was call with an index greater than the dimension of the polynomial.");
    }
    return orders_[i];
  }

protected:
  std::vector<matrix_t> coefLists_;
  std::size_t N_ = 0;
  std::vector<std::size_t> orders_;
  std::vector<Extremities> extremities_;
};

}  // namespace cbr

#endif  // CBR_MATH__INTERP__PIECEWISE_POLY_HPP_


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <boost/simd/include/constants/digits.hpp>
#include <boost/simd/include/functions/splat.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/simd/sdk/simd/io.hpp>

#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>

#include "../constant.hpp"

NT2_TEST_CASE_TPL( digit_value, BOOST_SIMD_SIMD_TYPES )
{
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef boost::simd::native<T,ext_t>  n_t;

  NT2_CHECK_CONSTANT(Mten     , -10  , n_t);
  NT2_CHECK_CONSTANT(Mnine    , -9   , n_t);
  NT2_CHECK_CONSTANT(Meight   , -8   , n_t);
  NT2_CHECK_CONSTANT(Mseven   , -7   , n_t);
  NT2_CHECK_CONSTANT(Msix     , -6   , n_t);
  NT2_CHECK_CONSTANT(Mfive    , -5   , n_t);
  NT2_CHECK_CONSTANT(Mfour    , -4   , n_t);
  NT2_CHECK_CONSTANT(Mthree   , -3   , n_t);
  NT2_CHECK_CONSTANT(Mtwo     , -2   , n_t);
  NT2_CHECK_CONSTANT(Mone     , -1   , n_t);
  NT2_CHECK_CONSTANT(Zero     ,  0   , n_t);
  NT2_CHECK_CONSTANT(One      ,  1   , n_t);
  NT2_CHECK_CONSTANT(Two      ,  2   , n_t);
  NT2_CHECK_CONSTANT(Three    ,  3   , n_t);
  NT2_CHECK_CONSTANT(Four     ,  4   , n_t);
  NT2_CHECK_CONSTANT(Five     ,  5   , n_t);
  NT2_CHECK_CONSTANT(Six      ,  6   , n_t);
  NT2_CHECK_CONSTANT(Seven    ,  7   , n_t);
  NT2_CHECK_CONSTANT(Eight    ,  8   , n_t);
  NT2_CHECK_CONSTANT(Nine     ,  9   , n_t);
  NT2_CHECK_CONSTANT(Ten      , 10   , n_t);
  NT2_CHECK_CONSTANT(Eleven   , 11   , n_t);
  NT2_CHECK_CONSTANT(Twelve   , 12   , n_t);
  NT2_CHECK_CONSTANT(Fifteen  , 15   , n_t);
  NT2_CHECK_CONSTANT(Twenty   , 20   , n_t);
  NT2_CHECK_CONSTANT(Hundred  , 100  , n_t);
  NT2_CHECK_CONSTANT(Thousand , 1000 , n_t);
}

NT2_TEST_CASE_TPL( fact_value, BOOST_SIMD_SIMD_TYPES )
{
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef boost::simd::native<T,ext_t>  n_t;

  NT2_CHECK_CONSTANT(Fact_4    ,        24, n_t);
  NT2_CHECK_CONSTANT(Fact_5    ,       120, n_t);
  NT2_CHECK_CONSTANT(Fact_6    ,       720, n_t);
  NT2_CHECK_CONSTANT(Fact_7    ,      5040, n_t);
  NT2_CHECK_CONSTANT(Fact_8    ,     40320, n_t);
  NT2_CHECK_CONSTANT(Fact_9    ,    362880, n_t);
  NT2_CHECK_CONSTANT(Fact_10   ,   3628800, n_t);
  NT2_CHECK_CONSTANT(Fact_11   ,  39916800, n_t);
  NT2_CHECK_CONSTANT(Fact_12   , 479001600, n_t);
}

NT2_TEST_CASE_TPL( integral_constant, BOOST_SIMD_SIMD_INTEGRAL_TYPES )
{
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef boost::simd::native<T,ext_t>  n_t;

  NT2_TEST_EXPR_TYPE( (boost::simd::integral_constant<n_t,42>())
                    , boost::mpl::_
                    , n_t
                    );

  NT2_TEST_EQUAL( (boost::simd::integral_constant<n_t,42>())
                , boost::simd::splat<n_t>(42)
                );
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

// #define MTL_VERBOSE_TEST  // to turn on the output

#include <iostream>
#include <cmath>
#include <boost/numeric/mtl/mtl.hpp>

using namespace std;
using namespace mtl;

template <typename Matrix>
void test(Matrix& A, const char* name)
{
    A= 1, 2, 3,
       4, 5, 6,
       7, 8, 9;
    mtl::io::tout << "\n" << name << "  --- A = \n" << A;

    typename mtl::ColumnInMatrix<Matrix>::type c0(A[iall][0]);
    c0[2]= 10.0;
    MTL_THROW_IF(A[2][0] != 10.0, mtl::runtime_error("Matrix modification in column did not work"));

    typename mtl::ColumnInMatrix<Matrix>::type c1(clone(A[iall][1]));
    c1[2]= 11.0;
    MTL_THROW_IF(A[2][1] != 8.0, mtl::runtime_error("Matrix modied by a cloned column"));

    dense_vector<float> c2(clone(A[iall][2]));
    c2[2]= 11.5;
    MTL_THROW_IF(A[2][2] != 9.0, mtl::runtime_error("Matrix modied by a cloned column"));

    typename mtl::RowInMatrix<Matrix>::type r0(A[0][iall]);
    r0[2]= 12.0;
    MTL_THROW_IF(A[0][2] != 12.0, mtl::runtime_error("Matrix modification in row did not work"));

    typename mtl::RowInMatrix<Matrix>::type r1(clone(A[1][iall]));
    r1[2]= 13.0;
    MTL_THROW_IF(A[1][2] != 6.0, mtl::runtime_error("Matrix modied by a cloned row"));

    dense_vector<float, mtl::vec::parameters<row_major> > r2(clone(A[2][iall]));
    r2[2]= 13.5;
    MTL_THROW_IF(A[2][2] != 9.0, mtl::runtime_error("Matrix modied by a cloned row"));

    mtl::io::tout << "A = \n" << A << std::endl;
}

int main(int, char**)
{
    using namespace mtl;

    dense2D<float>                                 A(3, 3);
    dense2D<float, mat::parameters<col_major> > B(3, 3);

    test(A, "Row-major matrix   ");
    test(B, "Column-major matrix");

    return 0;
}


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2018-2020, LAAS-CNRS, University of Edinburgh
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#ifndef CROCODDYL_CORE_ACTION_BASE_HPP_
#define CROCODDYL_CORE_ACTION_BASE_HPP_

#include <stdexcept>
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>

#include "crocoddyl/core/fwd.hpp"
#include "crocoddyl/core/state-base.hpp"
#include "crocoddyl/core/utils/math.hpp"
#include "crocoddyl/core/utils/to-string.hpp"

namespace crocoddyl {

template <typename _Scalar>
class ActionModelAbstractTpl {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  typedef _Scalar Scalar;
  typedef MathBaseTpl<Scalar> MathBase;
  typedef ActionDataAbstractTpl<Scalar> ActionDataAbstract;
  typedef StateAbstractTpl<Scalar> StateAbstract;
  typedef typename MathBase::VectorXs VectorXs;

  ActionModelAbstractTpl(boost::shared_ptr<StateAbstract> state, const std::size_t& nu, const std::size_t& nr = 0);

  virtual ~ActionModelAbstractTpl();

  virtual void calc(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
                    const Eigen::Ref<const VectorXs>& u) = 0;
  virtual void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
                        const Eigen::Ref<const VectorXs>& u) = 0;
  virtual boost::shared_ptr<ActionDataAbstract> createData();
  virtual bool checkData(const boost::shared_ptr<ActionDataAbstract>& data);

  void calc(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
  void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);

  /**
   * @brief Computes the quasic static commands
   *
   * The quasic static commands are the ones produced for a the reference posture as an equilibrium point, i.e.
   * for \f$\mathbf{f(q,v=0,u)=0}\f$
   *
   * @param[in] data    Differential action data
   * @param[out] u      Quasic static commands
   * @param[in] x       State point (velocity has to be zero)
   * @param[in] maxiter Maximum allowed number of iterations
   * @param[in] tol     Tolerance
   */
  virtual void quasiStatic(const boost::shared_ptr<ActionDataAbstract>& data, Eigen::Ref<VectorXs> u,
                           const Eigen::Ref<const VectorXs>& x, const std::size_t& maxiter = 100,
                           const Scalar& tol = Scalar(1e-9));

  /**
   * @copybrief quasicStatic()
   *
   * @copydetails quasicStatic()
   *
   * @param[in] data    Differential action data
   * @param[in] x       State point (velocity has to be zero)
   * @param[in] maxiter Maximum allowed number of iterations
   * @param[in] tol     Tolerance
   * @return Quasic static commands
   */
  VectorXs quasiStatic_x(const boost::shared_ptr<ActionDataAbstract>& data, const VectorXs& x,
                         const std::size_t& maxiter = 100, const Scalar& tol = Scalar(1e-9));

  const std::size_t& get_nu() const;
  const std::size_t& get_nr() const;
  const boost::shared_ptr<StateAbstract>& get_state() const;

  const VectorXs& get_u_lb() const;
  const VectorXs& get_u_ub() const;
  bool const& get_has_control_limits() const;

  void set_u_lb(const VectorXs& u_lb);
  void set_u_ub(const VectorXs& u_ub);

 protected:
  std::size_t nu_;                          //!< Control dimension
  std::size_t nr_;                          //!< Dimension of the cost residual
  boost::shared_ptr<StateAbstract> state_;  //!< Model of the state
  VectorXs unone_;                          //!< Neutral state
  VectorXs u_lb_;                           //!< Lower control limits
  VectorXs u_ub_;                           //!< Upper control limits
  bool has_control_limits_;                 //!< Indicates whether any of the control limits is finite

  void update_has_control_limits();
};

template <typename _Scalar>
struct ActionDataAbstractTpl {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  typedef _Scalar Scalar;
  typedef MathBaseTpl<Scalar> MathBase;

  template <template <typename Scalar> class Model>
  explicit ActionDataAbstractTpl(Model<Scalar>* const model)
      : cost(Scalar(0.)),
        xnext(model->get_state()->get_nx()),
        r(model->get_nr()),
        Fx(model->get_state()->get_ndx(), model->get_state()->get_ndx()),
        Fu(model->get_state()->get_ndx(), model->get_nu()),
        Lx(model->get_state()->get_ndx()),
        Lu(model->get_nu()),
        Lxx(model->get_state()->get_ndx(), model->get_state()->get_ndx()),
        Lxu(model->get_state()->get_ndx(), model->get_nu()),
        Luu(model->get_nu(), model->get_nu()) {
    xnext.setZero();
    r.setZero();
    Fx.setZero();
    Fu.setZero();
    Lx.setZero();
    Lu.setZero();
    Lxx.setZero();
    Lxu.setZero();
    Luu.setZero();
  }
  virtual ~ActionDataAbstractTpl() {}

  Scalar cost;
  typename MathBase::VectorXs xnext;
  typename MathBase::VectorXs r;
  typename MathBase::MatrixXs Fx;
  typename MathBase::MatrixXs Fu;
  typename MathBase::VectorXs Lx;
  typename MathBase::VectorXs Lu;
  typename MathBase::MatrixXs Lxx;
  typename MathBase::MatrixXs Lxu;
  typename MathBase::MatrixXs Luu;
};

}  // namespace crocoddyl

/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
#include "crocoddyl/core/action-base.hxx"

#endif  // CROCODDYL_CORE_ACTION_BASE_HPP_


// Copyright (c) 2015-2017 The Bitcoin Core developers
// Copyright (c) 2018 Equibit Group AG
// Distributed under the MIT/X11 software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include <chain.h>
#include <chainparams.h>
#include <pow.h>
#include <random.h>
#include <util.h>
#include <test/test_bitcoin.h>

#include <boost/test/unit_test.hpp>

BOOST_FIXTURE_TEST_SUITE(pow_tests, BasicTestingSetup)

#ifdef BUILD_BTC

/* Test calculation of next difficulty target with no constraints applying */
BOOST_AUTO_TEST_CASE(get_next_work)
{
    const auto chainParams = CreateChainParams(CBaseChainParams::MAIN);
    int64_t nLastRetargetTime = 1261130161; // Block #30240
    CBlockIndex pindexLast;
    pindexLast.nHeight = 32255;
    pindexLast.nTime = 1262152739; // Block #32255
    pindexLast.nBits = 0x1d00ffff;
    BOOST_CHECK_EQUAL(CalculateNextWorkRequired(&pindexLast, nLastRetargetTime, chainParams->GetConsensus()), 0x1d00d86a);
}

/* Test the constraint on the upper bound for next work */
BOOST_AUTO_TEST_CASE(get_next_work_pow_limit)
{
    const auto chainParams = CreateChainParams(CBaseChainParams::MAIN);
    int64_t nLastRetargetTime = 1231006505; // Block #0
    CBlockIndex pindexLast;
    pindexLast.nHeight = 2015;
    pindexLast.nTime = 1233061996; // Block #2015
    pindexLast.nBits = 0x1d00ffff;
    BOOST_CHECK_EQUAL(CalculateNextWorkRequired(&pindexLast, nLastRetargetTime, chainParams->GetConsensus()), 0x1d00ffff);
}

/* Test the constraint on the lower bound for actual time taken */
BOOST_AUTO_TEST_CASE(get_next_work_lower_limit_actual)
{
    const auto chainParams = CreateChainParams(CBaseChainParams::MAIN);
    int64_t nLastRetargetTime = 1279008237; // Block #66528
    CBlockIndex pindexLast;
    pindexLast.nHeight = 68543;
    pindexLast.nTime = 1279297671; // Block #68543
    pindexLast.nBits = 0x1c05a3f4;
    BOOST_CHECK_EQUAL(CalculateNextWorkRequired(&pindexLast, nLastRetargetTime, chainParams->GetConsensus()), 0x1c0168fd);
}

/* Test the constraint on the upper bound for actual time taken */
BOOST_AUTO_TEST_CASE(get_next_work_upper_limit_actual)
{
    const auto chainParams = CreateChainParams(CBaseChainParams::MAIN);
    int64_t nLastRetargetTime = 1263163443; // NOTE: Not an actual block time
    CBlockIndex pindexLast;
    pindexLast.nHeight = 46367;
    pindexLast.nTime = 1269211443; // Block #46367
    pindexLast.nBits = 0x1c387f6f;
    BOOST_CHECK_EQUAL(CalculateNextWorkRequired(&pindexLast, nLastRetargetTime, chainParams->GetConsensus()), 0x1d00e1fd);
}

#else // BUILD_TESR

static int64_t FirstBlockTime = 1000000000; // arbitrary start time

static uint256 StandardPowLimit = uint256S("00000000ffffffffffffffffffffffffffffffffffffffffffffffffffffffff"); // From CMainParams
static uint256 TestPowTarget = uint256S("0000000000000000ffffffffffffffffffffffffffffffffffffffffffffffff");

static int64_t nPowTargetTimespanBTC = 14 * 24 * 60 * 60; // two weeks
static int64_t nPowTargetTimespanDWG = 24 * 60 * 60;      // 1 day

// We can't expect to get exact values for calculated nBits because the compact format uses 32 bits to store 256 bits
// so instead we'll test for something close here differing by less than 1 part in a million.
// https://bitcoin.org/en/developer-reference#target-nbits
const double DiffThreshold = 1.e-6;

BOOST_AUTO_TEST_CASE(get_next_work_10_percent)
{
    int64_t nLastBlockTime = FirstBlockTime + nPowTargetTimespanDWG / 10;

    arith_uint256 bnPowInitial = UintToArith256(TestPowTarget);
    arith_uint256 bnPowExpected = UintToArith256(TestPowTarget) / 4; // Everything below 1/4 should be treated like 1/4
    uint32_t nBitsExpected = bnPowExpected.GetCompact();

    uint32_t nBitsNew = CalculateNextWorkRequired(bnPowInitial.GetCompact(), StandardPowLimit, FirstBlockTime, nLastBlockTime, nPowTargetTimespanDWG);
    double bitsDiff = fabs(log(1.0 * nBitsNew / nBitsExpected));

    BOOST_CHECK_LT(bitsDiff, DiffThreshold);
}

BOOST_AUTO_TEST_CASE(get_next_work_30_percent)
{
    int64_t nLastBlockTime = FirstBlockTime + nPowTargetTimespanDWG * 3 / 10;

    arith_uint256 bnPowInitial = UintToArith256(TestPowTarget);
    arith_uint256 bnPowExpected = UintToArith256(TestPowTarget) * 3 / 10;
    uint32_t nBitsExpected = bnPowExpected.GetCompact();

    uint32_t nBitsNew = CalculateNextWorkRequired(bnPowInitial.GetCompact(), StandardPowLimit, FirstBlockTime, nLastBlockTime, nPowTargetTimespanDWG);
    double bitsDiff = fabs(log(1.0 * nBitsNew / nBitsExpected));

    BOOST_CHECK_LT(bitsDiff, DiffThreshold);
}

BOOST_AUTO_TEST_CASE(get_next_work_80_percent)
{
    int64_t nLastBlockTime = FirstBlockTime + nPowTargetTimespanDWG * 4 / 5;

    arith_uint256 bnPowInitial = UintToArith256(TestPowTarget);
    arith_uint256 bnPowExpected = UintToArith256(TestPowTarget) * 4 / 5;
    uint32_t nBitsExpected = bnPowExpected.GetCompact();

    uint32_t nBitsNew = CalculateNextWorkRequired(bnPowInitial.GetCompact(), StandardPowLimit, FirstBlockTime, nLastBlockTime, nPowTargetTimespanDWG);
    double bitsDiff = fabs(log(1.0 * nBitsNew / nBitsExpected));

    BOOST_CHECK_LT(bitsDiff, DiffThreshold);
}

BOOST_AUTO_TEST_CASE(get_next_work_125_percent)
{
    int64_t nLastBlockTime = FirstBlockTime + nPowTargetTimespanDWG * 5 / 4;

    arith_uint256 bnPowInitial = UintToArith256(TestPowTarget);
    arith_uint256 bnPowExpected = UintToArith256(TestPowTarget) * 5 / 4;
    uint32_t nBitsExpected = bnPowExpected.GetCompact();

    uint32_t nBitsNew = CalculateNextWorkRequired(bnPowInitial.GetCompact(), StandardPowLimit, FirstBlockTime, nLastBlockTime, nPowTargetTimespanDWG);
    double bitsDiff = fabs(log(1.0 * nBitsNew / nBitsExpected));

    BOOST_CHECK_LT(bitsDiff, DiffThreshold);
}

BOOST_AUTO_TEST_CASE(get_next_work_300_percent)
{
    int64_t nLastBlockTime = FirstBlockTime + nPowTargetTimespanDWG * 3;

    arith_uint256 bnPowInitial = UintToArith256(TestPowTarget);
    arith_uint256 bnPowExpected = UintToArith256(TestPowTarget) * 3;
    uint32_t nBitsExpected = bnPowExpected.GetCompact();

    uint32_t nBitsNew = CalculateNextWorkRequired(bnPowInitial.GetCompact(), StandardPowLimit, FirstBlockTime, nLastBlockTime, nPowTargetTimespanDWG);
    double bitsDiff = fabs(log(1.0 * nBitsNew / nBitsExpected));

    BOOST_CHECK_LT(bitsDiff, DiffThreshold);
}

BOOST_AUTO_TEST_CASE(get_next_work_500_percent)
{
    int64_t nLastBlockTime = FirstBlockTime + nPowTargetTimespanDWG * 5;

    arith_uint256 bnPowInitial = UintToArith256(TestPowTarget);
    arith_uint256 bnPowExpected = UintToArith256(TestPowTarget) * 4; // Everything above 4x should be treated like 4x
    uint32_t nBitsExpected = bnPowExpected.GetCompact();

    uint32_t nBitsNew = CalculateNextWorkRequired(bnPowInitial.GetCompact(), StandardPowLimit, FirstBlockTime, nLastBlockTime, nPowTargetTimespanDWG);
    double bitsDiff = fabs(log(1.0 * nBitsNew / nBitsExpected));

    BOOST_CHECK_LT(bitsDiff, DiffThreshold);
}

// Initialize simulated blockchain
static int64_t InitializeBlocks(std::vector<CBlockIndex>& blocks, const Consensus::Params& params)
{
    const int64_t nInterval = 2 * params.DifficultyAdjustmentInterval();
    const uint32_t nProofOfWorkLimit = UintToArith256(params.powLimit).GetCompact();

    assert(blocks.size() > static_cast<size_t>(nInterval));

    for (auto i = 0; i < nInterval; i++) {
        blocks[i].pprev = i ? &blocks[i - 1] : nullptr;
        blocks[i].nHeight = i;
        blocks[i].nTime = i ? blocks[i - 1].nTime + params.nPowTargetSpacing : 1269211443; // Arbitrary start time copied from GetBlockProofEquivalentTime_test
        blocks[i].nBits = nProofOfWorkLimit;
        blocks[i].nChainWork = i ? blocks[i - 1].nChainWork + GetBlockProof(blocks[i - 1]) : arith_uint256(0);
    }

    return nInterval;
}

// Difficulty should remain constant when the block time interval is exactly the target interval
BOOST_AUTO_TEST_CASE(GetNextWorkRequired_steady_state)
{
    const auto chainParams = CreateChainParams(CBaseChainParams::MAIN);
    const Consensus::Params& params = chainParams->GetConsensus();

    CBlockHeader header;
    const uint64_t nBlocks = 1000;
    std::vector<CBlockIndex> blocks(nBlocks);

    uint64_t nBlocksInitialized = InitializeBlocks(blocks, params);

    for (uint64_t i = nBlocksInitialized; i < nBlocks; i++) {
        header.nVersion = 1;
        header.nTime = blocks[i - 1].nTime + params.nPowTargetSpacing;

        blocks[i].pprev = &blocks[i - 1];
        blocks[i].nHeight = i;
        blocks[i].nTime = header.nTime;
        blocks[i].nBits = GetNextWorkRequired(blocks[i].pprev, &header, params);
        blocks[i].nChainWork = blocks[i - 1].nChainWork + GetBlockProof(blocks[i - 1]);

        //std::cout << i << " " << std::hex << blocks[i].nBits << " " << blocks[i - 1].nBits << std::endl;
        double bitsDiff = fabs(log(1.0 * blocks[i].nBits / blocks[i - 1].nBits));

        BOOST_CHECK_LT(bitsDiff, DiffThreshold);

    }
}

// Simulate a blockchain to check that the difficulty changes in expected ways with the block times
BOOST_AUTO_TEST_CASE(GetNextWorkRequired_changing)
{
    const auto chainParams = CreateChainParams(CBaseChainParams::MAIN);
    const Consensus::Params& params = chainParams->GetConsensus();
    const unsigned int nProofOfWorkLimit = UintToArith256(params.powLimit).GetCompact();

    CBlockHeader header;
    const uint64_t nBlocks = 1000;
    std::vector<CBlockIndex> blocks(nBlocks);

    uint64_t nBlocksInitialized = InitializeBlocks(blocks, params);

    for (uint64_t i = nBlocksInitialized; i < nBlocks; i++) {
        uint32_t blockTime = params.nPowTargetSpacing;

        // In the first phase we'll simulate short block times to increase difficulty
        // and second phase we'll simulate long block times to decrease difficulty.
        if (i < nBlocks / 2) {
            blockTime /= 2;
        } else {
            blockTime *= 2;
        }

        header.nVersion = 1;
        header.nTime = blocks[i - 1].nTime + blockTime;

        blocks[i].pprev = &blocks[i - 1];
        blocks[i].nHeight = i;
        blocks[i].nTime = header.nTime;
        blocks[i].nBits = GetNextWorkRequired(blocks[i].pprev, &header, params);
        blocks[i].nChainWork = blocks[i - 1].nChainWork + GetBlockProof(blocks[i - 1]);

        int diff = blocks[i].nBits - blocks[i - 1].nBits;

        // std::cout << i << " " << blocks[i].nBits << " " << diff << std::endl;

        // Ensure that if the difficulty hasn't changed then we're already at the limit,
        // otherwise ensure that difficulty is increasing (nBits decreasing) for the first phase
        // and vice versa for the second phase.
        if (diff == 0) {
            BOOST_CHECK_EQUAL(blocks[i].nBits, nProofOfWorkLimit);
        } else {
            if (i <= nBlocks / 2) {
                BOOST_CHECK_LT(diff, 0);
            } else {
                BOOST_CHECK_GT(diff, 0);
            }
        }
    }
}

// BTC legacy tests below, functionally idential to the ones in the BUILD_BTC conditional compilation block above
// but updated with the new function signature.

/* Test calculation of next difficulty target with no constraints applying */
BOOST_AUTO_TEST_CASE(get_next_work_btc)
{
    int64_t nFirstBlockTime = 1261130161; // Block #30240
    int64_t nLastBlockTime = 1262152739;  // Block #32255

    BOOST_CHECK_EQUAL(CalculateNextWorkRequired(0x1d00ffff, StandardPowLimit, nFirstBlockTime, nLastBlockTime, nPowTargetTimespanBTC), 0x1d00d86a);
}

/* Test the constraint on the upper bound for next work */
BOOST_AUTO_TEST_CASE(get_next_work_pow_limit_btc)
{
    int64_t nFirstBlockTime = 1231006505; // Block #0
    int64_t nLastBlockTime = 1233061996;  // Block #2015

    BOOST_CHECK_EQUAL(CalculateNextWorkRequired(0x1d00ffff, StandardPowLimit, nFirstBlockTime, nLastBlockTime, nPowTargetTimespanBTC), 0x1d00ffff);
}

/* Test the constraint on the lower bound for actual time taken */
BOOST_AUTO_TEST_CASE(get_next_work_lower_limit_actual_btc)
{
    int64_t nFirstBlockTime = 1279008237; // Block #66528
    int64_t nLastBlockTime = 1279297671;  // Block #68543

    //BOOST_CHECK_EQUAL(CalculateNextWorkRequired(0x1c05a3f4, StandardPowLimit, nFirstBlockTime, nLastBlockTime, nPowTargetTimespanBTC), 0x1c0168fd);
    //
    // A fix in CalculateNextWorkRequired caused an off-by-one error in this unit test:

    BOOST_CHECK_EQUAL(CalculateNextWorkRequired(0x1c05a3f4, StandardPowLimit, nFirstBlockTime, nLastBlockTime, nPowTargetTimespanBTC), 0x1c0168fc);
}

/* Test the constraint on the upper bound for actual time taken */
BOOST_AUTO_TEST_CASE(get_next_work_upper_limit_actual_btc)
{
    int64_t nFirstBlockTime = 1263163443; // NOTE: Not an actual block time
    int64_t nLastBlockTime = 1269211443;  // Block #46367

    BOOST_CHECK_EQUAL(CalculateNextWorkRequired(0x1c387f6f, StandardPowLimit, nFirstBlockTime, nLastBlockTime, nPowTargetTimespanBTC), 0x1d00e1fd);
}

#endif // END_BUILD

BOOST_AUTO_TEST_CASE(GetBlockProofEquivalentTime_test)
{
    const auto chainParams = CreateChainParams(CBaseChainParams::MAIN);
    std::vector<CBlockIndex> blocks(10000);
    for (int i = 0; i < 10000; i++) {
        blocks[i].pprev = i ? &blocks[i - 1] : nullptr;
        blocks[i].nHeight = i;
        blocks[i].nTime = 1269211443 + i * chainParams->GetConsensus().nPowTargetSpacing;
        blocks[i].nBits = 0x207fffff; /* target 0x7fffff000... */
        blocks[i].nChainWork = i ? blocks[i - 1].nChainWork + GetBlockProof(blocks[i - 1]) : arith_uint256(0);
    }

    for (int j = 0; j < 1000; j++) {
        CBlockIndex *p1 = &blocks[InsecureRandRange(10000)];
        CBlockIndex *p2 = &blocks[InsecureRandRange(10000)];
        CBlockIndex *p3 = &blocks[InsecureRandRange(10000)];

        int64_t tdiff = GetBlockProofEquivalentTime(*p1, *p2, *p3, chainParams->GetConsensus());
        BOOST_CHECK_EQUAL(tdiff, p1->GetBlockTime() - p2->GetBlockTime());
    }
}

BOOST_AUTO_TEST_SUITE_END()


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2014, Oracle and/or its affiliates.

// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_IS_VALID_BOX_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_IS_VALID_BOX_HPP

#include <cstddef>

#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/tags.hpp>
#include <boost/geometry/core/coordinate_dimension.hpp>

#include <boost/geometry/algorithms/dispatch/is_valid.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace is_valid
{

template <typename Box, std::size_t I>
struct has_valid_corners
{
    static inline bool apply(Box const& box)
    {
        if ( geometry::get<geometry::max_corner, I-1>(box)
             <=
             geometry::get<geometry::min_corner, I-1>(box) )
        {
            return false;
        }
        return has_valid_corners<Box, I-1>::apply(box);
    }
};

template <typename Box>
struct has_valid_corners<Box, 0>
{
    static inline bool apply(Box const&)
    {
        return true;
    }
};

}} // namespace detail::is_valid
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

// A box is always simple
// A box is a Polygon, and it satisfies the conditions for Polygon validity.
//
// The only thing we have to check is whether the max corner lies in
// the upper-right quadrant as defined by the min corner
//
// Reference (for polygon validity): OGC 06-103r4 (6.1.11.1)
template <typename Box>
struct is_valid<Box, box_tag>
    : detail::is_valid::has_valid_corners<Box, dimension<Box>::value>
{};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_IS_VALID_BOX_HPP


// Boost.Geometry

// Copyright (c) 2019, Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#include "common.hpp"

#include <boost/geometry/algorithms/centroid.hpp>
#include <boost/geometry/algorithms/simplify.hpp>

template <typename G>
inline void simplify(G const& g1, G & g2)
{
    bg::simplify(g1, g2, 1, bg::strategy::simplify::douglas_peucker<geom::point, bg::strategy::distance::projected_point<> >());
    bg::simplify(g1, g2, 1, bg::strategy::simplify::douglas_peucker<geom::point, bg::strategy::distance::cross_track<> >());

    // TODO: douglas_peucker does not define a ctor taking distance strategy
    // which is needed in geographic CS
    bg::simplify(g1, g2, 1, bg::strategy::simplify::douglas_peucker<geom::point, bg::strategy::distance::geographic_cross_track<> >());
}

int test_main(int, char*[])
{
    geom g, o;

    // this compiles but it shouldn't!
    // internally default_strategy is defined as not_applicable_strategy for box
    bg::centroid(g.b, o.pt);

    ::simplify(g.ls, o.ls);
    // TODO:
    //::simplify(g.r, o.r); // area (point order) strategy not propagated
    //::simplify(g.po, o.po); // area (point order) strategy not propagated

    return 0;
}


//
// Created by philipp on 26.12.2019.
//

#ifndef FUNNELS_CPP_DISTANCES_HH
#define FUNNELS_CPP_DISTANCES_HH

#include <cassert>
#include <vector>
#include <Eigen/Core>
#include <cmath>

#ifndef M_PI
  #define M_PI 3.14159265358979323846
#endif

using namespace std;
using namespace Eigen;

double wrap_so2(double x){
  return fmod(x, M_PI);
}

class partial_so2_dist_t{
public:
  partial_so2_dist_t(vector<size_t> so2_dims=vector<size_t>()):
      _so2_dims(move(so2_dims)){};

//  template <class DERIVED0, class DERIVED1>
//  MatrixXd &operator()(const MatrixBase<DERIVED0> &x0,
//      const MatrixBase<DERIVED1> &x1){
//    // Multiple possibilities here
//    // so we have to do proper broadcasting
//    assert(x0.rows()==x1.rows());
//
//    if(x0.cols()==1 && x1.cols()==1){
//      // two vectors
//      _tmpMat = x0-x1;
//    }else if(x0.cols()==1 || x1.cols()==1){
//      if (x0.cols()==1){
//        _tmpMat = (-x1).colwise() + x0;
//      }else{
//        _tmpMat = x1.colwise()-x0;
//      }
//    }else if(x0.cols()==x1.cols()){
//      _tmpMat = x0-x1;
//    }
//

//    return _tmpMat;
//  }

  MatrixXd & cp_vv(const VectorXd & v0, const VectorXd & v1){
    _tmpMat = v0-v1;
    do_so2_wrap();
    return _tmpMat;
  }

  MatrixXd & cp_vM(const VectorXd & v0, const MatrixXd & m1){
    _tmpMat = (-m1).colwise() + v0;
    do_so2_wrap();
    return _tmpMat;
  }

  MatrixXd & cp_Mv(const MatrixXd & m0, const VectorXd & v1){
    _tmpMat = m0.colwise() - v1;
    do_so2_wrap();
    return _tmpMat;
  }

  MatrixXd & cp_MM(const MatrixXd & m0, const MatrixXd & m1){
    _tmpMat = m0 - m1;
    do_so2_wrap();
    return _tmpMat;
  }

protected:

  inline void do_so2_wrap(){
    // Take into account the angles
    for (size_t idx : _so2_dims){
      _tmpMat.col(idx).unaryExpr(&wrap_so2);
    }
  }

  const vector<size_t> _so2_dims;
  MatrixXd _tmpMat;
};

#endif //FUNNELS_CPP_DISTANCES_HH


// ---------------------------------------------------------------------------
//
//  This file is part of PermLib.
//
// Copyright (c) 2009-2011 Thomas Rehn <thomas@carmen76.de>
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
// 3. The name of the author may not be used to endorse or promote products
//    derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
// OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
// IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
// NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
// THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// ---------------------------------------------------------------------------

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE permutation test
#include <boost/test/unit_test.hpp>

#include <iostream>
#include <fstream>

#include <permlib/permutation.h>
#include <permlib/bsgs.h>
#include <permlib/transversal/schreier_tree_transversal.h>
#include <permlib/export/bsgs_schreier_export.h>

#include "test-common.h"

using namespace permlib;
using namespace permlib::exports;
using namespace permlib::test;

void testGroup(const GroupInformation& info) {
	typedef Permutation PERM;
	typedef SchreierTreeTransversal<PERM> TRANSVERSAL;

	BSGS<PERM,TRANSVERSAL> bsgs = construct<PERM,TRANSVERSAL>(info);

	BSGSSchreierExport exporter;
	BSGSSchreierImport importer;

	BSGSSchreierData* data = exporter.exportData(bsgs);

	BOOST_REQUIRE_EQUAL(bsgs.B.size(), data->baseSize);
	BOOST_REQUIRE_EQUAL(bsgs.S.size(), data->sgsSize);
	BOOST_REQUIRE_EQUAL(bsgs.U.size(), data->baseSize);

	BSGS<PERM,TRANSVERSAL>* bsgsImp = importer.importData(data);

	BOOST_CHECK_EQUAL(bsgs.order(), bsgsImp->order());

	// check that imported transversal is a transversal and that
	// it behaves like the original transversal
	unsigned int i = 0;
	BOOST_FOREACH(const TRANSVERSAL& U, bsgsImp->U) {
		for (unsigned int j = 0; j < U.n(); ++j) {
			boost::scoped_ptr<PERM> u_beta(U.at(j));
			if (u_beta)
				BOOST_CHECK_EQUAL(*u_beta / bsgsImp->B[i], j);
			else
				BOOST_CHECK( ! bsgs.U[i].at(j) );
		}
		++i;
	}

	delete bsgsImp;
	delete data;
}

BOOST_AUTO_TEST_CASE( test_export )
{
  testGroup(info_1997);
	testGroup(info_cov1075);
	testGroup(info_cyclic37_2);
	testGroup(info_S5wrS3);
	testGroup(info_S6);
}

BOOST_AUTO_TEST_CASE( test_export_empty )
{
  BSGSSchreierData* data = new BSGSSchreierData();
  data->n = 2;
  delete data;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// QuickBook Example

// Copyright (c) 2015, Oracle and/or its affiliates

// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

//[is_valid_message
//` Checks whether a geometry is valid and, if not valid, prints a message describing the reason

#include <iostream>
#include <string>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
/*<-*/ #include "create_svg_one.hpp" /*->*/

int main()
{
    typedef boost::geometry::model::d2::point_xy<double> point_type;
    typedef boost::geometry::model::polygon<point_type> polygon_type;

    polygon_type poly;
    boost::geometry::read_wkt("POLYGON((0 0,0 10,10 10,10 0,0 0),(0 0,9 1,9 2,0 0),(0 0,2 9,1 9,0 0),(2 9,9 2,9 9,2 9))", poly);

    std::string message;
    bool valid = boost::geometry::is_valid(poly, message);
    std::cout << "is valid? " << (valid ? "yes" : "no") << std::endl;
    if (! valid)
    {
        std::cout << "why not valid? " << message << std::endl;
    }
    /*<-*/ create_svg("is_valid_example.svg", poly); /*->*/
    return 0;
}

//]

//[is_valid_message_output
/*`
Output:
[pre
is valid? no
why not valid? Geometry has disconnected interior

[$img/algorithms/is_valid_example.png]

]

*/
//]


/*
@copyright Louis Dionne 2015
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <test/identity.hpp>

#include <boost/hana/assert.hpp>
#include <boost/hana/core/operators.hpp>
#include <boost/hana/functional/always.hpp>
#include <boost/hana/functional/compose.hpp>
#include <boost/hana/tuple.hpp>
#include <boost/hana/type.hpp>

#include <test/auto/base.hpp>
#include <test/injection.hpp>
#include <test/cnumeric.hpp>

// tested instances
#include <test/auto/applicative.hpp>
#include <test/auto/functor.hpp>
#include <test/auto/monad.hpp>
#include <test/auto/traversable.hpp>
using namespace boost::hana;

namespace boost { namespace hana { namespace test {
    template <>
    auto instances<Identity> = make<Tuple>(
        type<Functor>,
        type<Applicative>,
        type<Monad>,
        type<Traversable>
    );

    template <>
    auto objects<Identity> = make<Tuple>(
        identity(x<0>),
        identity(x<1>),
        identity(x<2>),
        identity(x<3>)
    );
}}}

namespace boost { namespace hana {
    template <>
    struct operators::of<test::Identity>
        : operators::of<Monad>
    { };
}}

int main() {
    using test::x;
    using test::cnumeric;
    auto f = test::injection([]{});

    test::check_datatype<test::Identity>();

    // Functor
    {
        auto functor = test::identity;
        // adjust_if
        {
            BOOST_HANA_CONSTANT_CHECK(equal(
                adjust_if(functor(x<0>), always(cnumeric<bool, true>), f),
                functor(f(x<0>))
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                adjust_if(functor(x<0>), always(cnumeric<bool, false>), f),
                functor(x<0>)
            ));
        }

        // fill
        {
            BOOST_HANA_CONSTANT_CHECK(equal(
                fill(functor(x<0>), x<1>),
                functor(x<1>)
            ));
        }

        // transform
        {
            BOOST_HANA_CONSTANT_CHECK(equal(
                transform(functor(x<0>), f),
                functor(f(x<0>))
            ));
        }

        // replace_if
        {
            BOOST_HANA_CONSTANT_CHECK(equal(
                replace_if(functor(x<0>), always(cnumeric<bool, true>), x<1>),
                functor(x<1>)
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                replace_if(functor(x<0>), always(cnumeric<bool, false>), x<1>),
                functor(x<0>)
            ));
        }
    }

    // Applicative
    {
        auto a = test::identity;
        using A = test::Identity;

        // ap
        {
            BOOST_HANA_CONSTANT_CHECK(equal(
                ap(a(f), a(x<0>)),
                a(f(x<0>))
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                ap(a(f), a(x<0>), a(x<1>)),
                a(f(x<0>, x<1>))
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                ap(a(f), a(x<0>), a(x<1>), a(x<2>)),
                a(f(x<0>, x<1>, x<2>))
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                ap(a(f), a(x<0>), a(x<1>), a(x<2>), a(x<3>)),
                a(f(x<0>, x<1>, x<2>, x<3>))
            ));
        }

        // lift
        {
            BOOST_HANA_CONSTANT_CHECK(equal(
                lift<A>(x<0>),
                a(x<0>)
            ));
        }
    }

    // Monad
    {
        auto monad = test::identity;
        using M = test::Identity;
        auto f = compose(monad, test::injection([]{}));
        auto g = compose(monad, test::injection([]{}));

        // bind
        {
            BOOST_HANA_CONSTANT_CHECK(equal(
                bind(monad(x<1>), f),
                f(x<1>)
            ));
        }

        // tap
        {
            bool executed = false;
            auto exec = [&](auto) { executed = true; };
            BOOST_HANA_CONSTANT_CHECK(equal(
                bind(monad(x<0>), tap<M>(exec)),
                monad(x<0>)
            ));
            BOOST_HANA_RUNTIME_CHECK(executed);
        }

        // then
        {
            struct invalid { };
            BOOST_HANA_CONSTANT_CHECK(equal(
                then(monad(invalid{}), monad(x<0>)),
                monad(x<0>)
            ));
        }

        // operators
        {
            using namespace boost::hana::operators;

            BOOST_HANA_CONSTANT_CHECK(equal(
                monad(x<1>) | f,
                bind(monad(x<1>), f)
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                monad(x<1>) | f | g,
                bind(bind(monad(x<1>), f), g)
            ));
        }
    }

    // Traversable
    {
        // traverse
        {
            auto f = test::injection([]{});
            BOOST_HANA_CONSTANT_CHECK(equal(
                traverse<test::Identity>(test::identity(x<0>), compose(test::identity, f)),
                test::identity(test::identity(f(x<0>)))
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                traverse<Tuple>(test::identity(x<0>), compose(make<Tuple>, f)),
                make<Tuple>(test::identity(f(x<0>)))
            ));
        }

        // sequence
        {
            BOOST_HANA_CONSTANT_CHECK(equal(
                sequence<test::Identity>(test::identity(test::identity(x<0>))),
                test::identity(test::identity(x<0>))
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                sequence<Tuple>(test::identity(make<Tuple>(x<0>, x<1>, x<2>))),
                make<Tuple>(test::identity(x<0>), test::identity(x<1>), test::identity(x<2>))
            ));
        }
    }
}


/*

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

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

This file is part of Chaste.

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

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

*/

#ifndef UNIFORMSOURCEPARABOLICPDE_HPP_
#define UNIFORMSOURCEPARABOLICPDE_HPP_

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

#include "AbstractLinearParabolicPde.hpp"

/**
 * An elliptic PDE to be solved numerically using the finite element method, for
 * coupling to a cell-based simulation.
 *
 * The PDE takes the form
 *
 * du/dt = Grad.(Grad(u)) + k,
 *
 * where the scalar k is specified by the member mSourceCoefficient, whose value
 * must be set in the constructor.
 *
 * Thus, there is no direct coupling between the cell-based simulation and the
 * terms of the PDE; here, the cell population just defines the spatial domain
 * on which to solve the PDE.
 */
template <unsigned DIM>
class UniformSourceParabolicPde : public AbstractLinearParabolicPde<DIM,DIM>
{
    friend class TestCellBasedParabolicPdes;

private:

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

    /** Constant source term (rate of production) of the dependent variable. */
    double mSourceCoefficient;

public:

    /**
     * Constructor.
     *
     * @param sourceCoefficient the source term coefficient (defaults to 0.0)
     */
    UniformSourceParabolicPde(double sourceCoefficient=0.0);

    /**
     * @return mSourceCoefficient
     */
    double GetCoefficient() const;

    /**
     * Overridden ComputeSourceTerm() method.
     *
     * @param rX the point in space at which the nonlinear source term is computed
     * @param u the value of the dependent variable at the point
     * @param pElement The element
     *
     * @return the the source term.
     */
    double ComputeSourceTerm(const ChastePoint<DIM>& rX, double u, Element<DIM,DIM>* pElement=NULL);

    /**
     * Overridden ComputeDiffusionTerm() method.
     *
     * @param rX the point in space at which the diffusion term is computed
     * @param pElement the mesh element that x is contained in (optional; defaults to NULL).
     *
     * @return a matrix.
     */
    c_matrix<double, DIM, DIM> ComputeDiffusionTerm(const ChastePoint<DIM>& rX, Element<DIM,DIM>* pElement=NULL);

    /**
     * Overridden ComputeDuDtCoefficientFunction() method.
     *
     * @return the function c(x) in "c(x) du/dt = Grad.(DiffusionTerm(x)*Grad(u))+LinearSourceTerm(x)+NonlinearSourceTerm(x, u)"
     *
     * @param rX the point in space at which the function c is computed
     */
    double ComputeDuDtCoefficientFunction(const ChastePoint<DIM>& rX);
};

#include "SerializationExportWrapper.hpp"
EXPORT_TEMPLATE_CLASS_SAME_DIMS(UniformSourceParabolicPde)

#endif //UNIFORMSOURCEPARABOLICPDE_HPP_


/*
 * IoMapping.cpp
 *
 *  Created on: 2013/06/18
 *      Author: kryozahiro
 */

#include "IoMapping.h"

#include <boost/lexical_cast.hpp>
#include "../../problem/Problem.h"
using namespace std;

IoMapping::IoMapping(const ProgramType& programType) : Program(programType) {
	const DataType& inputType = programType.getInputType();
	const DataType& outputType = programType.getOutputType();
	inputVariety = (inputType.getMax() - inputType.getMin() + 1) / inputType.getUnit();
	outputVariety = (outputType.getMax() - outputType.getMin() + 1) / outputType.getUnit();

	if (inputVariety == 0 or inputType.getSize() == 0) {
		//無引数関数
		inputVariety = 1;
	}

	//全ての入力の組み合わせの数
	int domain = pow(inputVariety, inputType.getSize());
	assert(0 < domain);
	assert(domain < 10000);

	mapping = vector<vector<double>>(domain, vector<double>(outputType.getSize(), 0));
}

vector<double> IoMapping::operator()(const vector<double>& input) {
	int pos = 0;
	for (double inputElement : input) {
		pos *= inputVariety;
		pos += inputElement - getProgramType().getInputType().getMin();
	}
	return mapping[pos];
}

string IoMapping::toString() const {
	string ret = "mapping: ";
	for (const vector<double>& output : mapping) {
		for (double outputElement : output) {
			ret += boost::lexical_cast<string>(outputElement) + "-";
		}
		ret += ", ";
	}
	ret += "\n";
	return ret;
}