#include "Polyline.h"
#include "Vertex.h"
#include <boost/math/constants/constants.hpp>
#include <boost/optional.hpp>
#include <fmt/format.h>
#include <smartmet/macgyver/Exception.h>
#include <algorithm>
#include <stdexcept>

using boost::math::double_constants::radian;

namespace Trax
{
// Begin a new polyline
Polyline::Polyline(std::initializer_list<double> init_list)
{
  if (init_list.size() == 0 || init_list.size() % 2 != 0)
    throw Fmi::Exception(BCP, "Invalid initialization of Polyline from elements");
  const auto* iter = init_list.begin();
  while (iter != init_list.end())
  {
    double x = *iter++;
    double y = *iter++;
    m_points.emplace_back(x, y, false);
  }
  update_bbox();
}

// A polyline is closed if it contains at least 3 points and the end points are equal
bool Polyline::closed() const
{
  if (size() < 3)
    return false;
  return (xbegin() == xend() && ybegin() == yend());
}

// A polyline is clockwise if it is closed and the signed area is positive.
bool Polyline::clockwise() const
{
  if (!closed())
    return false;

  double area = 0;
  auto n = size();
  for (std::size_t i = 0; i < n - 1; i++)
    area += (m_points[i + 1].x - m_points[i].x) * (m_points[i].y + m_points[i + 1].y);

  // The true area is |area/2|, but we care only about the orientation

  // Treating zero-size polygons as anti-clockwise lead to troubled code elsewhere,
  // hence we use equality as well.
  return area >= 0;
}

std::vector<double> Polyline::xcoordinates() const
{
  std::vector<double> ret;
  const auto n = m_points.size();
  ret.reserve(n);
  for (auto i = 0UL; i < n; i++)
    ret.push_back(m_points[i].x);
  return ret;
}

std::vector<double> Polyline::ycoordinates() const
{
  std::vector<double> ret;
  const auto n = m_points.size();
  ret.reserve(n);
  for (auto i = 0UL; i < n; i++)
    ret.push_back(m_points[i].y);
  return ret;
}

// Polygon exteriors and holes may share only singular vertices and not
// whole edges. Hence any point along any edge should be inside an exterior
// if the hole is inside it.

std::pair<double, double> Polyline::inside_point() const
{
  const auto x = 0.5 * (m_points[0].x + m_points[1].x);
  const auto y = 0.5 * (m_points[0].y + m_points[1].y);
  return std::make_pair(x, y);
}

// Polyline exit angle
double Polyline::end_angle() const
{
  const auto n = m_points.size() - 2;
  auto x1 = m_points[n].x;
  auto y1 = m_points[n].y;
  auto x2 = m_points[n + 1].x;
  auto y2 = m_points[n + 1].y;
  return atan2(y2 - y1, x2 - x1) * radian;
}

// Polyline start angle
double Polyline::start_angle() const
{
  auto x1 = m_points[0].x;
  auto y1 = m_points[0].y;
  auto x2 = m_points[1].x;
  auto y2 = m_points[1].y;
  return atan2(y2 - y1, x2 - x1) * radian;
}

bool Polyline::bbox_contains(const Polyline& other) const
{
  return bbox().contains(other.bbox());
}

// Does the (closed) polyline contain the other (closed) polyline
bool Polyline::contains(const Polyline& other) const
{
  // Quick exit based on bounding box containment
  if (!bbox_contains(other))
  {
    return false;
  }

  // A hole may touch the exterior at a vertex, but may not share
  // an edge with it. Hence choosing the middle of a vertex as a test
  // point is safe provided there are no rounding errors, huge values
  // causing problems, highly distorted grids or something similar
  // making the calculations not robust.

  const auto test_point = other.inside_point();
  const auto x = test_point.first;
  const auto y = test_point.second;

  // Refs 1-2 are for clarity since ref 3 only provides an idea for optimization by Stuart MacMartin
  //
  // 1: http://www.faqs.org/faqs/graphics/algorithms-faq/ question 2.03
  // 2: https://wrf.ecse.rpi.edu/Research/Short_Notes/pnpoly.html
  // 3: http://www.realtimerendering.com/resources/RTNews/html//rtnv5n3.html#art3

  const auto n = m_points.size();
  bool inside = false;
  for (std::size_t i = 1; i < n; i++)
  {
    // Skip continuously while below or above y
    if (m_points[i - 1].y < y)
    {
      while (i < n && m_points[i].y < y)
        i++;
    }
    else if (m_points[i - 1].y > y)
    {
      while (i < n && m_points[i].y > y)
        i++;
    }

    if (i >= n)
      break;  // reached end without crossing y

    auto x1 = m_points[i - 1].x;
    auto y1 = m_points[i - 1].y;
    auto x2 = m_points[i].x;
    auto y2 = m_points[i].y;
    if ((y1 > y) != (y2 > y))
      if (x < (x1 - x2) * (y - y2) / (y1 - y2) + x2)
        inside = !inside;
  }

  return inside;
}

// Append a new coordinate. Discard duplicates which appear from apthological cases
// such as an isoband range value appearing only in one corner of a grid cell.
void Polyline::append(const Vertex& vertex)
{
  if (m_points.empty())
  {
    m_points.reserve(256);  // do not settle for a small initial allocation
    m_points.emplace_back(vertex.x, vertex.y, vertex.ghost);
  }
  else if (m_points.back().x != vertex.x || m_points.back().y != vertex.y)  // ignore duplicates
    m_points.emplace_back(vertex.x, vertex.y, vertex.ghost);
}

// Append another polyline directly
void Polyline::append(const Polyline& other)
{
  m_points.insert(m_points.end(), ++other.m_points.begin(), other.m_points.end());
}

// Reverse the winding order
void Polyline::reverse()
{
  const auto n = size();
  for (auto i = 0UL, j = n - 1; i < j; i++, j--)
    std::swap(m_points[i], m_points[j]);
}

// Export normal WKT
std::string Polyline::wkt() const
{
  std::string ret = "LINESTRING ";
  ret += wkt_body();
  return ret;
}

// Export WKT body only for use in multilinestrings etc
std::string Polyline::wkt_body() const
{
  std::string ret = "(";

  const auto n = size();
  for (auto i = 0UL; i < n; i++)
  {
    if (i > 0)
      ret += ',';
    ret += fmt::format("{} {}", m_points[i].x, m_points[i].y);
  }
  ret += ')';
  return ret;
}

void Polyline::wkb(std::ostringstream& out) const
{
  unsigned char byteOrder = 1;
  int n = 1;
  if(*(char *)&n == 0)
    byteOrder = 0;

  out.write((const char*)&byteOrder,sizeof(byteOrder));
  uint type = 2; // Line
  out.write((const char*)&type,sizeof(type));
  wkb_body(out);
}

void Polyline::wkb_body(std::ostringstream& out) const
{
  uint n = size();
  out.write((const char*)&n,sizeof(n));

  for (uint i = 0; i < n; i++)
  {
    double x = m_points[i].x;
    double y = m_points[i].y;

    //printf("Point %f,%f\n",x,y);
    out.write((const char*)&x,8);
    out.write((const char*)&y,8);
  }
}

// Normalize coordinates to lexicographic order for testing purposes
Polyline& Polyline::normalize()
{
  if (!closed())
  {
    const auto n = size() - 1;
    // Lexicographically smallest end vertex first
    if (m_points[0].x > m_points[n].x ||
        (m_points[0].x == m_points[n].x && m_points[0].y > m_points[n].y))
      reverse();
    return *this;
  }

  // Find lexicographically smallest element

  auto n = size() - 1;  // minus one since the last vertex is the same as the first one
  auto best = 0UL;
  for (auto i = 1UL; i < n; i++)
  {
    if ((m_points[i].x < m_points[best].x) ||
        (m_points[i].x == m_points[best].x && m_points[i].y < m_points[best].y))
      best = i;
  }

  // Rotate until it is the first element
  if (best != 0)
  {
    auto pos = m_points.begin();
    std::advance(pos, best);
    std::rotate(m_points.begin(), pos, --m_points.end());

    // Fix closing vertex
    m_points[n] = m_points[0];
  }
  return *this;
}

// For normalizing collections of polylines
bool Polyline::operator<(const Polyline& other) const
{
  if (this == &other)
    return false;

  const auto n = std::min(size(), other.size());
  for (auto i = 0UL; i < n; i++)
  {
    if (m_points[i].x != other.m_points[i].x)
      return m_points[i].x < other.m_points[i].x;
    if (m_points[i].y != other.m_points[i].y)
      return m_points[i].y < other.m_points[i].y;
  }
  return false;
}

void Polyline::update_bbox()
{
  m_bbox.init(m_points);
}

bool Polyline::has_ghosts() const
{
  return std::any_of(m_points.begin(), m_points.end(), [](const Point& p) { return p.ghost; });
}

struct ValidRange
{
  std::size_t begin;
  std::size_t end;
};

boost::optional<ValidRange> find_valid_range(const Points& points, std::size_t startpos)
{
  const auto n = points.size();
  while (startpos < n && points[startpos].ghost)
    ++startpos;
  if (startpos >= n)
    return {};

  auto endpos = startpos;
  while (endpos < n && !points[endpos].ghost)
    ++endpos;

  return ValidRange{startpos, endpos};
}

// precondition: has_ghosts is true, closed is true
void Polyline::remove_ghosts(Polylines& new_polylines)
{
#if 0
  std::cout << "Removing ghosts from " << wkt() << "\n";
  for (auto i = 0UL; i < m_points.size(); i++)
    std::cout << fmt::format(
        "\t{} : {},{} {}\n", i, m_points[i].x, m_points[i].y, m_points[i].ghost ? "?" : "-");
#endif

  // Extract valid ranges
  std::vector<ValidRange> ranges;
  const auto n = m_points.size();
  auto startpos = 0UL;
  while (startpos < n)
  {
    auto range = find_valid_range(m_points, startpos);
    if (!range)
      break;
    if (range->end - range->begin > 1)
      ranges.emplace_back(*range);
    startpos = range->end;
  }

  // Detect whether there is a wraparound
  const bool wraparound =
      (ranges.size() > 1 && ranges.front().begin == 0 && ranges.back().end == n);

  auto sz = ranges.size();
  if (wraparound)  // if there is wraparound, the last range is handled along with the first one
    --sz;

  for (auto i = 0UL; i < sz; i++)
  {
    Polyline line;
    const auto& range = ranges[i];
    if (i == 0 && wraparound)
    {
      const auto& range2 = ranges.back();
      // std::cout << "Keeping wraparound range " << range2.begin << "..." << range2.end << "\n";
      for (auto j = range2.begin; j < range2.end - 1; j++)
        line.m_points.push_back(m_points[j]);
    }
    // std::cout << "Keeping  range " << range.begin << "..." << range.end << "\n";
    for (auto j = range.begin; j < range.end; j++)
      line.m_points.push_back(m_points[j]);
    new_polylines.emplace_back(std::move(line));
  }
}

}  // namespace Trax


/**
 * @file sdms_vector.hpp
 * @author David Albert (david.albert@insa-lyon.fr)
 * @brief
 * @version 0.1
 * @date 07/01/2021
 *
 * @copyright Copyright (c) 2021
 *
 */
#pragma once

#include <vector>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_sparse.hpp>
#include <boost/numeric/ublas/io.hpp>

#include <assert.h>

#include <sdm/utils/linear_algebra/vector_interface.hpp>

namespace sdm
{
  /**
   * @brief Create a SDMS Vector. A SMDS Vector is used to optimize the calculation, however, you have to be careful when using it because it's not possible to add element after the initialization
   *
   * @tparam I Type of index
   * @tparam T Type of value
   */
  template <class I, class T, class TBaseVector>
  class sdmsVector : public VectorInterface<I, T>
  {
  public:
    // using array_type = typename TBaseVector::array_type;
    // using value_type = typename array_type::value_type;

    static double PRECISION;

    sdmsVector();

    /**
     * @brief Create a SDMS Vector. In order to create a vector, it's necessary to provide a map that associated the I element with a specific index and a specific Value.
     *
     * @param std::shared_ptr<std::unordered_map<I, size_t>> : A map that associate the element I with a specific index
     * @param std::shared_ptr<std::unordered_map<I, T>> : A map that associate the element I with a specific value
     * @param double : the default value
     *
     */
    sdmsVector(std::shared_ptr<std::unordered_map<I, size_t>>, std::shared_ptr<std::unordered_map<I, T>>, double = 0);

    /**
     * @brief Create a SDMS Vector.
     *
     * @param std::vector<I>> : A vector for each element I
     * @param std::vector<T> : A vector for each value associate at the same position of the element I in vector
     * @param double : the default value
     *
     */
    sdmsVector(std::vector<I>, std::vector<T>, double = 0);

    virtual ~sdmsVector() {}

    T at(const I &) const;
    T getValueAt(const I &) const;
    void setValueAt(const I &, const T &value);
    void addValueAt(const I &, const T &value); // ps: added by baris, not tested.

    T sum() const;
    T norm_1() const;
    T norm_2() const;

    T min();
    I argmin();
    T max();
    I argmax();

    // sdmsVector transpose() const;

    const std::vector<I> &getIndexes() const;

    /**
     * @brief Compare two vectors. Return true if all values are lower or equal to the second vector.
     *
     * @return true
     * @return false
     */
    bool operator<=(const sdmsVector &) const;
    bool operator==(const sdmsVector &) const;
    bool operator!=(const sdmsVector &) const;
    bool isEqual(const sdmsVector &other, double precision) const;

    // template <class AE>
    T dot(const sdmsVector &v2) const;

    // template <class AE>
    T operator^(const sdmsVector &v2) const;

    std::string str() const;

    auto begin() const { return this->iterator_.begin(); }
    auto end() const { return this->iterator_.end(); }

    template <class Archive>
    void serialize(Archive &archive, const unsigned int);

    size_t size() const;
    std::shared_ptr<std::unordered_map<I, size_t>> getMapElementToIndex() const;

  protected:
    std::vector<I> vector_element_;
    // std::shared_ptr<std::unordered_map<I, size_t>> map_index_to_value_;
    std::shared_ptr<std::unordered_map<I, size_t>> map_element_to_index_;

    // std::map<I, T> iterator_;

    TBaseVector tbasevector_;

    std::pair<I, T> getMin() const;
    std::pair<I, T> getMax() const;

    // friend sdmsVector operator*(const T &arg1, const sdmsVector &arg2)
    // {
    //   sdmsVector vnew = arg1 * static_cast<TBaseVector>(arg2);
    //   vnew.setIndexes(arg2.getIndexes());
    //   return vnew;
    // }
  };

  /**
   * @brief Sparse vector are vectors that store only non-zero values.
   *
   * @tparam I Type of index
   * @tparam T Type of value
   */
  template <typename I = size_t, typename T = double>
  using SparseVector = sdmsVector<I, T, boost::numeric::ublas::mapped_vector<T>>;

  /**
   * @brief Dense vector are standard vector.
   *
   * @tparam I Type of index
   * @tparam T Type of value
   */
  template <typename I = size_t, typename T = double>
  using DenseVector = sdmsVector<I, T, boost::numeric::ublas::vector<T>>;

} // namespace sdm

#include <sdm/utils/linear_algebra/sdms_vector.tpp>


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 */

#include <cmath>
#include <numeric>

#include <boost/math/special_functions/erf.hpp>
#include <boost/math/distributions/normal.hpp>
#include <boost/math/distributions/lognormal.hpp>

#include "tudat/math/statistics/multiVariateGaussianProbabilityDistributions.h"
namespace tudat
{
namespace statistics
{

//! Function to evaluate pdf of Gaussian cupola distribution
double GaussianCopulaDistributionXd::evaluatePdf(
        const Eigen::VectorXd& independentVariables )
{
    double probabilityDensity = 0.0 ;

    // Check if vector independentVariables is inside [0,1]
    int inBound = 0 ;
    for( int i = 0 ; i < dimension_ ; i++ )
    {
        if( independentVariables(i) > 0.0 && independentVariables(i) < 1.0 )
        {
            inBound++;
        }
    }

    // If data is in bounds
    if( inBound == dimension_ )
    {
        // Convert U[0,1] to N[0,1] using inverse CDF of standard normal distribution
        Eigen::VectorXd gaussianQuantiles( dimension_ ) ;
        boost::math::normal distribution( 0.0 , 1.0 );

        for( int i = 0 ; i < dimension_ ; i++ )
        {
            gaussianQuantiles( i ) = boost::math::quantile( distribution , independentVariables( i ) ); // Inverse cdf
        }

        // Calculate probability density
        Eigen::MatrixXd location = - 0.5 * ( gaussianQuantiles.transpose() *
                       ( inverseCorrelationMatrix_ - Eigen::MatrixXd::Identity( dimension_ , dimension_ ) ) *
                                             gaussianQuantiles );

        probabilityDensity = ( ( 1.0 / ( std::sqrt( determinant_ ) ) ) * std::exp( location( 0, 0 ) ) ) ;
    }

    return probabilityDensity ;
}

} // namespace statistics
} // namespace tudat


/*
 [auto_generated]
 boost/numeric/odeint/stepper/bulirsch_stoer_dense_out.hpp

 [begin_description]
 Implementaiton of the Burlish-Stoer method with dense output
 [end_description]

 Copyright 2009-2011 Karsten Ahnert
 Copyright 2009-2011 Mario Mulansky

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

#ifndef BOOST_NUMERIC_ODEINT_STEPPER_BULIRSCH_STOER_DENSE_OUT_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_BULIRSCH_STOER_DENSE_OUT_HPP_INCLUDED

#include <iostream>

#include <algorithm>

#include <boost/config.hpp> // for min/max guidelines

#include <boost/numeric/odeint/util/bind.hpp>

#include <boost/math/special_functions/binomial.hpp>

#include <boost/numeric/odeint/stepper/controlled_runge_kutta.hpp>
#include <boost/numeric/odeint/stepper/modified_midpoint.hpp>
#include <boost/numeric/odeint/stepper/controlled_step_result.hpp>
#include <boost/numeric/odeint/algebra/range_algebra.hpp>
#include <boost/numeric/odeint/algebra/default_operations.hpp>
#include <boost/numeric/odeint/algebra/algebra_dispatcher.hpp>
#include <boost/numeric/odeint/algebra/operations_dispatcher.hpp>

#include <boost/numeric/odeint/util/state_wrapper.hpp>
#include <boost/numeric/odeint/util/is_resizeable.hpp>
#include <boost/numeric/odeint/util/resizer.hpp>
#include <boost/numeric/odeint/util/unit_helper.hpp>

#include <boost/type_traits.hpp>

namespace boost {
namespace numeric {
namespace odeint {

template<
    class State ,
    class Value = double ,
    class Deriv = State ,
    class Time = Value ,
    class Algebra = typename algebra_dispatcher< State >::algebra_type ,
    class Operations = typename operations_dispatcher< State >::operations_type ,
    class Resizer = initially_resizer
    >
class bulirsch_stoer_dense_out {

public:

    typedef State state_type;
    typedef Value value_type;
    typedef Deriv deriv_type;
    typedef Time time_type;
    typedef Algebra algebra_type;
    typedef Operations operations_type;
    typedef Resizer resizer_type;
    typedef dense_output_stepper_tag stepper_category;
#ifndef DOXYGEN_SKIP
    typedef state_wrapper< state_type > wrapped_state_type;
    typedef state_wrapper< deriv_type > wrapped_deriv_type;

    typedef bulirsch_stoer_dense_out< State , Value , Deriv , Time , Algebra , Operations , Resizer > controlled_error_bs_type;

    typedef typename inverse_time< time_type >::type inv_time_type;

    typedef std::vector< value_type > value_vector;
    typedef std::vector< time_type > time_vector;
    typedef std::vector< inv_time_type > inv_time_vector;  //should be 1/time_type for boost.units
    typedef std::vector< value_vector > value_matrix;
    typedef std::vector< size_t > int_vector;
    typedef std::vector< wrapped_state_type > state_vector_type;
    typedef std::vector< wrapped_deriv_type > deriv_vector_type;
    typedef std::vector< deriv_vector_type > deriv_table_type;
#endif //DOXYGEN_SKIP

    const static size_t m_k_max = 8;

    bulirsch_stoer_dense_out(
        value_type eps_abs = 1E-6 , value_type eps_rel = 1E-6 ,
        value_type factor_x = 1.0 , value_type factor_dxdt = 1.0 ,
        bool control_interpolation = false )
        : m_error_checker( eps_abs , eps_rel , factor_x, factor_dxdt ) ,
          m_control_interpolation( control_interpolation) ,
          m_last_step_rejected( false ) , m_first( true ) ,
          m_current_state_x1( true ) ,
          m_error( m_k_max ) ,
          m_interval_sequence( m_k_max+1 ) ,
          m_coeff( m_k_max+1 ) ,
          m_cost( m_k_max+1 ) ,
          m_table( m_k_max ) ,
          m_mp_states( m_k_max+1 ) ,
          m_derivs( m_k_max+1 ) ,
          m_diffs( 2*m_k_max+1 ) ,
          STEPFAC1( 0.65 ) , STEPFAC2( 0.94 ) , STEPFAC3( 0.02 ) , STEPFAC4( 4.0 ) , KFAC1( 0.8 ) , KFAC2( 0.9 )
    {
        BOOST_USING_STD_MIN();
        BOOST_USING_STD_MAX();

        for( unsigned short i = 0; i < m_k_max+1; i++ )
        {
            /* only this specific sequence allows for dense output */
            m_interval_sequence[i] = 2 + 4*i;  // 2 6 10 14 ...
            m_derivs[i].resize( m_interval_sequence[i] );
            if( i == 0 )
                m_cost[i] = m_interval_sequence[i];
            else
                m_cost[i] = m_cost[i-1] + m_interval_sequence[i];
            m_coeff[i].resize(i);
            for( size_t k = 0 ; k < i ; ++k  )
            {
                const value_type r = static_cast< value_type >( m_interval_sequence[i] ) / static_cast< value_type >( m_interval_sequence[k] );
                m_coeff[i][k] = 1.0 / ( r*r - static_cast< value_type >( 1.0 ) ); // coefficients for extrapolation
            }
            // crude estimate of optimal order

            m_current_k_opt = 4;
            /* no calculation because log10 might not exist for value_type!
            const value_type logfact( -log10( max BOOST_PREVENT_MACRO_SUBSTITUTION( eps_rel , static_cast< value_type >( 1.0E-12 ) ) ) * 0.6 + 0.5 );
            m_current_k_opt = max BOOST_PREVENT_MACRO_SUBSTITUTION( 1 , min BOOST_PREVENT_MACRO_SUBSTITUTION( static_cast<int>( m_k_max-1 ) , static_cast<int>( logfact ) ));
            */
        }
        int num = 1;
        for( int i = 2*(m_k_max) ; i >=0  ; i-- )
        {
            m_diffs[i].resize( num );
            num += (i+1)%2;
        }
    }

    template< class System , class StateIn , class DerivIn , class StateOut , class DerivOut >
    controlled_step_result try_step( System system , const StateIn &in , const DerivIn &dxdt , time_type &t , StateOut &out , DerivOut &dxdt_new , time_type &dt )
    {
        BOOST_USING_STD_MIN();
        BOOST_USING_STD_MAX();
        using std::pow;

        static const value_type val1( 1.0 );

        typename odeint::unwrap_reference< System >::type &sys = system;

        bool reject( true );

        time_vector h_opt( m_k_max+1 );
        inv_time_vector work( m_k_max+1 );

        m_k_final = 0;
        time_type new_h = dt;

        //std::cout << "t=" << t <<", dt=" << dt << ", k_opt=" << m_current_k_opt << ", first: " << m_first << std::endl;

        for( size_t k = 0 ; k <= m_current_k_opt+1 ; k++ )
        {
            m_midpoint.set_steps( m_interval_sequence[k] );
            if( k == 0 )
            {
                m_midpoint.do_step( sys , in , dxdt , t , out , dt , m_mp_states[k].m_v , m_derivs[k]);
            }
            else
            {
                m_midpoint.do_step( sys , in , dxdt , t , m_table[k-1].m_v , dt , m_mp_states[k].m_v , m_derivs[k] );
                extrapolate( k , m_table , m_coeff , out );
                // get error estimate
                m_algebra.for_each3( m_err.m_v , out , m_table[0].m_v ,
                                     typename operations_type::template scale_sum2< value_type , value_type >( val1 , -val1 ) );
                const value_type error = m_error_checker.error( m_algebra , in , dxdt , m_err.m_v , dt );
                h_opt[k] = calc_h_opt( dt , error , k );
                work[k] = static_cast<value_type>( m_cost[k] ) / h_opt[k];

                m_k_final = k;

                if( (k == m_current_k_opt-1) || m_first )
                { // convergence before k_opt ?
                    if( error < 1.0 )
                    {
                        //convergence
                        reject = false;
                        if( (work[k] < KFAC2*work[k-1]) || (m_current_k_opt <= 2) )
                        {
                            // leave order as is (except we were in first round)
                            m_current_k_opt = min BOOST_PREVENT_MACRO_SUBSTITUTION( static_cast<int>(m_k_max)-1 , max BOOST_PREVENT_MACRO_SUBSTITUTION( 2 , static_cast<int>(k)+1 ) );
                            new_h = h_opt[k] * static_cast<value_type>( m_cost[k+1] ) / static_cast<value_type>( m_cost[k] );
                        } else {
                            m_current_k_opt = min BOOST_PREVENT_MACRO_SUBSTITUTION( static_cast<int>(m_k_max)-1 , max BOOST_PREVENT_MACRO_SUBSTITUTION( 2 , static_cast<int>(k) ) );
                            new_h = h_opt[k];
                        }
                        break;
                    }
                    else if( should_reject( error , k ) && !m_first )
                    {
                        reject = true;
                        new_h = h_opt[k];
                        break;
                    }
                }
                if( k == m_current_k_opt )
                { // convergence at k_opt ?
                    if( error < 1.0 )
                    {
                        //convergence
                        reject = false;
                        if( (work[k-1] < KFAC2*work[k]) )
                        {
                            m_current_k_opt = max BOOST_PREVENT_MACRO_SUBSTITUTION( 2 , static_cast<int>(m_current_k_opt)-1 );
                            new_h = h_opt[m_current_k_opt];
                        }
                        else if( (work[k] < KFAC2*work[k-1]) && !m_last_step_rejected )
                        {
                            m_current_k_opt = min BOOST_PREVENT_MACRO_SUBSTITUTION( static_cast<int>(m_k_max)-1 , static_cast<int>(m_current_k_opt)+1 );
                            new_h = h_opt[k]*static_cast<value_type>( m_cost[m_current_k_opt] ) / static_cast<value_type>( m_cost[k] );
                        } else
                            new_h = h_opt[m_current_k_opt];
                        break;
                    }
                    else if( should_reject( error , k ) )
                    {
                        reject = true;
                        new_h = h_opt[m_current_k_opt];
                        break;
                    }
                }
                if( k == m_current_k_opt+1 )
                { // convergence at k_opt+1 ?
                    if( error < 1.0 )
                    {   //convergence
                        reject = false;
                        if( work[k-2] < KFAC2*work[k-1] )
                            m_current_k_opt = max BOOST_PREVENT_MACRO_SUBSTITUTION( 2 , static_cast<int>(m_current_k_opt)-1 );
                        if( (work[k] < KFAC2*work[m_current_k_opt]) && !m_last_step_rejected )
                            m_current_k_opt = min BOOST_PREVENT_MACRO_SUBSTITUTION( static_cast<int>(m_k_max)-1 , static_cast<int>(k) );
                        new_h = h_opt[m_current_k_opt];
                    } else
                    {
                        reject = true;
                        new_h = h_opt[m_current_k_opt];
                    }
                    break;
                }
            }
        }

        if( !reject )
        {

            //calculate dxdt for next step and dense output
            sys( out , dxdt_new , t+dt );

            //prepare dense output
            value_type error = prepare_dense_output( m_k_final , in , dxdt , out , dxdt_new , dt );

            if( error > static_cast<value_type>(10) ) // we are not as accurate for interpolation as for the steps
            {
                reject = true;
                new_h = dt * pow BOOST_PREVENT_MACRO_SUBSTITUTION( error , static_cast<value_type>(-1)/(2*m_k_final+2) );
            } else {
                t += dt;
            }
        }
        //set next stepsize
        if( !m_last_step_rejected || (new_h < dt) )
            dt = new_h;

        m_last_step_rejected = reject;
        if( reject )
            return fail;
        else
            return success;
    }

    template< class StateType >
    void initialize( const StateType &x0 , const time_type &t0 , const time_type &dt0 )
    {
        m_resizer.adjust_size( x0 , detail::bind( &controlled_error_bs_type::template resize_impl< StateType > , detail::ref( *this ) , detail::_1 ) );
        boost::numeric::odeint::copy( x0 , get_current_state() );
        m_t = t0;
        m_dt = dt0;
        reset();
    }

    /*  =======================================================
     *  the actual step method that should be called from outside (maybe make try_step private?)
     */
    template< class System >
    std::pair< time_type , time_type > do_step( System system )
    {
        const size_t max_count = 1000;

        if( m_first )
        {
            typename odeint::unwrap_reference< System >::type &sys = system;
            sys( get_current_state() , get_current_deriv() , m_t );
        }

        controlled_step_result res = fail;
        m_t_last = m_t;
        size_t count = 0;
        while( res == fail )
        {
            res = try_step( system , get_current_state() , get_current_deriv() , m_t , get_old_state() , get_old_deriv() , m_dt );
            m_first = false;
            if( count++ == max_count )
                throw std::overflow_error( "bulirsch_stoer : too much iterations!");
        }
        toggle_current_state();
        return std::make_pair( m_t_last , m_t );
    }

    /* performs the interpolation from a calculated step */
    template< class StateOut >
    void calc_state( time_type t , StateOut &x ) const
    {
        do_interpolation( t , x );
    }

    const state_type& current_state( void ) const
    {
        return get_current_state();
    }

    time_type current_time( void ) const
    {
        return m_t;
    }

    const state_type& previous_state( void ) const
    {
        return get_old_state();
    }

    time_type previous_time( void ) const
    {
        return m_t_last;
    }

    time_type current_time_step( void ) const
    {
        return m_dt;
    }

    /** \brief Resets the internal state of the stepper. */
    void reset()
    {
        m_first = true;
        m_last_step_rejected = false;
    }

    template< class StateIn >
    void adjust_size( const StateIn &x )
    {
        resize_impl( x );
        m_midpoint.adjust_size();
    }

private:

    template< class StateInOut , class StateVector >
    void extrapolate( size_t k , StateVector &table , const value_matrix &coeff , StateInOut &xest , size_t order_start_index = 0 )
    //polynomial extrapolation, see http://www.nr.com/webnotes/nr3web21.pdf
    {
        static const value_type val1( 1.0 );
        for( int j=k-1 ; j>0 ; --j )
        {
            m_algebra.for_each3( table[j-1].m_v , table[j].m_v , table[j-1].m_v ,
                                 typename operations_type::template scale_sum2< value_type , value_type >( val1 + coeff[k + order_start_index][j + order_start_index] ,
                                                                                -coeff[k + order_start_index][j + order_start_index] ) );
        }
        m_algebra.for_each3( xest , table[0].m_v , xest ,
                             typename operations_type::template scale_sum2< value_type , value_type >( val1 + coeff[k + order_start_index][0 + order_start_index] ,
                                                                                -coeff[k + order_start_index][0 + order_start_index]) );
    }

    template< class StateVector >
    void extrapolate_dense_out( size_t k , StateVector &table , const value_matrix &coeff , size_t order_start_index = 0 )
    //polynomial extrapolation, see http://www.nr.com/webnotes/nr3web21.pdf
    {
        // result is written into table[0]
        static const value_type val1( 1.0 );
        for( int j=k ; j>1 ; --j )
        {
            m_algebra.for_each3( table[j-1].m_v , table[j].m_v , table[j-1].m_v ,
                                 typename operations_type::template scale_sum2< value_type , value_type >( val1 + coeff[k + order_start_index][j + order_start_index - 1] ,
                                                                                -coeff[k + order_start_index][j + order_start_index - 1] ) );
        }
        m_algebra.for_each3( table[0].m_v , table[1].m_v , table[0].m_v ,
                             typename operations_type::template scale_sum2< value_type , value_type >( val1 + coeff[k + order_start_index][order_start_index] ,
                                                                                -coeff[k + order_start_index][order_start_index]) );
    }

    time_type calc_h_opt( time_type h , value_type error , size_t k ) const
    {
        BOOST_USING_STD_MIN();
        BOOST_USING_STD_MAX();
        using std::pow;

        value_type expo=1.0/(m_interval_sequence[k-1]);
        value_type facmin = pow BOOST_PREVENT_MACRO_SUBSTITUTION( STEPFAC3 , expo );
        value_type fac;
        if (error == 0.0)
            fac=1.0/facmin;
        else
        {
            fac = STEPFAC2 / pow BOOST_PREVENT_MACRO_SUBSTITUTION( error / STEPFAC1 , expo );
            fac = max BOOST_PREVENT_MACRO_SUBSTITUTION( facmin/STEPFAC4 , min BOOST_PREVENT_MACRO_SUBSTITUTION( 1.0/facmin , fac ) );
        }
        return h*fac;
    }

    bool in_convergence_window( size_t k ) const
    {
        if( (k == m_current_k_opt-1) && !m_last_step_rejected )
            return true; // decrease order only if last step was not rejected
        return ( (k == m_current_k_opt) || (k == m_current_k_opt+1) );
    }

    bool should_reject( value_type error , size_t k ) const
    {
        if( k == m_current_k_opt-1 )
        {
            const value_type d = m_interval_sequence[m_current_k_opt] * m_interval_sequence[m_current_k_opt+1] /
                (m_interval_sequence[0]*m_interval_sequence[0]);
            //step will fail, criterion 17.3.17 in NR
            return ( error > d*d );
        }
        else if( k == m_current_k_opt )
        {
            const value_type d = m_interval_sequence[m_current_k_opt+1] / m_interval_sequence[0];
            return ( error > d*d );
        } else
            return error > 1.0;
    }

    template< class StateIn1 , class DerivIn1 , class StateIn2 , class DerivIn2 >
    value_type prepare_dense_output( int k , const StateIn1 &x_start , const DerivIn1 &dxdt_start ,
                                     const StateIn2 & /* x_end */ , const DerivIn2 & /*dxdt_end */ , time_type dt )
    /* k is the order to which the result was approximated */
    {

        /* compute the coefficients of the interpolation polynomial
         * we parametrize the interval t .. t+dt by theta = -1 .. 1
         * we use 2k+3 values at the interval center theta=0 to obtain the interpolation coefficients
         * the values are x(t+dt/2) and the derivatives dx/dt , ... d^(2k+2) x / dt^(2k+2) at the midpoints
         * the derivatives are approximated via finite differences
         * all values are obtained from interpolation of the results from the increasing orders of the midpoint calls
         */

        // calculate finite difference approximations to derivatives at the midpoint
        for( int j = 0 ; j<=k ; j++ )
        {
            /* not working with boost units... */
            const value_type d = m_interval_sequence[j] / ( static_cast<value_type>(2) * dt );
            value_type f = 1.0; //factor 1/2 here because our interpolation interval has length 2 !!!
            for( int kappa = 0 ; kappa <= 2*j+1 ; ++kappa )
            {
                calculate_finite_difference( j , kappa , f , dxdt_start );
                f *= d;
            }

            if( j > 0 )
                extrapolate_dense_out( j , m_mp_states , m_coeff );
        }

        time_type d = dt/2;

        // extrapolate finite differences
        for( int kappa = 0 ; kappa<=2*k+1 ; kappa++ )
        {
            for( int j=1 ; j<=(k-kappa/2) ; ++j )
                extrapolate_dense_out( j , m_diffs[kappa] , m_coeff , kappa/2 );

            // extrapolation results are now stored in m_diffs[kappa][0]

            // divide kappa-th derivative by kappa because we need these terms for dense output interpolation
            m_algebra.for_each1( m_diffs[kappa][0].m_v , typename operations_type::template scale< time_type >( static_cast<time_type>(d) ) );

            d *= dt/(2*(kappa+2));
        }

        // dense output coefficients a_0 is stored in m_mp_states[0], a_i for i = 1...2k are stored in m_diffs[i-1][0]

        // the error is just the highest order coefficient of the interpolation polynomial
        // this is because we use only the midpoint theta=0 as support for the interpolation (remember that theta = -1 .. 1)

        value_type error = 0.0;
        if( m_control_interpolation )
        {
            boost::numeric::odeint::copy( m_diffs[2*k+1][0].m_v , m_err.m_v );
            error = m_error_checker.error( m_algebra , x_start , dxdt_start , m_err.m_v , dt );
        }

        return error;
    }

    template< class DerivIn >
    void calculate_finite_difference( size_t j , size_t kappa , value_type fac , const DerivIn &dxdt )
    {
        const int m = m_interval_sequence[j]/2-1;
        if( kappa == 0) // no calculation required for 0th derivative of f
        {
            m_algebra.for_each2( m_diffs[0][j].m_v , m_derivs[j][m].m_v ,
                                 typename operations_type::template scale_sum1< value_type >( fac ) );
        }
        else
        {
            // calculate the index of m_diffs for this kappa-j-combination
            const int j_diffs = j - kappa/2;

            m_algebra.for_each2( m_diffs[kappa][j_diffs].m_v , m_derivs[j][m+kappa].m_v ,
                                 typename operations_type::template scale_sum1< value_type >( fac ) );
            value_type sign = -1.0;
            int c = 1;
            //computes the j-th order finite difference for the kappa-th derivative of f at t+dt/2 using function evaluations stored in m_derivs
            for( int i = m+static_cast<int>(kappa)-2 ; i >= m-static_cast<int>(kappa) ; i -= 2 )
            {
                if( i >= 0 )
                {
                    m_algebra.for_each3( m_diffs[kappa][j_diffs].m_v , m_diffs[kappa][j_diffs].m_v , m_derivs[j][i].m_v ,
                                         typename operations_type::template scale_sum2< value_type , value_type >( 1.0 ,
                                                                                sign * fac * boost::math::binomial_coefficient< value_type >( kappa , c ) ) );
                }
                else
                {
                    m_algebra.for_each3( m_diffs[kappa][j_diffs].m_v , m_diffs[kappa][j_diffs].m_v , dxdt ,
                                         typename operations_type::template scale_sum2< value_type , value_type >( 1.0 , sign * fac ) );
                }
                sign *= -1;
                ++c;
            }
        }
    }

    template< class StateOut >
    void do_interpolation( time_type t , StateOut &out ) const
    {
        // interpolation polynomial is defined for theta = -1 ... 1
        // m_k_final is the number of order-iterations done for the last step - it governs the order of the interpolation polynomial
        const value_type theta = 2 * get_unit_value( (t - m_t_last) / (m_t - m_t_last) ) - 1;
        // we use only values at interval center, that is theta=0, for interpolation
        // our interpolation polynomial is thus of order 2k+2, hence we have 2k+3 terms

        boost::numeric::odeint::copy( m_mp_states[0].m_v , out );
        // add remaining terms: x += a_1 theta + a2 theta^2 + ... + a_{2k} theta^{2k}
        value_type theta_pow( theta );
        for( size_t i=0 ; i<=2*m_k_final+1 ; ++i )
        {
            m_algebra.for_each3( out , out , m_diffs[i][0].m_v ,
                                 typename operations_type::template scale_sum2< value_type >( static_cast<value_type>(1) , theta_pow ) );
            theta_pow *= theta;
        }
    }

    /* Resizer methods */
    template< class StateIn >
    bool resize_impl( const StateIn &x )
    {
        bool resized( false );

        resized |= adjust_size_by_resizeability( m_x1 , x , typename is_resizeable<state_type>::type() );
        resized |= adjust_size_by_resizeability( m_x2 , x , typename is_resizeable<state_type>::type() );
        resized |= adjust_size_by_resizeability( m_dxdt1 , x , typename is_resizeable<state_type>::type() );
        resized |= adjust_size_by_resizeability( m_dxdt2 , x , typename is_resizeable<state_type>::type() );
        resized |= adjust_size_by_resizeability( m_err , x , typename is_resizeable<state_type>::type() );

        for( size_t i = 0 ; i < m_k_max ; ++i )
            resized |= adjust_size_by_resizeability( m_table[i] , x , typename is_resizeable<state_type>::type() );
        for( size_t i = 0 ; i < m_k_max+1 ; ++i )
            resized |= adjust_size_by_resizeability( m_mp_states[i] , x , typename is_resizeable<state_type>::type() );
        for( size_t i = 0 ; i < m_k_max+1 ; ++i )
            for( size_t j = 0 ; j < m_derivs[i].size() ; ++j )
                resized |= adjust_size_by_resizeability( m_derivs[i][j] , x , typename is_resizeable<deriv_type>::type() );
        for( size_t i = 0 ; i < 2*m_k_max+1 ; ++i )
            for( size_t j = 0 ; j < m_diffs[i].size() ; ++j )
                resized |= adjust_size_by_resizeability( m_diffs[i][j] , x , typename is_resizeable<deriv_type>::type() );

        return resized;
    }

    state_type& get_current_state( void )
    {
        return m_current_state_x1 ? m_x1.m_v : m_x2.m_v ;
    }

    const state_type& get_current_state( void ) const
    {
        return m_current_state_x1 ? m_x1.m_v : m_x2.m_v ;
    }

    state_type& get_old_state( void )
    {
        return m_current_state_x1 ? m_x2.m_v : m_x1.m_v ;
    }

    const state_type& get_old_state( void ) const
    {
        return m_current_state_x1 ? m_x2.m_v : m_x1.m_v ;
    }

    deriv_type& get_current_deriv( void )
    {
        return m_current_state_x1 ? m_dxdt1.m_v : m_dxdt2.m_v ;
    }

    const deriv_type& get_current_deriv( void ) const
    {
        return m_current_state_x1 ? m_dxdt1.m_v : m_dxdt2.m_v ;
    }

    deriv_type& get_old_deriv( void )
    {
        return m_current_state_x1 ? m_dxdt2.m_v : m_dxdt1.m_v ;
    }

    const deriv_type& get_old_deriv( void ) const
    {
        return m_current_state_x1 ? m_dxdt2.m_v : m_dxdt1.m_v ;
    }

    void toggle_current_state( void )
    {
        m_current_state_x1 = ! m_current_state_x1;
    }

    default_error_checker< value_type, algebra_type , operations_type > m_error_checker;
    modified_midpoint_dense_out< state_type , value_type , deriv_type , time_type , algebra_type , operations_type , resizer_type > m_midpoint;

    bool m_control_interpolation;

    bool m_last_step_rejected;
    bool m_first;

    time_type m_t;
    time_type m_dt;
    time_type m_dt_last;
    time_type m_t_last;

    size_t m_current_k_opt;
    size_t m_k_final;

    algebra_type m_algebra;

    resizer_type m_resizer;

    wrapped_state_type m_x1 , m_x2;
    wrapped_deriv_type m_dxdt1 , m_dxdt2;
    wrapped_state_type m_err;
    bool m_current_state_x1;

    value_vector m_error; // errors of repeated midpoint steps and extrapolations
    int_vector m_interval_sequence; // stores the successive interval counts
    value_matrix m_coeff;
    int_vector m_cost; // costs for interval count

    state_vector_type m_table; // sequence of states for extrapolation

    //for dense output:
    state_vector_type m_mp_states; // sequence of approximations of x at distance center
    deriv_table_type m_derivs; // table of function values
    deriv_table_type m_diffs; // table of function values

    //wrapped_state_type m_a1 , m_a2 , m_a3 , m_a4;

    const value_type STEPFAC1 , STEPFAC2 , STEPFAC3 , STEPFAC4 , KFAC1 , KFAC2;
};

/********** DOXYGEN **********/

/**
 * \class bulirsch_stoer_dense_out
 * \brief The Bulirsch-Stoer algorithm.
 *
 * The Bulirsch-Stoer is a controlled stepper that adjusts both step size
 * and order of the method. The algorithm uses the modified midpoint and
 * a polynomial extrapolation compute the solution. This class also provides
 * dense output facility.
 *
 * \tparam State The state type.
 * \tparam Value The value type.
 * \tparam Deriv The type representing the time derivative of the state.
 * \tparam Time The time representing the independent variable - the time.
 * \tparam Algebra The algebra type.
 * \tparam Operations The operations type.
 * \tparam Resizer The resizer policy type.
 */

    /**
     * \fn bulirsch_stoer_dense_out::bulirsch_stoer_dense_out( value_type eps_abs , value_type eps_rel , value_type factor_x , value_type factor_dxdt , bool control_interpolation )
     * \brief Constructs the bulirsch_stoer class, including initialization of
     * the error bounds.
     *
     * \param eps_abs Absolute tolerance level.
     * \param eps_rel Relative tolerance level.
     * \param factor_x Factor for the weight of the state.
     * \param factor_dxdt Factor for the weight of the derivative.
     * \param control_interpolation Set true to additionally control the error of
     * the interpolation.
     */

    /**
     * \fn bulirsch_stoer_dense_out::try_step( System system , const StateIn &in , const DerivIn &dxdt , time_type &t , StateOut &out , DerivOut &dxdt_new , time_type &dt )
     * \brief Tries to perform one step.
     *
     * This method tries to do one step with step size dt. If the error estimate
     * is to large, the step is rejected and the method returns fail and the
     * step size dt is reduced. If the error estimate is acceptably small, the
     * step is performed, success is returned and dt might be increased to make
     * the steps as large as possible. This method also updates t if a step is
     * performed. Also, the internal order of the stepper is adjusted if required.
     *
     * \param system The system function to solve, hence the r.h.s. of the ODE.
     * It must fulfill the Simple System concept.
     * \param in The state of the ODE which should be solved.
     * \param dxdt The derivative of state.
     * \param t The value of the time. Updated if the step is successful.
     * \param out Used to store the result of the step.
     * \param dt The step size. Updated.
     * \return success if the step was accepted, fail otherwise.
     */

    /**
     * \fn bulirsch_stoer_dense_out::initialize( const StateType &x0 , const time_type &t0 , const time_type &dt0 )
     * \brief Initializes the dense output stepper.
     *
     * \param x0 The initial state.
     * \param t0 The initial time.
     * \param dt0 The initial time step.
     */

    /**
     * \fn bulirsch_stoer_dense_out::do_step( System system )
     * \brief Does one time step. This is the main method that should be used to
     * integrate an ODE with this stepper.
     * \note initialize has to be called before using this method to set the
     * initial conditions x,t and the stepsize.
     * \param system The system function to solve, hence the r.h.s. of the
     * ordinary differential equation. It must fulfill the Simple System concept.
     * \return Pair with start and end time of the integration step.
     */

    /**
     * \fn bulirsch_stoer_dense_out::calc_state( time_type t , StateOut &x ) const
     * \brief Calculates the solution at an intermediate point within the last step
     * \param t The time at which the solution should be calculated, has to be
     * in the current time interval.
     * \param x The output variable where the result is written into.
     */

    /**
     * \fn bulirsch_stoer_dense_out::current_state( void ) const
     * \brief Returns the current state of the solution.
     * \return The current state of the solution x(t).
     */

    /**
     * \fn bulirsch_stoer_dense_out::current_time( void ) const
     * \brief Returns the current time of the solution.
     * \return The current time of the solution t.
     */

    /**
     * \fn bulirsch_stoer_dense_out::previous_state( void ) const
     * \brief Returns the last state of the solution.
     * \return The last state of the solution x(t-dt).
     */

    /**
     * \fn bulirsch_stoer_dense_out::previous_time( void ) const
     * \brief Returns the last time of the solution.
     * \return The last time of the solution t-dt.
     */

    /**
     * \fn bulirsch_stoer_dense_out::current_time_step( void ) const
     * \brief Returns the current step size.
     * \return The current step size.
     */

    /**
     * \fn bulirsch_stoer_dense_out::adjust_size( const StateIn &x )
     * \brief Adjust the size of all temporaries in the stepper manually.
     * \param x A state from which the size of the temporaries to be resized is deduced.
     */

}
}
}

#endif // BOOST_NUMERIC_ODEINT_STEPPER_BULIRSCH_STOER_HPP_INCLUDED


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_TANH_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_TANH_HPP_INCLUDED

#include <boost/simd/arch/common/detail/generic/tanh_kernel.hpp>
#include <boost/simd/constant/mtwo.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/ratio.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/bitofsign.hpp>
#include <boost/simd/function/bitwise_xor.hpp>
#include <boost/simd/function/exp.hpp>
#include <boost/simd/function/fma.hpp>
#include <boost/simd/function/inc.hpp>
#include <boost/simd/function/rec.hpp>
#include <boost/simd/function/sqr.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD ( tanh_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_<bd::floating_<A0> >
                          )
  {

    BOOST_FORCEINLINE A0 operator() ( A0  a0) const BOOST_NOEXCEPT
    {
      //////////////////////////////////////////////////////////////////////////////
      // if x = abs(a0) is less than 5/8 sinh is computed using a polynomial(float)
      // (respectively rational(double)) approx from cephes.
      // else
      // tanh(a0) is  sign(a0)*(2/(exp(2*x)+1)+1)
      //////////////////////////////////////////////////////////////////////////////
      A0 x = bs::abs(a0);
      if( x < Ratio<A0, 5, 8>())
      {
       return detail::tanh_kernel<A0>::tanh(a0, sqr(x));
      }
      else
      {
       A0 r = fma(Mtwo<A0>(), rec(inc(exp(x+x))), One<A0>());
       return bitwise_xor(r, bitofsign(a0));
      }
    }
  };
} } }

#endif


// Copyright 2015 XLGAMES Inc.
//
// Distributed under the MIT License (See
// accompanying file "LICENSE" or the website
// http://www.opensource.org/licenses/mit-license.php)

#include "FluidAdvection.h"
#include "../Math/RegularNumberField.h"

#pragma warning(disable:4714)
#pragma push_macro("new")
#undef new
#include <Eigen/Dense>
#pragma pop_macro("new")

namespace SceneEngine
{
    using ScalarField2D = XLEMath::ScalarField2D<Eigen::VectorXf>;
    using VectorField2D = VectorField2DSeparate<Eigen::VectorXf>;
    using ScalarField3D = XLEMath::ScalarField3D<Eigen::VectorXf>;
    using VectorField3D = VectorField3DSeparate<Eigen::VectorXf>;

    UInt3 As3DDims(UInt2 input)     { return Expand(input, 1u); }
    UInt3 As3DBorder(UInt2 input)   { return UInt3(1,1,0); }
    UInt3 As3DDims(UInt3 input)     { return input; }
    UInt3 As3DBorder(UInt3 input)   { return UInt3(1,1,1); }

    template<typename OutType, typename InType>
        OutType ConvertVector(const InType& in)
        {
            OutType result;
            unsigned i=0u;
            for (; i<std::min((unsigned)InType::dimension, (unsigned)OutType::dimension); ++i)
                result[i] = (OutType::value_type)in[i];
            for (; i<(unsigned)OutType::dimension; ++i)
                result[i] = (OutType::value_type)0;
            return result;
        }

    template<unsigned SamplingFlags, typename Field>
        static typename Field::ValueType AdvectRK4(
            const Field& velFieldT0, const Field& velFieldT1,
            typename Field::Coord pt, typename Field::FloatCoord velScale)
        {
            const auto s = velScale;
            const auto halfS = decltype(s)(s / 2);

            auto startTap = ConvertVector<typename Field::FloatCoord>(pt);
            auto k1 = velFieldT0.Load(pt);
            auto k2 = .5f * velFieldT0.Sample<SamplingFlags>(startTap + MultiplyAcross(halfS, k1))
                    + .5f * velFieldT1.Sample<SamplingFlags>(startTap + MultiplyAcross(halfS, k1))
                    ;
            auto k3 = .5f * velFieldT0.Sample<SamplingFlags>(startTap + MultiplyAcross(halfS, k2))
                    + .5f * velFieldT1.Sample<SamplingFlags>(startTap + MultiplyAcross(halfS, k2))
                    ;
            auto k4 = velFieldT1.Sample<SamplingFlags>(startTap + MultiplyAcross(s, k3));

            auto finalVel = (1.f / 6.f) * (k1 + 2.f * k2 + 2.f * k3 + k4);
            return startTap + MultiplyAcross(s, finalVel);
        }

    template<unsigned SamplingFlags, typename Field>
        static typename Field::ValueType AdvectRK4(
            const Field& velFieldT0, const Field& velFieldT1,
            typename Field::FloatCoord pt, typename Field::FloatCoord velScale)
        {
            const auto s = velScale;
            const auto halfS = decltype(s)(s / 2);

                // when using a float point input, we need bilinear interpolation
            auto k1 = velFieldT0.Sample<SamplingFlags>(pt);
            auto k2 = .5f * velFieldT0.Sample<SamplingFlags>(pt + MultiplyAcross(halfS, k1))
                    + .5f * velFieldT1.Sample<SamplingFlags>(pt + MultiplyAcross(halfS, k1))
                    ;
            auto k3 = .5f * velFieldT0.Sample<SamplingFlags>(pt + MultiplyAcross(halfS, k2))
                    + .5f * velFieldT1.Sample<SamplingFlags>(pt + MultiplyAcross(halfS, k2))
                    ;
            auto k4 = velFieldT1.Sample<SamplingFlags>(pt + MultiplyAcross(s, k3));

            auto finalVel = (1.f / 6.f) * (k1 + 2.f * k2 + 2.f * k3 + k4);
            return pt + MultiplyAcross(s, finalVel);
        }

    template<typename Type> static Type MaxValue();
    template<> static float MaxValue()         { return FLT_MAX; }
    template<> static Float2 MaxValue()        { return Float2(FLT_MAX, FLT_MAX); }
    template<> static Float3 MaxValue()        { return Float3(FLT_MAX, FLT_MAX, FLT_MAX); }
    static float   MinAcross(float lhs, float rhs)   { return std::min(lhs, rhs); }
    static Float2  MinAcross(Float2 lhs, Float2 rhs) { return Float2(std::min(lhs[0], rhs[0]), std::min(lhs[1], rhs[1])); }
    static Float3  MinAcross(Float3 lhs, Float3 rhs) { return Float3(std::min(lhs[0], rhs[0]), std::min(lhs[1], rhs[1]), std::min(lhs[2], rhs[2])); }
    static float   MaxAcross(float lhs, float rhs)   { return std::max(lhs, rhs); }
    static Float2  MaxAcross(Float2 lhs, Float2 rhs) { return Float2(std::max(lhs[0], rhs[0]), std::max(lhs[1], rhs[1])); }
    static Float3  MaxAcross(Float3 lhs, Float3 rhs) { return Float3(std::max(lhs[0], rhs[0]), std::max(lhs[1], rhs[1]), std::max(lhs[2], rhs[2])); }

    // static Float2 ClampAcross(const Float2& input, const Float2& mins, const Float2& maxs)
    // {
    //     return Float2(Clamp(input[0], mins[0], maxs[0]), Clamp(input[1], mins[1], maxs[1]));
    // }
    //
    // static Float3 ClampAcross(const Float3& input, const Float3& mins, const Float3& maxs)
    // {
    //     return Float3(Clamp(input[0], mins[0], maxs[0]), Clamp(input[1], mins[1], maxs[1]), Clamp(input[2], mins[2], maxs[2]));
    // }

    template<unsigned WrappingFlags, typename VectorType>
        static VectorType ApplyBoundary(
            const VectorType& input,
            const VectorType& dims)
    {
        VectorType result;
        static_assert(RNFSample::WrapY == (RNFSample::WrapX<<1), "Expecting wrapping flags to be sequential bits");
        static_assert(RNFSample::WrapZ == (RNFSample::WrapX<<2), "Expecting wrapping flags to be sequential bits");
        for (unsigned c=0; c<VectorType::dimension; ++c) {
            if (WrappingFlags & (RNFSample::WrapX<<c)) {
                result[c] = XlFMod(input[c]+dims[c], dims[c]);
            } else {
                result[c] = Clamp(input[c], 0.f, dims[c]-1.f-1e-5f);
            }
        }
        return result;
    }

    template<unsigned SamplingFlags, typename Field>
        typename Field::ValueType LoadWithNearbyRange(
            typename Field::ValueType& minNeighbour,
            typename Field::ValueType& maxNeighbour, const Field& field, typename Field::FloatCoord pt)
        {
            typename Field::ValueType predictorParts[Field::NeighborCount];
            float predictorWeights[Field::BilinearWeightCount];
            field.GatherNeighbors(predictorParts, predictorWeights, pt, SamplingFlags);

            minNeighbour =  MaxValue<typename Field::ValueType>();
            maxNeighbour = -MaxValue<typename Field::ValueType>();
            for (unsigned c=0; c<Field::NeighborCount; ++c) {
                minNeighbour = MinAcross(predictorParts[c], minNeighbour);
                maxNeighbour = MaxAcross(predictorParts[c], maxNeighbour);
            }

            if (constant_expression<(SamplingFlags & RNFSample::Cubic)==0>::result()) {
                Field::ValueType result =  predictorWeights[0] * predictorParts[0];
                for (unsigned i=1; i<Field::BilinearWeightCount; ++i)   // hopefully the compiler should unroll this loop (which is short, there are only 4 or 8 weights)
                    result += predictorWeights[i] * predictorParts[i];
                return result;
            } else {
                return field.Sample<SamplingFlags>(pt);
            }
        }

    template<unsigned WrappingFlags, typename Field, typename VelField>
        static void PerformAdvection_Internal(
            Field dstValues, Field srcValues,
            VelField velFieldT0, VelField velFieldT1,
            float deltaTime, const AdvectionSettings& settings)
    {
        //
        // This is the advection step. We will use the method of characteristics.
        //
        // We have a few different options for the stepping method:
        //  * basic euler forward integration (ie, just step forward in time)
        //  * forward integration method divided into smaller time steps
        //  * Runge-Kutta integration
        //  * Modified MacCormack methods
        //  * Back and Forth Error Compensation and Correction (BFECC)
        //
        // Let's start without any complex boundary conditions.
        //
        // We have to be careful about how the velocity sample is aligned with
        // the grid cell. Incorrect alignment will produce a bias in the way that
        // we interpolate the field.
        //
        // We could consider offsetting the velocity field by half a cell (see
        // Visual Simulation of Smoke, Fedkiw, et al)
        //
        // Also consider Semi-Lagrangian methods for large timesteps (when the CFL
        // number is larger than 1)
        //

        const auto advectionMethod = settings._method;
        const auto adjvectionSteps = settings._subSteps;

        assert(dstValues.Dimensions() == srcValues.Dimensions());
        assert(dstValues.Dimensions() == velFieldT0.Dimensions());
        assert(dstValues.Dimensions() == velFieldT1.Dimensions());
        const UInt3 dims = As3DDims(dstValues.Dimensions());

            // when the border condition is "margin" we create a 1 cell margin on that
            // edge that will be read from, but not written to
        UInt3 margin = As3DBorder(dstValues.Dimensions());
        if (settings._borderX != AdvectionBorder::Margin) margin[0] = 0;
        if (settings._borderY != AdvectionBorder::Margin) margin[1] = 0;
        if (settings._borderZ != AdvectionBorder::Margin) margin[2] = 0;

        using FloatCoord = typename VelField::FloatCoord;
        using Coord = typename VelField::Coord;
        const auto velFieldScale = ConvertVector<FloatCoord>(
            Float3(
                float(dims[0]-2*margin[0]),
                float(dims[1]-2*margin[1]),
                float(dims[2]-2*margin[2])));   // (grid size without borders)
        const auto clampMax = ConvertVector<FloatCoord>(dims);

        if (advectionMethod == AdvectionMethod::ForwardEuler) {

                //  For each cell in the grid, trace backwards
                //  through the velocity field to find an approximation
                //  of where the point was in the previous frame.

            for (unsigned z=margin[2]; z<dims[2]-margin[2]; ++z)
                for (unsigned y=margin[1]; y<dims[1]-margin[1]; ++y)
                    for (unsigned x=margin[0]; x<dims[0]-margin[0]; ++x) {
                        auto coord = ConvertVector<Coord>(UInt3(x, y, z));
                        auto startVel = velFieldT1.Load(coord);
                        FloatCoord tap = ConvertVector<FloatCoord>(coord) - MultiplyAcross(deltaTime * velFieldScale, startVel);
                        tap = ApplyBoundary<WrappingFlags>(tap, clampMax);
                        dstValues.Write(coord, srcValues.Sample<0>(tap));
                    }

        } else if (advectionMethod == AdvectionMethod::ForwardEulerDiv) {

            auto stepScale = decltype(velFieldScale)(deltaTime * velFieldScale / float(adjvectionSteps));
            for (unsigned z=margin[2]; z<dims[2]-margin[2]; ++z)
                for (unsigned y=margin[1]; y<dims[1]-margin[1]; ++y)
                    for (unsigned x=margin[0]; x<dims[0]-margin[0]; ++x) {

                        auto coord = ConvertVector<Coord>(UInt3(x, y, z));
                        auto tap = ConvertVector<FloatCoord>(UInt3(x, y, z));
                        auto vel = velFieldT0.Load(coord);
                        for (unsigned s=1; ; ++s) {
                            tap -= MultiplyAcross(stepScale, vel);
                            tap = ApplyBoundary<WrappingFlags>(tap, clampMax);
                            if (s>=adjvectionSteps) break;

                            vel = LinearInterpolate(
                                velFieldT0.Sample<0>(tap),
                                velFieldT1.Sample<0>(tap),
                                s / float(adjvectionSteps-1));
                        }

                        dstValues.Write(coord, srcValues.Sample<WrappingFlags>(tap));
                    }

        } else if (advectionMethod == AdvectionMethod::RungeKutta) {

            if (settings._interpolation == AdvectionInterp::Bilinear) {

                const auto SamplingFlags = WrappingFlags;
                for (unsigned z=margin[2]; z<dims[2]-margin[2]; ++z)
                    for (unsigned y=margin[1]; y<dims[1]-margin[1]; ++y)
                        for (unsigned x=margin[0]; x<dims[0]-margin[0]; ++x) {

                                // This is the RK4 version
                                // We'll use the average of the velocity field at t and
                                // the velocity field at t+dt as an estimate of the field
                                // at t+.5*dt

                                // Note that we're tracing the velocity field backwards.
                                // So doing k1 on velField1, and k4 on velFieldT0
                                //      -- hoping this will interact with the velocity diffusion more sensibly
                            auto coord = ConvertVector<Coord>(UInt3(x, y, z));
                            const auto tap = AdvectRK4<SamplingFlags>(velFieldT1, velFieldT0, coord, -deltaTime * velFieldScale);
                            dstValues.Write(coord, srcValues.Sample<SamplingFlags>(tap));

                        }

            } else {

                const auto SamplingFlags = RNFSample::Cubic|WrappingFlags;
                for (unsigned z=margin[2]; z<dims[2]-margin[2]; ++z)
                    for (unsigned y=margin[1]; y<dims[1]-margin[1]; ++y)
                        for (unsigned x=margin[0]; x<dims[0]-margin[0]; ++x) {
                            auto coord = ConvertVector<Coord>(UInt3(x, y, z));
                            const auto tap = AdvectRK4<SamplingFlags>(velFieldT1, velFieldT0, coord, -deltaTime * velFieldScale);
                            dstValues.Write(coord, srcValues.Sample<SamplingFlags>(tap));
                        }

            }

        } else if (advectionMethod == AdvectionMethod::MacCormackRK4) {

                //
                // This is a modified MacCormack scheme, as described in An Unconditionally
                // Stable MacCormack Method -- Selle & Fedkiw, et al.
                //  http://physbam.stanford.edu/~fedkiw/papers/stanford2006-09.pdf
                //
                // It's also similar to the (oddly long nammed) Back And Forth Error Compensation
                // and Correction (BFECC).
                //
                // Basically, we want to run an initial predictor step, then run a backwards
                // advection to find an intermediate point. The difference between the value at
                // the initial point and the intermediate point is used as a error term.
                //
                // This way, we get an improved estimate, but with only 2 advection steps.
                //
                // We need to use some advection method for the forward and advection steps. Often
                // a semi-lagrangian method is used (particularly velocities and timesteps are large
                // with respect to the grid size).
                //
                // But here, we'll use RK4.
                //
                // We also need a way to check for overruns and oscillation cases. Selle & Fedkiw
                // suggest using a normal semi-Lagrangian method in these cases. We'll try a simplier
                // method and just clamp.
                //

            if (settings._interpolation == AdvectionInterp::Bilinear) {

                const auto SamplingFlags = WrappingFlags;
                for (unsigned z=margin[2]; z<dims[2]-margin[2]; ++z)
                    for (unsigned y=margin[1]; y<dims[1]-margin[1]; ++y)
                        for (unsigned x=margin[0]; x<dims[0]-margin[0]; ++x) {

                            auto coord = ConvertVector<Coord>(UInt3(x, y, z));

                                // advect backwards in time first, to find the predictor
                            const auto predictor = AdvectRK4<SamplingFlags>(velFieldT1, velFieldT0, coord, -deltaTime * velFieldScale);
                                // advect forward again to find the error tap
                            const auto reversedTap = AdvectRK4<SamplingFlags>(velFieldT0, velFieldT1, predictor, deltaTime * velFieldScale);

                            auto originalValue = srcValues.Load(coord);
                            auto reversedValue = srcValues.Sample<SamplingFlags>(reversedTap);
                            Field::ValueType finalValue;

                                // Here we clamp the final result within the range of the neighbour cells of the
                                // original predictor. This prevents the scheme from becoming unstable (by avoiding
                                // irrational values for 0.5f * (originalValue - reversedValue)
                            const bool doRangeClamping = true;
                            if (constant_expression<doRangeClamping>::result()) {
                                typename Field::ValueType minNeighbour, maxNeighbour;
                                auto predictorValue = LoadWithNearbyRange<SamplingFlags>(minNeighbour, maxNeighbour, srcValues, predictor);
                                finalValue = typename Field::ValueType(predictorValue + .5f * (originalValue - reversedValue));
                                finalValue = MaxAcross(finalValue, minNeighbour);
                                finalValue = MinAcross(finalValue, maxNeighbour);
                            } else {
                                auto predictorValue = srcValues.Sample<SamplingFlags>(predictor);
                                finalValue = typename Field::ValueType(predictorValue + .5f * (originalValue - reversedValue));
                            }

                            dstValues.Write(coord, finalValue);

                        }

            } else {

                const auto SamplingFlags = RNFSample::Cubic|WrappingFlags;
                for (unsigned z=margin[2]; z<dims[2]-margin[2]; ++z)
                    for (unsigned y=margin[1]; y<dims[1]-margin[1]; ++y)
                        for (unsigned x=margin[0]; x<dims[0]-margin[0]; ++x) {

                            auto coord = ConvertVector<Coord>(UInt3(x, y, z));
                            const auto predictor = AdvectRK4<SamplingFlags>(velFieldT1, velFieldT0, coord, -deltaTime * velFieldScale);
                            const auto reversedTap = AdvectRK4<SamplingFlags>(velFieldT0, velFieldT1, predictor, deltaTime * velFieldScale);

                            auto originalValue = srcValues.Load(coord);
                            auto reversedValue = srcValues.Sample<SamplingFlags>(reversedTap);

                            Field::ValueType minNeighbour, maxNeighbour;
                            auto predictorValue = LoadWithNearbyRange<SamplingFlags>(minNeighbour, maxNeighbour, srcValues, predictor);
                            auto finalValue = Field::ValueType(predictorValue + .5f * (originalValue - reversedValue));
                            finalValue = MaxAcross(finalValue, minNeighbour);
                            finalValue = MinAcross(finalValue, maxNeighbour);

                            dstValues.Write(coord, finalValue);

                        }

            }

        }

    }

    template<typename Field, typename VelField>
        void PerformAdvection(
            Field dstValues, Field srcValues,
            VelField velFieldT0, VelField velFieldT1,
            float deltaTime, const AdvectionSettings& settings)
    {
            // it's awkward, but we need to convertion between the
            // variables "settings._border..." and the compile time
            // static RNFSample flags. Only a fixed number of variations
            // are supported... others will fall back to clamping in all
            // directions.
        if (    settings._borderX == AdvectionBorder::Wrap
            &&  settings._borderY != AdvectionBorder::Wrap
            &&  settings._borderZ != AdvectionBorder::Wrap) {

            PerformAdvection_Internal<RNFSample::WrapX|RNFSample::ClampY|RNFSample::ClampZ>(
                dstValues, srcValues, velFieldT0, velFieldT1, deltaTime, settings);

        } else if ( settings._borderX != AdvectionBorder::Wrap
            &&      settings._borderY == AdvectionBorder::Wrap
            &&      settings._borderZ != AdvectionBorder::Wrap) {

            PerformAdvection_Internal<RNFSample::ClampX|RNFSample::WrapY|RNFSample::ClampZ>(
                dstValues, srcValues, velFieldT0, velFieldT1, deltaTime, settings);

        } else if ( settings._borderX != AdvectionBorder::Wrap
            &&      settings._borderY != AdvectionBorder::Wrap
            &&      settings._borderZ == AdvectionBorder::Wrap) {

            PerformAdvection_Internal<RNFSample::ClampX|RNFSample::ClampY|RNFSample::WrapZ>(
                dstValues, srcValues, velFieldT0, velFieldT1, deltaTime, settings);

        } else if ( settings._borderX == AdvectionBorder::Wrap
            &&      settings._borderY == AdvectionBorder::Wrap
            &&      settings._borderZ == AdvectionBorder::Wrap) {

            PerformAdvection_Internal<RNFSample::WrapX|RNFSample::WrapY|RNFSample::WrapZ>(
                dstValues, srcValues, velFieldT0, velFieldT1, deltaTime, settings);

        } else if ( settings._borderX == AdvectionBorder::Wrap
            &&      settings._borderY == AdvectionBorder::Wrap
            &&      settings._borderZ != AdvectionBorder::Wrap) {

            PerformAdvection_Internal<RNFSample::WrapX|RNFSample::WrapY|RNFSample::ClampZ>(
                dstValues, srcValues, velFieldT0, velFieldT1, deltaTime, settings);

        } else {

            assert(settings._borderX != AdvectionBorder::Wrap && settings._borderY != AdvectionBorder::Wrap && settings._borderZ != AdvectionBorder::Wrap);
            PerformAdvection_Internal<RNFSample::ClampX|RNFSample::ClampY|RNFSample::ClampZ>(
                dstValues, srcValues, velFieldT0, velFieldT1, deltaTime, settings);

        }
    }

    AdvectionSettings::AdvectionSettings()
    {
        _method = AdvectionMethod::MacCormackRK4;
        _interpolation = AdvectionInterp::Bilinear;
        _subSteps = 4;
        _borderX = AdvectionBorder::Margin;
        _borderY = AdvectionBorder::Margin;
        _borderZ = AdvectionBorder::Margin;
    }

    AdvectionSettings::AdvectionSettings(
        AdvectionMethod method,
        AdvectionInterp interpolation,
        unsigned        subSteps,
        AdvectionBorder borderX, AdvectionBorder borderY, AdvectionBorder borderZ)
    {
        _method = method;
        _interpolation = interpolation;
        _subSteps = subSteps;
        _borderX = borderX;
        _borderY = borderY;
        _borderZ = borderZ;
    }

    template void PerformAdvection(
        ScalarField2D, ScalarField2D,
        VectorField2D, VectorField2D,
        float, const AdvectionSettings&);

    template void PerformAdvection(
        VectorField2D, VectorField2D,
        VectorField2D, VectorField2D,
        float, const AdvectionSettings&);

    template void PerformAdvection(
        ScalarField3D, ScalarField3D,
        VectorField3D, VectorField3D,
        float, const AdvectionSettings&);

    template void PerformAdvection(
        VectorField3D, VectorField3D,
        VectorField3D, VectorField3D,
        float, const AdvectionSettings&);
}


///////////////////////////////////////////////////////////////////////////////
// statistics::survival::data::data::event.hpp                               //
//                                                                           //
//  Copyright 2009 Erwann Rogard. 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)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_SURVIVAL_DATA_DATA_EVENT_HPP_ER_2009
#define BOOST_STATISTICS_SURVIVAL_DATA_DATA_EVENT_HPP_ER_2009
#include <limits>
#include <ostream>
#include <boost/format.hpp>
#include <boost/operators.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/statistics/survival/constant.hpp>
#include <boost/arithmetic/equal.hpp>

namespace boost {
namespace statistics{
namespace survival {
namespace data {

    // Abstraction for failure indicator and event time.
    //
    // B == T can be useful to represent a proportion of failures (see
    // mean_event)
    template<typename T,typename B = bool>
    class event : equality_comparable<event<T,B> >{
        typedef constant<T> const_;
        public:
        typedef T value_type;
        typedef B failure_type;
        // Construction
        event(); //  (f = false, t = infinity)
        explicit event(B isf,value_type rt);
        event(const event&);
        event& operator=(const event&);

        // Access
        B failure()const;
        value_type time()const;

        // Operators
        bool operator==(const event&);

        protected:
        friend class boost::serialization::access;

        template<class Archive>
        void serialize(Archive & ar, const unsigned int version);

        B failure_;         //!censored
        value_type time_;   //since entry time
    };

    template<typename T,typename B>
    std::ostream& operator<<(std::ostream& out,const event<T,B>& e);

    // Implementation //

    //Construct
    template<typename T,typename B>
    event<T,B>::event():failure_(false),time_(const_::inf_){}

    template<typename T,typename B>
    event<T,B>::event(B isf,value_type rt)
    :failure_(isf),time_(rt){}

    template<typename T,typename B>
    event<T,B>::event(const event& that)
    :failure_(that.failure_),time_(that.time_){}

    template<typename T,typename B>
    event<T,B>&
    event<T,B>::operator=(const event& that){
        if(&that!=this){
            failure_ = (that.failure_);
            time_ = (that.time_);
        }
        return *this;
    }

    template<typename T,typename B>
    std::ostream& operator<<(std::ostream& out,const event<T,B>& e){
        static const char* str = "(%1%,%2%)";
        format f(str);
        f % e.failure() % e.time();
        out << f.str();
        return out;
    }

    template<typename T,typename B>
    B event<T,B>::failure()const{ return failure_; }

    template<typename T,typename B>
    typename event<T,B>::value_type event<T,B>::time()const{
        return time_;
    }

    template<typename T,typename B>
    template<class Archive>
    void event<T,B>::serialize(Archive & ar, const unsigned int version)
    {
        ar & failure_;
        ar & time_;
    }

    // Operators
    template<typename T,typename B>
    bool event<T,B>::operator==(const event& e){
        bool eq_1 = arithmetic_tools::equal(
            e.time(),
            (this->time())
        );
        bool eq_2 = arithmetic_tools::equal(
            (this->failure_) , (e.failure())
        );
        return  (eq_1 && eq_2);
    }

}// data
}// survival
}// statistics
}// boost

#endif


#include <Eigen/Dense>
#include <vector>
#include <algorithm>
#include "geometryUtil.hpp"
#include "privdef.hpp"

std::vector<Eigen::Vector3f> cornersWorkspaceAll(const Eigen::Vector3f& corner0, const Eigen::Vector3f& corner1) {
	std::vector<Eigen::Vector3f> corners;
	int index[3];
	for (int i = 0; i < 8; i++) { // for the number of the corners
		int res = i;
		for (int j = 0; j < 3; j++) { // for x, y, z
			index[j] = res % 2;
			res /= 2;
		}
		float x = (index[0] == 0 ? corner0.x() : corner1.x());
		float y = (index[1] == 0 ? corner0.y() : corner1.y());
		float z = (index[2] == 0 ? corner0.z() : corner1.z());
		corners.push_back(Eigen::Vector3f(x, y, z));
	}
	return corners;
}

std::vector<float> range2Points(const Eigen::Vector3f& pos_self, const Eigen::Quaternionf& quo_self, std::vector<Eigen::Vector3f> const& points) {
	std::vector<float> ranges;
	std::for_each(points.begin(), points.end(), [&ranges, &pos_self, &quo_self](Eigen::Vector3f point) {
		ranges.push_back((point - pos_self).dot(quo_self * Eigen::Vector3f::UnitZ()));
		});
	return ranges;
}

namespace dynaman {
	bool isInsideWorkspace(const Eigen::Vector3f& pos, const Eigen::Vector3f& lowerbound, const Eigen::Vector3f& upperbound) {
		Eigen::Vector3f v0 = pos - lowerbound;
		Eigen::Vector3f v1 = pos - upperbound;
		return (v0.x() * v1.x() <= 0) && (v0.y() * v1.y() <= 0) && (v0.z() * v1.z() <= 0);
	}

	Eigen::Matrix3f RotForDeviceId(int device_id, autd::GeometryPtr geo) {
		Eigen::Vector3f pos_origin = geo->position(device_id * NUM_TRANS_IN_UNIT);
		Eigen::Vector3f pos_trans_on_xaxis = geo->position(device_id * NUM_TRANS_IN_UNIT + NUM_TRANS_X - 1);
		Eigen::Vector3f pos_trans_on_yaxis = geo->position((device_id + 1) * NUM_TRANS_IN_UNIT - NUM_TRANS_X);
		Eigen::Vector3f unitX_device = (pos_trans_on_xaxis - pos_origin).normalized();
		Eigen::Vector3f unitY_device = (pos_trans_on_yaxis - pos_origin).normalized();
		Eigen::Vector3f unitZ_device = geo->direction(device_id * NUM_TRANS_IN_UNIT);
		Eigen::Matrix3f rot;
		rot << unitX_device, unitY_device, unitZ_device;
		return rot;
	}

	std::vector<Eigen::Matrix3f> RotsAutd(autd::GeometryPtr geo) {
		std::vector<Eigen::Matrix3f> rots(geo->numDevices());
		for (auto itr_device = rots.begin(); itr_device != rots.end(); itr_device++) {
			*itr_device = RotForDeviceId(std::distance(rots.begin(), itr_device), geo);
		}
		return rots;
	}

	std::vector<Eigen::Matrix3f> RotsAutd(std::shared_ptr<autd::Controller> pAupa) {
		return RotsAutd(pAupa->geometry());
	}

	Eigen::Vector3f CenterForDeviceId(int deviceId, autd::GeometryPtr geo) {
		Eigen::Vector3f center_local(TRANS_SIZE_MM * (NUM_TRANS_X / 2 - 0.5f), TRANS_SIZE_MM * (NUM_TRANS_Y / 2 - 0.5f), 0.f);
		return  RotForDeviceId(deviceId, geo) * center_local + geo->position(NUM_TRANS_IN_UNIT * deviceId);
	}

	Eigen::Matrix3Xf CentersAutd(autd::GeometryPtr geo) {
		Eigen::Matrix3Xf centers(3, geo->numDevices());
		for (int i_autd = 0; i_autd < geo->numDevices(); i_autd++) {
			centers.col(i_autd) = CenterForDeviceId(i_autd, geo);
		}
		return centers;
	}

	Eigen::Matrix3Xf DirectionsAutd(autd::GeometryPtr geo) {
		Eigen::Matrix3Xf directions;
		directions.resize(3, geo->numDevices());
		for (int i_col = 0; i_col < directions.cols(); i_col++) {
			directions.col(i_col) = geo->direction(i_col * NUM_TRANS_IN_UNIT);
		}
		return directions;
	}
}