//
// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>

#include <OpenTissue/core/math/math_basic_types.h>

#define BOOST_AUTO_TEST_MAIN
#include <OpenTissue/utility/utility_push_boost_filter.h>
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/test_tools.hpp>
#include <OpenTissue/utility/utility_pop_boost_filter.h>

template<typename math_types>
void compile_test_math_types()
{
  typedef typename math_types::index_type         index_type;
  typedef typename math_types::real_type          real_type;
  typedef typename math_types::vector3_type       vector3_type;
  typedef typename math_types::matrix3x3_type     matrix3x3_type;
  typedef typename math_types::quaternion_type    quaternion_type;
  typedef typename math_types::coordsys_type      coordsys_type;
  typedef typename math_types::index_vector3_type index_vector3_type;
  typedef typename math_types::value_traits       value_traits;

  for(index_type i=0;i<10;++i);  // micky: right :|

  vector3_type     v;
  matrix3x3_type   m;
  quaternion_type  q;
  coordsys_type    c;
  index_vector3_type iv;

  real_type s1 = value_traits::zero();
  real_type s2 = value_traits::one();
  real_type s3 = value_traits::two();
  real_type s4 = value_traits::pi();
  real_type s5 = value_traits::pi_2();
  real_type s6 = value_traits::infinity();
  real_type s7 = value_traits::degree();
  real_type s8 = value_traits::radian();

  BOOST_CHECK( s1 == value_traits::zero() );
  BOOST_CHECK( s2 == value_traits::one() );
  BOOST_CHECK( s3 == value_traits::two() );
  BOOST_CHECK( s4 == value_traits::pi() );
  BOOST_CHECK( s5 == value_traits::pi_2() );
  BOOST_CHECK( s6 == value_traits::infinity() );
  BOOST_CHECK( s7 == value_traits::degree() );
  BOOST_CHECK( s8 == value_traits::radian() );
}

BOOST_AUTO_TEST_SUITE(opentissue_math_basic_math_types);

    BOOST_AUTO_TEST_CASE(type_and_member_compile_test)
    {
      //--- Compile testing that we can instantiate most common types and access members
      typedef OpenTissue::math::BasicMathTypes<float ,      size_t> type1;
      typedef OpenTissue::math::BasicMathTypes<double,      size_t> type2;
      typedef OpenTissue::math::BasicMathTypes<float ,unsigned int> type3;
      typedef OpenTissue::math::BasicMathTypes<double,unsigned int> type4;
      typedef OpenTissue::math::BasicMathTypes<float ,         int> type5;
      typedef OpenTissue::math::BasicMathTypes<double,         int> type6;

      void (*ptr1)() = &(compile_test_math_types<type1>);
      void (*ptr2)() = &(compile_test_math_types<type2>);
      void (*ptr3)() = &(compile_test_math_types<type3>);
      void (*ptr4)() = &(compile_test_math_types<type4>);
      void (*ptr5)() = &(compile_test_math_types<type5>);
      void (*ptr6)() = &(compile_test_math_types<type6>);

      ptr1 = 0;
      ptr2 = 0;
      ptr3 = 0;
      ptr4 = 0;
      ptr5 = 0;
      ptr6 = 0;
    }

BOOST_AUTO_TEST_SUITE_END();


//=======================================================================
// Copyright 2001 Jeremy G. Siek, Andrew Lumsdaine, Lie-Quan Lee,
//
// 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)
//=======================================================================
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/depth_first_search.hpp>
#include <boost/range/irange.hpp>
#include <boost/pending/indirect_cmp.hpp>

#include <iostream>

using namespace boost;
template < typename TimeMap > class dfs_time_visitor:public default_dfs_visitor {
  typedef typename property_traits < TimeMap >::value_type T;
public:
  dfs_time_visitor(TimeMap dmap, TimeMap fmap, T & t)
:  m_dtimemap(dmap), m_ftimemap(fmap), m_time(t) {
  }
  template < typename Vertex, typename Graph >
    void discover_vertex(Vertex u, const Graph & g) const
  {
    put(m_dtimemap, u, m_time++);
  }
  template < typename Vertex, typename Graph >
    void finish_vertex(Vertex u, const Graph & g) const
  {
    put(m_ftimemap, u, m_time++);
  }
  TimeMap m_dtimemap;
  TimeMap m_ftimemap;
  T & m_time;
};

int
main()
{
  // Select the graph type we wish to use
  typedef adjacency_list < vecS, vecS, directedS > graph_t;
  typedef graph_traits < graph_t >::vertices_size_type size_type;
  // Set up the vertex names
  enum
  { u, v, w, x, y, z, N };
  char name[] = { 'u', 'v', 'w', 'x', 'y', 'z' };
  // Specify the edges in the graph
  typedef std::pair < int, int >E;
  E edge_array[] = { E(u, v), E(u, x), E(x, v), E(y, x),
    E(v, y), E(w, y), E(w, z), E(z, z)
  };
#if defined(BOOST_MSVC) && BOOST_MSVC <= 1300
  graph_t g(N);
  for (std::size_t j = 0; j < sizeof(edge_array) / sizeof(E); ++j)
    add_edge(edge_array[j].first, edge_array[j].second, g);
#else
  graph_t g(edge_array, edge_array + sizeof(edge_array) / sizeof(E), N);
#endif

  // discover time and finish time properties
  std::vector < size_type > dtime(num_vertices(g));
  std::vector < size_type > ftime(num_vertices(g));
  typedef
    iterator_property_map<std::vector<size_type>::iterator,
                          property_map<graph_t, vertex_index_t>::const_type>
    time_pm_type;
  time_pm_type dtime_pm(dtime.begin(), get(vertex_index, g));
  time_pm_type ftime_pm(ftime.begin(), get(vertex_index, g));
  size_type t = 0;
  dfs_time_visitor < time_pm_type >vis(dtime_pm, ftime_pm, t);

  depth_first_search(g, visitor(vis));

  // use std::sort to order the vertices by their discover time
  std::vector < size_type > discover_order(N);
  integer_range < size_type > r(0, N);
  std::copy(r.begin(), r.end(), discover_order.begin());
  std::sort(discover_order.begin(), discover_order.end(),
            indirect_cmp < time_pm_type, std::less < size_type > >(dtime_pm));
  std::cout << "order of discovery: ";
  int i;
  for (i = 0; i < N; ++i)
    std::cout << name[discover_order[i]] << " ";

  std::vector < size_type > finish_order(N);
  std::copy(r.begin(), r.end(), finish_order.begin());
  std::sort(finish_order.begin(), finish_order.end(),
            indirect_cmp < time_pm_type, std::less < size_type > >(ftime_pm));
  std::cout << std::endl << "order of finish: ";
  for (i = 0; i < N; ++i)
    std::cout << name[finish_order[i]] << " ";
  std::cout << std::endl;

  return EXIT_SUCCESS;
}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 *  @file   TangentPreintegration.cpp
 *  @author Frank Dellaert
 *  @author Adam Bry
 **/

#include "TangentPreintegration.h"
#include <gtsam/base/numericalDerivative.h>
#include <boost/make_shared.hpp>

using namespace std;

namespace gtsam {

//------------------------------------------------------------------------------
TangentPreintegration::TangentPreintegration(const boost::shared_ptr<Params>& p,
    const Bias& biasHat) :
    PreintegrationBase(p, biasHat) {
  resetIntegration();
}

//------------------------------------------------------------------------------
void TangentPreintegration::resetIntegration() {
  deltaTij_ = 0.0;
  preintegrated_.setZero();
  preintegrated_H_biasAcc_.setZero();
  preintegrated_H_biasOmega_.setZero();
}

//------------------------------------------------------------------------------
bool TangentPreintegration::equals(const TangentPreintegration& other,
    double tol) const {
  return p_->equals(*other.p_, tol) && std::abs(deltaTij_ - other.deltaTij_) < tol
      && biasHat_.equals(other.biasHat_, tol)
      && equal_with_abs_tol(preintegrated_, other.preintegrated_, tol)
      && equal_with_abs_tol(preintegrated_H_biasAcc_,
          other.preintegrated_H_biasAcc_, tol)
      && equal_with_abs_tol(preintegrated_H_biasOmega_,
          other.preintegrated_H_biasOmega_, tol);
}

//------------------------------------------------------------------------------
// See extensive discussion in ImuFactor.lyx
Vector9 TangentPreintegration::UpdatePreintegrated(const Vector3& a_body,
    const Vector3& w_body, double dt, const Vector9& preintegrated,
    OptionalJacobian<9, 9> A, OptionalJacobian<9, 3> B,
    OptionalJacobian<9, 3> C) {
  const auto theta = preintegrated.segment<3>(0);
  const auto position = preintegrated.segment<3>(3);
  const auto velocity = preintegrated.segment<3>(6);

  // This functor allows for saving computation when exponential map and its
  // derivatives are needed at the same location in so<3>
  so3::DexpFunctor local(theta);

  // Calculate exact mean propagation
  Matrix3 w_tangent_H_theta, invH;
  const Vector3 w_tangent = // angular velocity mapped back to tangent space
      local.applyInvDexp(w_body, A ? &w_tangent_H_theta : 0, C ? &invH : 0);
  const Rot3 R(local.expmap());  // nRb: rotation of body in nav frame
  const Vector3 a_nav = R * a_body;
  const double dt22 = 0.5 * dt * dt;

  Vector9 preintegratedPlus;
  preintegratedPlus <<                          // new preintegrated vector:
      theta + w_tangent * dt,                   // theta
      position + velocity * dt + a_nav * dt22,  // position
      velocity + a_nav * dt;                    // velocity

  if (A) {
    // Exact derivative of R*a with respect to theta:
    const Matrix3 a_nav_H_theta = R.matrix() * skewSymmetric(-a_body) * local.dexp();

    A->setIdentity();
    A->block<3, 3>(0, 0).noalias() += w_tangent_H_theta * dt;  // theta
    A->block<3, 3>(3, 0) = a_nav_H_theta * dt22;  // position wrpt theta...
    A->block<3, 3>(3, 6) = I_3x3 * dt;            // .. and velocity
    A->block<3, 3>(6, 0) = a_nav_H_theta * dt;    // velocity wrpt theta
  }
  if (B) {
    B->block<3, 3>(0, 0) = Z_3x3;
    B->block<3, 3>(3, 0) = R.matrix() * dt22;
    B->block<3, 3>(6, 0) = R.matrix() * dt;
  }
  if (C) {
    C->block<3, 3>(0, 0) = invH * dt;
    C->block<3, 3>(3, 0) = Z_3x3;
    C->block<3, 3>(6, 0) = Z_3x3;
  }

  return preintegratedPlus;
}

//------------------------------------------------------------------------------
void TangentPreintegration::update(const Vector3& measuredAcc,
    const Vector3& measuredOmega, const double dt, Matrix9* A, Matrix93* B,
    Matrix93* C) {

  // std::cout << "TangentPreintegration::update" << std::endl;
  // Correct for bias in the sensor frame
  Vector3 acc = biasHat_.correctAccelerometer(measuredAcc);
  Vector3 omega = biasHat_.correctGyroscope(measuredOmega);

  // Possibly correct for sensor pose by converting to body frame
  Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega;
  if (p().body_P_sensor)
    boost::tie(acc, omega) = correctMeasurementsBySensorPose(acc, omega,
        D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega);

  // Do update
  deltaTij_ += dt;
  preintegrated_ = UpdatePreintegrated(acc, omega, dt, preintegrated_, A, B, C);

  if (p().body_P_sensor) {
    // More complicated derivatives in case of non-trivial sensor pose
    *C *= D_correctedOmega_omega;
    if (!p().body_P_sensor->translation().isZero())
      *C += *B * D_correctedAcc_omega;
    *B *= D_correctedAcc_acc; // NOTE(frank): needs to be last
  }

  // new_H_biasAcc = new_H_old * old_H_biasAcc + new_H_acc * acc_H_biasAcc
  // where acc_H_biasAcc = -I_3x3, hence
  // new_H_biasAcc = new_H_old * old_H_biasAcc - new_H_acc
  preintegrated_H_biasAcc_ = (*A) * preintegrated_H_biasAcc_ - (*B);

  // new_H_biasOmega = new_H_old * old_H_biasOmega + new_H_omega * omega_H_biasOmega
  // where omega_H_biasOmega = -I_3x3, hence
  // new_H_biasOmega = new_H_old * old_H_biasOmega - new_H_omega
  preintegrated_H_biasOmega_ = (*A) * preintegrated_H_biasOmega_ - (*C);
}

//------------------------------------------------------------------------------
Vector9 TangentPreintegration::biasCorrectedDelta(
    const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H) const {
  // We correct for a change between bias_i and the biasHat_ used to integrate
  // This is a simple linear correction with obvious derivatives
  const imuBias::ConstantBias biasIncr = bias_i - biasHat_;
  const Vector9 biasCorrected = preintegrated()
      + preintegrated_H_biasAcc_ * biasIncr.accelerometer()
      + preintegrated_H_biasOmega_ * biasIncr.gyroscope();

  if (H) {
    (*H) << preintegrated_H_biasAcc_, preintegrated_H_biasOmega_;
  }
  return biasCorrected;
}

//------------------------------------------------------------------------------
// sugar for derivative blocks
#define D_R_R(H) (H)->block<3,3>(0,0)
#define D_R_t(H) (H)->block<3,3>(0,3)
#define D_R_v(H) (H)->block<3,3>(0,6)
#define D_t_R(H) (H)->block<3,3>(3,0)
#define D_t_t(H) (H)->block<3,3>(3,3)
#define D_t_v(H) (H)->block<3,3>(3,6)
#define D_v_R(H) (H)->block<3,3>(6,0)
#define D_v_t(H) (H)->block<3,3>(6,3)
#define D_v_v(H) (H)->block<3,3>(6,6)

//------------------------------------------------------------------------------
Vector9 TangentPreintegration::Compose(const Vector9& zeta01,
    const Vector9& zeta12, double deltaT12, OptionalJacobian<9, 9> H1,
    OptionalJacobian<9, 9> H2) {
  const auto t01 = zeta01.segment<3>(0);
  const auto p01 = zeta01.segment<3>(3);
  const auto v01 = zeta01.segment<3>(6);

  const auto t12 = zeta12.segment<3>(0);
  const auto p12 = zeta12.segment<3>(3);
  const auto v12 = zeta12.segment<3>(6);

  Matrix3 R01_H_t01, R12_H_t12;
  const Rot3 R01 = Rot3::Expmap(t01, R01_H_t01);
  const Rot3 R12 = Rot3::Expmap(t12, R12_H_t12);

  Matrix3 R02_H_R01, R02_H_R12; // NOTE(frank): R02_H_R12 == Identity
  const Rot3 R02 = R01.compose(R12, R02_H_R01, R02_H_R12);

  Matrix3 t02_H_R02;
  Vector9 zeta02;
  const Matrix3 R = R01.matrix();
  zeta02 << Rot3::Logmap(R02, t02_H_R02), // theta
  p01 + v01 * deltaT12 + R * p12, // position
  v01 + R * v12; // velocity

  if (H1) {
    H1->setIdentity();
    D_R_R(H1) = t02_H_R02 * R02_H_R01 * R01_H_t01;
    D_t_R(H1) = R * skewSymmetric(-p12) * R01_H_t01;
    D_t_v(H1) = I_3x3 * deltaT12;
    D_v_R(H1) = R * skewSymmetric(-v12) * R01_H_t01;
  }

  if (H2) {
    H2->setZero();
    D_R_R(H2) = t02_H_R02 * R02_H_R12 * R12_H_t12;
    D_t_t(H2) = R;
    D_v_v(H2) = R;
  }

  return zeta02;
}

//------------------------------------------------------------------------------
void TangentPreintegration::mergeWith(const TangentPreintegration& pim12,
    Matrix9* H1, Matrix9* H2) {
  if (!matchesParamsWith(pim12)) {
    throw std::domain_error(
        "Cannot merge pre-integrated measurements with different params");
  }

  if (params()->body_P_sensor) {
    throw std::domain_error(
        "Cannot merge pre-integrated measurements with sensor pose yet");
  }

  const double t01 = deltaTij();
  const double t12 = pim12.deltaTij();
  deltaTij_ = t01 + t12;

  const Vector9 zeta01 = preintegrated();
  Vector9 zeta12 = pim12.preintegrated(); // will be modified.

  const imuBias::ConstantBias bias_incr_for_12 = biasHat() - pim12.biasHat();
  zeta12 += pim12.preintegrated_H_biasOmega_ * bias_incr_for_12.gyroscope()
      + pim12.preintegrated_H_biasAcc_ * bias_incr_for_12.accelerometer();

  preintegrated_ = TangentPreintegration::Compose(zeta01, zeta12, t12, H1, H2);

  preintegrated_H_biasAcc_ = (*H1) * preintegrated_H_biasAcc_
      + (*H2) * pim12.preintegrated_H_biasAcc_;

  preintegrated_H_biasOmega_ = (*H1) * preintegrated_H_biasOmega_
      + (*H2) * pim12.preintegrated_H_biasOmega_;
}

//------------------------------------------------------------------------------

}// namespace gtsam


/**
 * @file HashFunction.hpp
 * @author bwu
 * @brief Hush functions of geometries
 * @version 0.1
 * @date 2022-02-22
 */
#ifndef GENERIC_GEOMETRY_HASHFUNCTION_HPP
#define GENERIC_GEOMETRY_HASHFUNCTION_HPP
#include "Point.hpp"
#include <boost/functional/hash.hpp>
#include <functional>
namespace generic  {
namespace geometry {

template <typename num_type>
struct PointHash
{
};

template <>
struct PointHash<int32_t>
{
    size_t operator()(const Point2D<int32_t> & point) const noexcept
    {
        size_t seed(0);
        boost::hash_combine(seed, point[0]);
        boost::hash_combine(seed, point[1]);
        return seed;
    }
};

template <>
struct PointHash<int64_t>
{
    size_t operator()(const Point2D<int64_t> & point) const noexcept
    {
        size_t seed(0);
        boost::hash_combine(seed, point[0]);
        boost::hash_combine(seed, point[1]);
        return seed;
    }
};

}//namespace geometry
}//namespace generic
#endif//GENERIC_GEOMETRY_HASHFUNCTION_HPP


/*
 * MIT License
 *
 * Copyright (c) 2015 Alexis LE GOADEC
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 *
 */

#include "include/kRegular.hh"
#include <boost/foreach.hpp>

kRegular::kRegular(const boost::shared_ptr<HypergrapheAbstrait>& ptrHypergrapheAbstrait) {
	_ptrHypergrapheAbstrait = ptrHypergrapheAbstrait;
}

void
kRegular::runAlgorithme() {

	_result.setBooleanResult(true);
	AdjacentMatrix matrix ( _ptrHypergrapheAbstrait->getAdjacentMatrix() );

	int compteur = -1;
	BOOST_FOREACH(const auto& e, _ptrHypergrapheAbstrait->getIndexHyperVertex() ) {
		if(compteur==-1) {
			compteur = matrix.getVertexDegree(e.first);
		} else {
			if( (int)matrix.getVertexDegree(e.first) != compteur ) {
				_result.setBooleanResult(false);
			};
		};
	}

	if( compteur==-1 ) _result.setBooleanResult(true);
}

RStructure
kRegular::getResult() const {
	return _result;
}

kRegular::~kRegular() {

}


#define BOOST_TEST_MODULE SolutionTest

#include "solution.hpp"

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

BOOST_AUTO_TEST_SUITE(SolutionSuite)

BOOST_AUTO_TEST_CASE(PlainTest1)
{
    vector<int> heights{2,1,5,6,2,3};
    int result = Solution().largestRectangleArea(heights);

    int expected = 10;

    BOOST_CHECK_EQUAL(result, expected);
}

BOOST_AUTO_TEST_SUITE_END()


#ifndef BINDING_UTILS_HPP
#define BINDING_UTILS_HPP

/// @cond

#include <stddef.h>
#include <iostream>
#include <boost/numeric/bindings/size.hpp>
#include <boost/numeric/bindings/begin.hpp>
#include <boost/numeric/bindings/value_type.hpp>
#include "random.hpp"

/* \file binding_utils.hpp
   \brief Some useful functions to handle ublas matrices and vectors.
   Source = boost bindings examples.
*/

namespace Siconos {
  namespace algebra {

///////////////////////////////
// vectors

// element access:
    template <typename V>
    struct vct_access_traits {
      typedef typename
      boost::numeric::bindings::value_type<V>::type val_t;
      typedef val_t& ref_t;
      static ref_t elem (V& v, size_t i) { return v[i]; }
    };

    template <typename V>
    struct vct_access_traits<V const> {
      typedef typename
      boost::numeric::bindings::value_type<V>::type val_t;
      typedef val_t ref_t;
      static ref_t elem (V const& v, size_t i) { return v[i]; }
    };

    template <typename V>
    inline
    typename vct_access_traits<V>::ref_t elem_v (V& v, size_t i) {
      return vct_access_traits<V>::elem (v, i);
    }

// initialization:
    struct ident {
      size_t operator() (size_t i) const { return i; }
    };
    struct iplus1 {
      size_t operator() (size_t i) const { return i + 1; }
    };
    template <typename T>
    struct const_val {
      T val;
      const_val (T v = T()) : val (v) {}
      T operator() (size_t) const { return val; }
      T operator() (size_t, size_t) const { return val; }
    };
    struct kpp {
      size_t val;
      kpp (size_t v = 0) : val (v) {}
      size_t operator() (size_t) {
        size_t tmp = val;
        ++val;
        return tmp;
      }
      size_t operator() (size_t, size_t) {
        size_t tmp = val;
        ++val;
        return tmp;
      }
    };
    template <typename T>
    struct times_plus {
      T s, a, b;
      times_plus (T ss, T aa = T(), T bb = T()) : s (ss), a (aa), b (bb) {}
      T operator() (size_t i) const {
        return s * (T) i + a;
      }
      T operator() (size_t i, size_t j) const {
        return s * ((T) i + a) + (T) j + b;
      }
    };

    template <typename F, typename V>
    void init_v (V& v, F f = F()) {
      size_t sz
        = boost::numeric::bindings::size( v );
      for (std::size_t i = 0; i < sz; ++i)
        elem_v (v, i) = f (i);
    }

// printing:
    template <typename V>
    void print_v (V const& v, char const* ch = nullptr) {
      if (ch)
        std::cout << ch << ": ";
      size_t sz
        = boost::numeric::bindings::size( v );
      for (std::size_t i = 0; i < sz; ++i)
        std::cout << elem_v (v, i) << " ";
      std::cout << std::endl;
    }

/////////////////////////////////////
// matrices

// element access:
    template <typename M>
    struct matr_access_traits {
      typedef M mat_t;
      typedef typename boost::numeric::bindings::value_type<mat_t>::type val_t;
      typedef typename mat_t::reference ref_t;
      static ref_t elem (mat_t& m, size_t i, size_t j) { return m (i, j); }
    };

#ifdef EIGEN_FORWARDDECLARATIONS_H
    template<typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
    struct matr_access_traits<Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols> > {
      typedef Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols> mat_t;
      typedef typename boost::numeric::bindings::value_type<mat_t>::type val_t;
      typedef val_t& ref_t;
      static ref_t elem (mat_t& m, size_t i, size_t j) { return m (i, j); }
    };
#endif

    template <typename M>
    struct matr_access_traits<M const> {
      typedef M const mat_t;
      typedef typename boost::numeric::bindings::value_type<mat_t>::type val_t;
      typedef val_t ref_t;
      static ref_t elem (mat_t& m, size_t i, size_t j) { return m (i, j); }
    };

    template <typename M>
    inline
    typename matr_access_traits<M>::ref_t elem_m (M& m, size_t i, size_t j) {
      return matr_access_traits<M>::elem (m, i, j);
    }

// initialization:
    struct rws {
      size_t operator() (size_t i, size_t) const { return i; }
    };
    struct rws1 {
      size_t operator() (size_t i, size_t) const { return i + 1; }
    };
    struct cls {
      size_t operator() (size_t, size_t j) const { return j; }
    };
    struct cls1 {
      size_t operator() (size_t, size_t j) const { return j + 1; }
    };

    template <typename F, typename M>
    void init_m (M& m, F f = F()) {
      size_t sz1
        = boost::numeric::bindings::size_row( m );
      size_t sz2
        = boost::numeric::bindings::size_column( m );
      for (std::size_t i = 0; i < sz1; ++i)
        for (std::size_t j = 0; j < sz2; ++j)
          elem_m (m, i, j) = f (i, j);
    }

    template <typename M>
    void init_symm (M& m, char uplo = 'f') {
      size_t n
        = boost::numeric::bindings::size_row( m );
      for (size_t i = 0; i < n; ++i) {
        elem_m (m, i, i) = n;
        for (size_t j = i + 1; j < n; ++j) {
          if (uplo == 'u' || uplo == 'U')
            elem_m (m, i, j) = n - (j - i);
          else if (uplo == 'l' || uplo == 'L')
            elem_m (m, j, i) = n - (j - i);
          else
            elem_m (m, i, j) = elem_m (m, j, i) = n - (j - i);
        }
      }
    }

/** Print a boost-ublas matrix
 */
    template <typename M>
    void print_m (M const& m, char const* ch = nullptr) {
      if (ch)
        std::cout << ch << ":\n";
      size_t sz1
        = boost::numeric::bindings::size_row( m );
      size_t sz2
        = boost::numeric::bindings::size_column( m );
      for (std::size_t i = 0 ; i < sz1 ; ++i) {
        for (std::size_t j = 0 ; j < sz2 ; ++j)
          std::cout << elem_m (m, i, j) << ", ";
        std::cout << std::endl;
      }
      //  std::cout << std::endl;
    }

    template <typename M>
    void print_m_data (M const& m, char const* ch = nullptr) {
      if (ch)
        std::cout << ch << " data:\n";
      using namespace boost::numeric::bindings;
      std::copy( begin_value( m ), end_value( m ), std::ostream_iterator
                 < typename value_type< const M >::type >( std::cout, " " ) );
      std::cout << std::endl;
    }

// Below are functions from boost bindings tests (see ublas_heev.hpp).

    inline float conj(float v) { return v; }
    inline double conj(double v) { return v; }
    inline float real(float v) { return v; }
    inline double real(double v) { return v; }

/** Fill a banded matrix with random values
 */
    template <typename M>
    void fill_banded(M& m) {
      typedef typename M::value_type value_type ;

      int size = m.size2() ;
      int band = m.upper() ;

      for (int i=0; i<size; ++i) {
        for (int j=std::max(0,i-band); j<i; ++j) m(j,i) = random_value<value_type>();
        m(i,i) = real( random_value<value_type>() );
      }
    } // randomize()

/** Fill a symmetric matrix with random values
 */
    template <typename M>
    void fill_sym(M& m) {
      typedef typename M::value_type value_type ;

      int size = m.size2() ;

      for (int i=0; i<size; ++i) {
        for (int j=0; j<i; ++j) {
          m(j,i) = random_value<value_type>();
          m(i,j) = conj( m(j,i) ) ;
        }
        m(i,i) = real( random_value<value_type>() );
      }
    } // randomize()

/** Fill a  matrix with random values
 */
    template <typename M>
    void fill(M& m) {
      typedef typename M::size_type  size_type ;
      typedef typename M::value_type value_type ;

      for (size_type i=0; i<m.size1(); ++i) {
        for (size_type j=0; j<m.size2(); ++j) {
          m(i,j) = random_value<value_type>();
        }
      }
    } // randomize()

  } // Algebra namespace
} // Siconos namespace

/// @endcond

#endif


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   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
//==============================================================================
#ifndef NT2_TOOLBOX_TRIGONOMETRIC_CONSTANTS_INVPIO_2_HPP_INCLUDED
#define NT2_TOOLBOX_TRIGONOMETRIC_CONSTANTS_INVPIO_2_HPP_INCLUDED
/*!
 * \file
**/
#include <boost/simd/sdk/constant/constant.hpp>
#include <boost/simd/sdk/constant/register.hpp>

/*!
 * \ingroup boost_simd_constant
 * \defgroup boost_simd_constant_invpio_2 invpio_2 constant
 *
 * \par Description
 * Constant Invpio_2 : \f$\frac1\pi\f$.
 *
 * \par Header file
 *
 * \code
 * #include <nt2/include/functions/invpio_2.hpp>
 * \endcode
 *
 *
 * \synopsis
 *
 * \code
 * namespace boost::simd
 * {
 *   template <class T,class A0>
 *     meta::call<tag::_invpio_2_(A0)>::type
 *     Invpio_2();
 * }
 * \endcode
 *
 *
 * \param T template parameter of Invpio_2
 *
 * \return type T value
 *
 *
**/

namespace nt2
{
  namespace tag
  {
    // 6.36619772367581382433e-01
    BOOST_SIMD_CONSTANT_REGISTER( Invpio_2, double
                                , 0, 0x3f22f984
                                , 0x3FE45F306DC9C883ll
                                );
  }

  BOOST_SIMD_CONSTANT_IMPLEMENTATION(tag::Invpio_2, Invpio_2);
}

#endif


#include "viscosity3d.h"
#include "array3.h"
//#include "sparse/sparsematrix.h"
//#include "sparse/cgsolver.h"
#include "pcgsolver/pcg_solver.h"
#include <fstream>
#include <cmath>
//#include "wallclocktime.h"

//#include <Eigen/Sparse>

//include ViennaCL stuff for multithreaded (OpenMP) iterative solvers
/*
#define VIENNACL_WITH_EIGEN 1

#include "viennacl/linalg/ilu.hpp"
#include "viennacl/linalg/cg.hpp"
#include "viennacl/linalg/bicgstab.hpp"
#include "viennacl/linalg/gmres.hpp"
#include "viennacl/linalg/norm_2.hpp"
#include "viennacl/linalg/prod.hpp"
*/

static int u_ind(int i, int j, int k, int nx, int ny) {
   return i + j*(nx + 1) + k*(nx + 1)*ny;
}

static int v_ind(int i, int j, int k, int nx, int ny, int nz){
   return i + j*nx + k*nx*(ny + 1) + (nx + 1)*ny*nz;
}

static int w_ind(int i, int j, int k, int nx, int ny, int nz){
   return i + j*nx + k*nx*ny + (nx + 1)*ny*nz + nx*(ny + 1)*nz;
}

Array3c u_state;//(nx+1,ny,nz,0);
Array3c v_state;//(nx,ny+1,nz,0);
Array3c w_state;//(nx,ny,nz+1,0);

SparseMatrixd matrix;//(dim,dim,15);
SparseMatrixd matrix2;//(dim,dim
std::vector<double> rhs;//(dim);
std::vector<double> soln;//(dim);

void advance_viscosity_implicit_weighted(Array3f& u, Array3f& v, Array3f& w,
   const Array3f& vol_u, const Array3f& vol_v, const Array3f& vol_w,
   const Array3f& vol_c, const Array3f& vol_ex, const Array3f& vol_ey, const Array3f& vol_ez,
   const Array3f& solid_phi,
   const Array3f& viscosity, float dt, float dx) {
   float over_dx = 1.0f / dx;
   int nx = solid_phi.ni;
   int ny = solid_phi.nj;
   int nz = solid_phi.nk;
   printf("Creating state arrays.\n");
   std::cout << "Phi-size:" << nx << " " << ny << " " << nz << std::endl;
   int dim = (nx + 1)*ny*nz + nx*(ny + 1)*nz + nx*ny*(nz + 1);
   if (u_state.ni != u.ni) {
      printf("Creating matrices and vectors.\n");
      u_state.resize(nx + 1, ny, nz);
      v_state.resize(nx, ny + 1, nz);
      w_state.resize(nx, ny, nz + 1);
      matrix.resize(dim);
      matrix2.resize(dim);
      rhs.resize(dim);
      soln.resize(dim);
      printf("Done that for good.");
   }

   u_state.assign(0);
   v_state.assign(0);
   w_state.assign(0);
   matrix.zero();
   //matrix2.zero();
   rhs.assign(dim, 0);
   soln.assign(dim, 0);

   const int SOLID = 3;
   const int FLUID = 2;
   //const int AIR = 1;

   //check if interpolated velocity positions are inside solid
   for (int k = 0; k < nz; ++k) for (int j = 0; j < ny; ++j) for (int i = 0; i < nx + 1; ++i) {
      if (i - 1 < 0 || i >= nx || solid_phi(i - 1, j, k) + solid_phi(i, j, k) <= 0)
         u_state(i, j, k) = SOLID;
      else
         u_state(i, j, k) = FLUID;
   }

   for (int k = 0; k < nz; ++k) for (int j = 0; j < ny + 1; ++j) for (int i = 0; i < nx; ++i) {
      if (j - 1 < 0 || j >= ny || solid_phi(i, j - 1, k) + solid_phi(i, j, k) <= 0)
         v_state(i, j, k) = SOLID;
      else
         v_state(i, j, k) = FLUID;
   }

   for (int k = 0; k < nz + 1; ++k) for (int j = 0; j < ny; ++j) for (int i = 0; i < nx; ++i) {
      if (k - 1 < 0 || k >= nz || solid_phi(i, j, k - 1) + solid_phi(i, j, k) <= 0)
         w_state(i, j, k) = SOLID;
      else
         w_state(i, j, k) = FLUID;
   }

   float factor = dt*sqr(over_dx);
   //u-terms
   //2u_xx+ v_xy +uyy + u_zz + w_xz
   printf("Building u-components.\n");
   for (int k = 1; k < nz; ++k) for (int j = 1; j < ny; ++j) for (int i = 1; i < nx; ++i) {

      if (u_state(i, j, k) == FLUID) {
         int index = u_ind(i, j, k, nx, ny);

         rhs[index] = vol_u(i, j, k)*u(i, j, k);
         matrix.set_element(index, index, vol_u(i, j, k));

         float visc_right = viscosity(i, j, k);
         float visc_left = viscosity(i - 1, j, k);
         float vol_right = vol_c(i, j, k);
         float vol_left = vol_c(i - 1, j, k);

         float visc_top = 0.25f*(viscosity(i - 1, j + 1, k) + viscosity(i - 1, j, k) + viscosity(i, j + 1, k) + viscosity(i, j, k));
         float visc_bottom = 0.25f*(viscosity(i - 1, j, k) + viscosity(i - 1, j - 1, k) + viscosity(i, j, k) + viscosity(i, j - 1, k));
         float vol_top = vol_ez(i, j + 1, k);
         float vol_bottom = vol_ez(i, j, k);

         float visc_front = 0.25f*(viscosity(i - 1, j, k + 1) + viscosity(i - 1, j, k) + viscosity(i, j, k + 1) + viscosity(i, j, k));
         float visc_back = 0.25f*(viscosity(i - 1, j, k) + viscosity(i - 1, j, k - 1) + viscosity(i, j, k) + viscosity(i, j, k - 1));
         float vol_front = vol_ey(i, j, k + 1);
         float vol_back = vol_ey(i, j, k);

         //u_x_right
         matrix.add_to_element(index, index, 2 * factor*visc_right*vol_right);
         if (u_state(i + 1, j, k) == FLUID) {
            matrix.add_to_element(index, u_ind(i + 1, j, k, nx, ny), -2 * factor*visc_right*vol_right);
         }
         else if (u_state(i + 1, j, k) == SOLID)
            rhs[index] -= -2 * factor*visc_right*vol_right*u(i + 1, j, k);

         //u_x_left
         matrix.add_to_element(index, index, 2 * factor*visc_left*vol_left);
         if (u_state(i - 1, j, k) == FLUID)
            matrix.add_to_element(index, u_ind(i - 1, j, k, nx, ny), -2 * factor*visc_left*vol_left);
         else if (u_state(i - 1, j, k) == SOLID)
            rhs[index] -= -2 * factor*visc_left*vol_left*u(i - 1, j, k);

         //u_y_top
         matrix.add_to_element(index, index, +factor*visc_top*vol_top);
         if (u_state(i, j + 1, k) == FLUID)
            matrix.add_to_element(index, u_ind(i, j + 1, k, nx, ny), -factor*visc_top*vol_top);
         else if (u_state(i, j + 1, k) == SOLID)
            rhs[index] -= -u(i, j + 1, k)*factor*visc_top*vol_top;

         //u_y_bottom
         matrix.add_to_element(index, index, +factor*visc_bottom*vol_bottom);
         if (u_state(i, j - 1, k) == FLUID)
            matrix.add_to_element(index, u_ind(i, j - 1, k, nx, ny), -factor*visc_bottom*vol_bottom);
         else if (u_state(i, j - 1, k) == SOLID)
            rhs[index] -= -u(i, j - 1, k)*factor*visc_bottom*vol_bottom;

         //u_z_front
         matrix.add_to_element(index, index, +factor*visc_front*vol_front);
         if (u_state(i, j, k + 1) == FLUID)
            matrix.add_to_element(index, u_ind(i, j, k + 1, nx, ny), -factor*visc_front*vol_front);
         else if (u_state(i, j, k + 1) == SOLID)
            rhs[index] -= -u(i, j, k + 1)*factor*visc_front*vol_front;

         //u_z_back
         matrix.add_to_element(index, index, +factor*visc_back*vol_back);
         if (u_state(i, j, k - 1) == FLUID)
            matrix.add_to_element(index, u_ind(i, j, k - 1, nx, ny), -factor*visc_back*vol_back);
         else if (u_state(i, j, k - 1) == SOLID)
            rhs[index] -= -u(i, j, k - 1)*factor*visc_back*vol_back;

         //v_x_top
         if (v_state(i, j + 1, k) == FLUID)
            matrix.add_to_element(index, v_ind(i, j + 1, k, nx, ny, nz), -factor*visc_top*vol_top);
         else if (v_state(i, j + 1, k) == SOLID)
            rhs[index] -= -v(i, j + 1, k)*factor*visc_top*vol_top;

         if (v_state(i - 1, j + 1, k) == FLUID)
            matrix.add_to_element(index, v_ind(i - 1, j + 1, k, nx, ny, nz), factor*visc_top*vol_top);
         else if (v_state(i - 1, j + 1, k) == SOLID)
            rhs[index] -= v(i - 1, j + 1, k)*factor*visc_top*vol_top;

         //v_x_bottom
         if (v_state(i, j, k) == FLUID)
            matrix.add_to_element(index, v_ind(i, j, k, nx, ny, nz), +factor*visc_bottom*vol_bottom);
         else if (v_state(i, j, k) == SOLID)
            rhs[index] -= v(i, j, k)*factor*visc_bottom*vol_bottom;

         if (v_state(i - 1, j, k) == FLUID)
            matrix.add_to_element(index, v_ind(i - 1, j, k, nx, ny, nz), -factor*visc_bottom*vol_bottom);
         else if (v_state(i - 1, j, k) == SOLID)
            rhs[index] -= -v(i - 1, j, k)*factor*visc_bottom*vol_bottom;

         //w_x_front
         if (w_state(i, j, k + 1) == FLUID)
            matrix.add_to_element(index, w_ind(i, j, k + 1, nx, ny, nz), -factor*visc_front*vol_front);
         else if (w_state(i, j, k + 1) == SOLID)
            rhs[index] -= -w(i, j, k + 1)*factor*visc_front*vol_front;

         if (w_state(i - 1, j, k + 1) == FLUID)
            matrix.add_to_element(index, w_ind(i - 1, j, k + 1, nx, ny, nz), factor*visc_front*vol_front);
         else if (w_state(i - 1, j, k + 1) == SOLID)
            rhs[index] -= w(i - 1, j, k + 1)*factor*visc_front*vol_front;

         //w_x_back
         if (w_state(i, j, k) == FLUID)
            matrix.add_to_element(index, w_ind(i, j, k, nx, ny, nz), +factor*visc_back*vol_back);
         else if (w_state(i, j, k) == SOLID)
            rhs[index] -= w(i, j, k)*factor*visc_back*vol_back;

         if (w_state(i - 1, j, k) == FLUID)
            matrix.add_to_element(index, w_ind(i - 1, j, k, nx, ny, nz), -factor*visc_back*vol_back);
         else if (w_state(i - 1, j, k) == SOLID)
            rhs[index] -= -w(i - 1, j, k)*factor*visc_back*vol_back;
      }
   }

   //v-terms
   //vxx + 2vyy + vzz + u_yx + w_yz
   printf("Building v-components.\n");
   for (int k = 1; k < nz; ++k) for (int j = 1; j < ny; ++j) for (int i = 1; i < nx; ++i) {
      if (v_state(i, j, k) == FLUID) {
         int index = v_ind(i, j, k, nx, ny, nz);

         rhs[index] = vol_v(i, j, k)*v(i, j, k);
         matrix.set_element(index, index, vol_v(i, j, k));

         float visc_right = 0.25f*(viscosity(i, j - 1, k) + viscosity(i + 1, j - 1, k) + viscosity(i, j, k) + viscosity(i + 1, j, k));
         float visc_left = 0.25f*(viscosity(i, j - 1, k) + viscosity(i - 1, j - 1, k) + viscosity(i, j, k) + viscosity(i - 1, j, k));
         float vol_right = vol_ez(i + 1, j, k);
         float vol_left = vol_ez(i, j, k);

         float visc_top = viscosity(i, j, k);
         float visc_bottom = viscosity(i, j - 1, k);
         float vol_top = vol_c(i, j, k);
         float vol_bottom = vol_c(i, j - 1, k);

         float visc_front = 0.25f*(viscosity(i, j - 1, k) + viscosity(i, j - 1, k + 1) + viscosity(i, j, k) + viscosity(i, j, k + 1));
         float visc_back = 0.25f*(viscosity(i, j - 1, k) + viscosity(i, j - 1, k - 1) + viscosity(i, j, k) + viscosity(i, j, k - 1));
         float vol_front = vol_ex(i, j, k + 1);
         float vol_back = vol_ex(i, j, k);

         //v_x_right
         matrix.add_to_element(index, index, +factor*visc_right*vol_right);
         if (v_state(i + 1, j, k) == FLUID)
            matrix.add_to_element(index, v_ind(i + 1, j, k, nx, ny, nz), -factor*visc_right*vol_right);
         else if (v_state(i + 1, j, k) == SOLID)
            rhs[index] -= -v(i + 1, j, k)*factor*visc_right*vol_right;

         //v_x_left
         matrix.add_to_element(index, index, +factor*visc_left*vol_left);
         if (v_state(i - 1, j, k) == FLUID)
            matrix.add_to_element(index, v_ind(i - 1, j, k, nx, ny, nz), -factor*visc_left*vol_left);
         else if (v_state(i - 1, j, k) == SOLID)
            rhs[index] -= -v(i - 1, j, k)*factor*visc_left*vol_left;

         //vy_top
         matrix.add_to_element(index, index, +2 * factor*visc_top*vol_top);
         if (v_state(i, j + 1, k) == FLUID)
            matrix.add_to_element(index, v_ind(i, j + 1, k, nx, ny, nz), -2 * factor*visc_top*vol_top);
         else if (v_state(i, j + 1, k) == SOLID)
            rhs[index] -= -2 * factor*visc_top*vol_top*v(i, j + 1, k);

         //vy_bottom
         matrix.add_to_element(index, index, +2 * factor*visc_bottom*vol_bottom);
         if (v_state(i, j - 1, k) == FLUID)
            matrix.add_to_element(index, v_ind(i, j - 1, k, nx, ny, nz), -2 * factor*visc_bottom*vol_bottom);
         else if (v_state(i, j - 1, k) == SOLID)
            rhs[index] -= -2 * factor*visc_bottom*vol_bottom*v(i, j - 1, k);

         //v_z_front
         matrix.add_to_element(index, index, +factor*visc_front*vol_front);
         if (v_state(i, j, k + 1) == FLUID)
            matrix.add_to_element(index, v_ind(i, j, k + 1, nx, ny, nz), -factor*visc_front*vol_front);
         else if (v_state(i, j, k + 1) == SOLID)
            rhs[index] -= -v(i, j, k + 1)*factor*visc_front*vol_front;

         //v_z_back
         matrix.add_to_element(index, index, +factor*visc_back*vol_back);
         if (v_state(i, j, k - 1) == FLUID)
            matrix.add_to_element(index, v_ind(i, j, k - 1, nx, ny, nz), -factor*visc_back*vol_back);
         else if (v_state(i, j, k - 1) == SOLID)
            rhs[index] -= -v(i, j, k - 1)*factor*visc_back*vol_back;

         //u_y_right
         if (u_state(i + 1, j, k) == FLUID)
            matrix.add_to_element(index, u_ind(i + 1, j, k, nx, ny), -factor*visc_right*vol_right);
         else if (u_state(i + 1, j, k) == SOLID)
            rhs[index] -= -u(i + 1, j, k)*factor*visc_right*vol_right;

         if (u_state(i + 1, j - 1, k) == FLUID)
            matrix.add_to_element(index, u_ind(i + 1, j - 1, k, nx, ny), factor*visc_right*vol_right);
         else if (u_state(i + 1, j - 1, k) == SOLID)
            rhs[index] -= u(i + 1, j - 1, k)*factor*visc_right*vol_right;

         //u_y_left
         if (u_state(i, j, k) == FLUID)
            matrix.add_to_element(index, u_ind(i, j, k, nx, ny), factor*visc_left*vol_left);
         else if (u_state(i, j, k) == SOLID)
            rhs[index] -= u(i, j, k)*factor*visc_left*vol_left;

         if (u_state(i, j - 1, k) == FLUID)
            matrix.add_to_element(index, u_ind(i, j - 1, k, nx, ny), -factor*visc_left*vol_left);
         else if (u_state(i, j - 1, k) == SOLID)
            rhs[index] -= -u(i, j - 1, k)*factor*visc_left*vol_left;

         //w_y_front
         if (w_state(i, j, k + 1) == FLUID)
            matrix.add_to_element(index, w_ind(i, j, k + 1, nx, ny, nz), -factor*visc_front*vol_front);
         else if (w_state(i, j, k + 1) == SOLID)
            rhs[index] -= -w(i, j, k + 1)*factor*visc_front*vol_front;

         if (w_state(i, j - 1, k + 1) == FLUID)
            matrix.add_to_element(index, w_ind(i, j - 1, k + 1, nx, ny, nz), factor*visc_front*vol_front);
         else if (w_state(i, j - 1, k + 1) == SOLID)
            rhs[index] -= w(i, j - 1, k + 1)*factor*visc_front*vol_front;

         //w_y_back
         if (w_state(i, j, k) == FLUID)
            matrix.add_to_element(index, w_ind(i, j, k, nx, ny, nz), factor*visc_back*vol_back);
         else if (w_state(i, j, k) == SOLID)
            rhs[index] -= w(i, j, k)*factor*visc_back*vol_back;

         if (w_state(i, j - 1, k) == FLUID)
            matrix.add_to_element(index, w_ind(i, j - 1, k, nx, ny, nz), -factor*visc_back*vol_back);
         else if (w_state(i, j - 1, k) == SOLID)
            rhs[index] -= -w(i, j - 1, k)*factor*visc_back*vol_back;
      }
   }

   //w-terms
   //wxx+ wyy+ 2wzz + u_zx + v_zy
   printf("Building w-components.\n");
   for (int k = 1; k < nz; ++k) for (int j = 1; j < ny; ++j) for (int i = 1; i < nx; ++i) {
      if (w_state(i, j, k) == FLUID) {
         int index = w_ind(i, j, k, nx, ny, nz);
         rhs[index] = vol_w(i, j, k)*w(i, j, k);
         matrix.set_element(index, index, vol_w(i, j, k));

         float visc_right = 0.25f*(viscosity(i, j, k) + viscosity(i, j, k - 1) + viscosity(i + 1, j, k) + viscosity(i + 1, j, k - 1));
         float visc_left = 0.25f*(viscosity(i, j, k) + viscosity(i, j, k - 1) + viscosity(i - 1, j, k) + viscosity(i - 1, j, k - 1));
         float vol_right = vol_ey(i + 1, j, k);
         float vol_left = vol_ey(i, j, k);

         float visc_top = 0.25f*(viscosity(i, j, k) + viscosity(i, j, k - 1) + viscosity(i, j + 1, k) + viscosity(i, j + 1, k - 1));;
         float visc_bottom = 0.25f*(viscosity(i, j, k) + viscosity(i, j, k - 1) + viscosity(i, j - 1, k) + viscosity(i, j - 1, k - 1));;
         float vol_top = vol_ex(i, j + 1, k);
         float vol_bottom = vol_ex(i, j, k);

         float visc_front = viscosity(i, j, k);
         float visc_back = viscosity(i, j, k - 1);
         float vol_front = vol_c(i, j, k);
         float vol_back = vol_c(i, j, k - 1);

         //w_x_right
         matrix.add_to_element(index, index, +factor*visc_right*vol_right);
         if (w_state(i + 1, j, k) == FLUID)
            matrix.add_to_element(index, w_ind(i + 1, j, k, nx, ny, nz), -factor*visc_right*vol_right);
         else if (w_state(i + 1, j, k) == SOLID)
            rhs[index] -= -factor*visc_right*vol_right*w(i + 1, j, k);

         //w_x_left
         matrix.add_to_element(index, index, factor*visc_left*vol_left);
         if (w_state(i - 1, j, k) == FLUID)
            matrix.add_to_element(index, w_ind(i - 1, j, k, nx, ny, nz), -factor*visc_left*vol_left);
         else if (w_state(i - 1, j, k) == SOLID)
            rhs[index] -= -factor*visc_left*vol_left*w(i - 1, j, k);

         //w_y_top
         matrix.add_to_element(index, index, +factor*visc_top*vol_top);
         if (w_state(i, j + 1, k) == FLUID)
            matrix.add_to_element(index, w_ind(i, j + 1, k, nx, ny, nz), -factor*visc_top*vol_top);
         else if (w_state(i, j + 1, k) == SOLID)
            rhs[index] -= -factor*visc_top*vol_top*w(i, j + 1, k);

         //w_y_bottom
         matrix.add_to_element(index, index, factor*visc_bottom*vol_bottom);
         if (w_state(i, j - 1, k) == FLUID)
            matrix.add_to_element(index, w_ind(i, j - 1, k, nx, ny, nz), -factor*visc_bottom*vol_bottom);
         else if (w_state(i, j - 1, k) == SOLID)
            rhs[index] -= -factor*visc_bottom*vol_bottom*w(i, j - 1, k);

         //w_z_front
         matrix.add_to_element(index, index, +2 * factor*visc_front*vol_front);
         if (w_state(i, j, k + 1) == FLUID)
            matrix.add_to_element(index, w_ind(i, j, k + 1, nx, ny, nz), -2 * factor*visc_front*vol_front);
         else if (w_state(i, j, k + 1) == SOLID)
            rhs[index] -= -2 * factor*visc_front*vol_front*w(i, j, k + 1);

         //w_z_back
         matrix.add_to_element(index, index, +2 * factor*visc_back*vol_back);
         if (w_state(i, j, k - 1) == FLUID)
            matrix.add_to_element(index, w_ind(i, j, k - 1, nx, ny, nz), -2 * factor*visc_back*vol_back);
         else if (w_state(i, j, k - 1) == SOLID)
            rhs[index] -= -2 * factor*visc_back*vol_back*w(i, j, k - 1);

         //u_z_right
         if (u_state(i + 1, j, k) == FLUID)
            matrix.add_to_element(index, u_ind(i + 1, j, k, nx, ny), -factor*visc_right*vol_right);
         else if (u_state(i + 1, j, k) == SOLID)
            rhs[index] -= -u(i + 1, j, k)*factor*visc_right*vol_right;

         if (u_state(i + 1, j, k - 1) == FLUID)
            matrix.add_to_element(index, u_ind(i + 1, j, k - 1, nx, ny), factor*visc_right*vol_right);
         else if (u_state(i + 1, j, k - 1) == SOLID)
            rhs[index] -= u(i + 1, j, k - 1)*factor*visc_right*vol_right;

         //u_z_left
         if (u_state(i, j, k) == FLUID)
            matrix.add_to_element(index, u_ind(i, j, k, nx, ny), factor*visc_left*vol_left);
         else if (u_state(i, j, k) == SOLID)
            rhs[index] -= u(i, j, k)*factor*visc_left*vol_left;

         if (u_state(i, j, k - 1) == FLUID)
            matrix.add_to_element(index, u_ind(i, j, k - 1, nx, ny), -factor*visc_left*vol_left);
         else if (u_state(i, j, k - 1) == SOLID)
            rhs[index] -= -u(i, j, k - 1)*factor*visc_left*vol_left;

         //v_z_top
         if (v_state(i, j + 1, k) == FLUID)
            matrix.add_to_element(index, v_ind(i, j + 1, k, nx, ny, nz), -factor*visc_top*vol_top);
         else if (v_state(i, j + 1, k) == SOLID)
            rhs[index] -= -v(i, j + 1, k)*factor*visc_top*vol_top;

         if (v_state(i, j + 1, k - 1) == FLUID)
            matrix.add_to_element(index, v_ind(i, j + 1, k - 1, nx, ny, nz), factor*visc_top*vol_top);
         else if (v_state(i, j + 1, k - 1) == SOLID)
            rhs[index] -= v(i, j + 1, k - 1)*factor*visc_top*vol_top;

         //v_z_bottom
         if (v_state(i, j, k) == FLUID)
            matrix.add_to_element(index, v_ind(i, j, k, nx, ny, nz), +factor*visc_bottom*vol_bottom);
         else if (v_state(i, j, k) == SOLID)
            rhs[index] -= v(i, j, k)*factor*visc_bottom*vol_bottom;

         if (v_state(i, j, k - 1) == FLUID)
            matrix.add_to_element(index, v_ind(i, j, k - 1, nx, ny, nz), -factor*visc_bottom*vol_bottom);
         else if (v_state(i, j, k - 1) == SOLID)
            rhs[index] -= -v(i, j, k - 1)*factor*visc_bottom*vol_bottom;

      }
   }

   ////strip out near zero entries to speed this thing up!
   //printf("Stripping out near-zeros.\n");
   //SparseMatrixd matrix2(matrix.m,matrix.n,15);

   //std::ofstream outfile("matrix.m");
   //matrix.write_matlab(outfile, "A_mat");
   //outfile.close();

   /*
   std::vector<Eigen::Triplet<double> > entries;
   Eigen::SparseMatrix<double> eigen_system(dim, dim);
   Eigen::VectorXd eigen_rhs(dim);

   //copy to eigen matrix
   for (unsigned int row = 0; row < matrix.n; ++row){
   for (unsigned int col = 0; col < matrix.index[row].size(); ++col) {
   int index = matrix.index[row][col];
   double val = matrix(row, index);
   //if(std::abs(val) > 1e-10)
   entries.push_back(Eigen::Triplet<double>(row, index, val));
   //double val = matrix(row,index);
   //if(std::abs(val) > 1e-10)
   //   matrix2.set_element(row,index, val);
   }
   if (matrix.index[row].size() == 0) {
   entries.push_back(Eigen::Triplet<double>(row, row, 1));
   }
   }
   eigen_system.setFromTriplets(entries.begin(), entries.end());

   double solve_start_time, solve_end_time;
   try {
   //try solving with ViennaCL, for a nice multithreaded change of pace.
   std::cout << "Running ViennaCL BiCGStab Viscosity solve." << std::endl;
   //solve_start_time = get_time_in_seconds();

   //copy the data

   viennacl::vector<double> vcl_rhs(rhs.size()), vcl_soln(rhs.size());
   viennacl::compressed_matrix<double> vcl_sparsemat(eigen_system.rows(), eigen_system.cols());

   viennacl::copy(rhs, vcl_rhs);
   viennacl::copy(eigen_system, vcl_sparsemat);
   std::cout << "..Preconditioner\n";
   viennacl::linalg::ilut_tag ilut_config(20U, 0.01, false);
   viennacl::linalg::ilut_precond< viennacl::compressed_matrix<double> > vcl_ilut(vcl_sparsemat, ilut_config);

   std::cout << "..Running solve\n";
   viennacl::linalg::bicgstab_tag custom_tag(1e-10, 1000);
   vcl_soln = viennacl::linalg::solve(vcl_sparsemat, vcl_rhs, custom_tag, vcl_ilut);

   //solve_end_time = get_time_in_seconds();

   viennacl::vector<double> residual(rhs.size());
   residual = viennacl::linalg::prod(vcl_sparsemat, vcl_soln) - vcl_rhs;
   std::cout << "Residual: " << viennacl::linalg::norm_2(residual) << std::endl;

   viennacl::copy(vcl_soln, soln);
   }
   catch (...) {
   std::cout << "ViennaCL FAILED*****\n";
   std::cout << "ViennaCL failed, trying Eigen BiCGStab.\n";
   //solve_start_time = get_time_in_seconds();
   //iterative solver
   Eigen::IncompleteLUT<double> precon;
   Eigen::BiCGSTAB<Eigen::SparseMatrix<double>, Eigen::IncompleteLUT<double> > solver2;
   solver2.compute(eigen_system);
   solver2.setMaxIterations(1000);
   solver2.setTolerance(1e-9);
   Eigen::VectorXd eigen_rhs(dim);
   for (int i = 0; i < dim; ++i) eigen_rhs[i] = rhs[i];

   Eigen::VectorXd solution = solver2.solve(eigen_rhs);
   //solve_end_time = get_time_in_seconds();
   Eigen::VectorXd residual = eigen_system * solution - eigen_rhs;
   std::cout << "Residual magnitude: " << residual.norm() << std::endl;
   if (solver2.info() != Eigen::Success) {
   std::cout << "Solve failed.\n";
   //assert(false);
   //exit(1);
   }
   else {
   std::cout << "Solve succeeded!\n";
   for (int i = 0; i < soln.size(); ++i) {
   soln[i] = solution(i);
   }
   }
   }
   */

   printf("Solving sparse system.\n");
   PCGSolver<double> solver;
   double res_out;
   int iter_out;
   solver.set_solver_parameters(1e-9, 10000, 0.97, 0.1);

   printf("Launching CG\n");
   solver.solve(matrix, rhs, soln, res_out, iter_out);

   std::cout << "Finished with residual :" << res_out << " after iterations: " << iter_out << std::endl;

   if (iter_out >= 1000) {
      printf("\n\n\n***************FAILED******************\n\n\n");
      std::cout << "Residual :" << res_out << " iterations: " << iter_out << std::endl;
      exit(1);
   }

   printf("Copying back.\n");
   for (int k = 0; k < nz; ++k)
   for (int j = 0; j < ny; ++j)
   for (int i = 0; i < nx + 1; ++i) {
      if (u_state(i, j, k) == FLUID) {
         u(i, j, k) = (float)soln[u_ind(i, j, k, nx, ny)];
      }
      else {
         u(i, j, k) = 0;
      }
   }

   for (int k = 0; k < nz; ++k)
   for (int j = 0; j < ny + 1; ++j)
   for (int i = 0; i < nx; ++i) {
      if (v_state(i, j, k) == FLUID) {
         v(i, j, k) = (float)soln[v_ind(i, j, k, nx, ny, nz)];
      }
      else
         v(i, j, k) = 0;
   }

   for (int k = 0; k < nz + 1; ++k)
   for (int j = 0; j < ny; ++j)
   for (int i = 0; i < nx; ++i) {
      if (w_state(i, j, k) == FLUID) {
         w(i, j, k) = (float)soln[w_ind(i, j, k, nx, ny, nz)];
      }
      else
         w(i, j, k) = 0;
   }
   printf("Done copying back.\n");

}


#include <iostream>
#include <vector>
#include <array>
#include <functional>

#include <vexcl/devlist.hpp>
#include <vexcl/vector.hpp>
#include <vexcl/multivector.hpp>
#include <vexcl/generator.hpp>
#include <vexcl/element_index.hpp>

// http://headmyshoulder.github.com/odeint-v2
#include <boost/numeric/odeint.hpp>

namespace odeint = boost::numeric::odeint;

typedef double value_type;
typedef vex::symbolic< value_type > sym_value;
typedef std::array<sym_value, 3> sym_state;

// System  function for Lorenz attractor ensemble ODE.
// [1] http://headmyshoulder.github.com/odeint-v2/doc/boost_numeric_odeint/tutorial/chaotic_systems_and_lyapunov_exponents.html
// This is only used to record operations chain for autogenerated kernel.
struct sys_func
{
    const value_type sigma;
    const value_type b;
    const sym_value &R;

    sys_func(value_type sigma, value_type b, const sym_value &R)
        : sigma(sigma), b(b), R(R) {}

    template <class Sig>
    struct result {
        typedef void type;
    };

    void operator()( const sym_state &x , sym_state &dxdt , value_type ) const
    {
        dxdt[0] = sigma * (x[1] - x[0]);
        dxdt[1] = R * x[0] - x[1] - x[0] * x[2];
        dxdt[2] = x[0] * x[1] - b * x[2];
    }
};

int main( int argc , char **argv )
{
    size_t n;
    const value_type dt = 0.01;
    const value_type t_max = 100.0;

    using namespace std;

    n = argc > 1 ? atoi( argv[1] ) : 1024;

    vex::Context ctx( vex::Filter::DoublePrecision && vex::Filter::Env );
    cout << ctx << endl;

    // Custom kernel body will be recorded here:
    std::ostringstream body;
    vex::generator::set_recorder(body);

    // State types that would become kernel parameters:
    sym_state sym_S = {
        sym_value(sym_value::VectorParameter),
        sym_value(sym_value::VectorParameter),
        sym_value(sym_value::VectorParameter)
    };

    // Const kernel parameter.
    sym_value sym_R(sym_value::VectorParameter, sym_value::Const);

    /* Odeint is modern C++ library for ODE solution. We can use its collection
     * of ODE steppers to generate effective kernel which would compute one
     * iteration of a 4th order Runge-Kutta method. For that, we instantiate
     * appropriate odeint stepper with vex::symbolic<double> as a value type.
     * One iteration of the stepper would provide us with sequence of
     * expressions suitable for generation of requiered kernel.  This technique
     * may be used with any generic algorithms for generation of customized and
     * effective kernels.
     */
    // Symbolic stepper:
    odeint::runge_kutta4<
            sym_state , value_type , sym_state , value_type ,
            odeint::range_algebra , odeint::default_operations
            > sym_stepper;

    sys_func sys(10.0, 8.0 / 3.0, sym_R);
    sym_stepper.do_step(std::ref(sys), sym_S, 0, dt);

    auto kernel = vex::generator::build_kernel(ctx, "lorenz", body.str(),
            sym_S[0], sym_S[1], sym_S[2], sym_R
            );

    // Real state initialization:
    value_type Rmin = 0.1;
    value_type Rmax = 50.0;
    value_type dR   = (Rmax - Rmin) / (n - 1);

    vex::vector<value_type> X(ctx, n);
    vex::vector<value_type> Y(ctx, n);
    vex::vector<value_type> Z(ctx, n);
    vex::vector<value_type> R(ctx, n);

    X = 10.0;
    Y = 10.0;
    Z = 10.0;
    R = Rmin + dR * vex::element_index();

    // Integration loop:
    for(value_type t = 0; t < t_max; t += dt)
        kernel(X, Y, Z, R);

    std::vector< value_type > result( n );
    vex::copy( X , result );
    cout << result[0] << endl;
}

// vim: et


#include "modules/bio_base/dna_multiseq.h"
#include <boost/multi_array.hpp>

// 4 diffs is better than 1 insert/delete, but 5 diffs is worse
const int del_cost = 9;
const int diff_cost = 4;

dna_multiseq::dna_multiseq(const dna_sequence& s1, const dna_sequence& s2)
{
	int s1s = s1.size();
	int s2s = s2.size();
	boost::multi_array<int, 2> cost(boost::extents[s1.size()+1][s2.size()+1]);
	boost::multi_array<int, 2> dir(boost::extents[s1.size()+1][s2.size()+1]);
	cost[s1s][s2s] = 0;
	for(int i = 0; i < s1s; i++)
	{
		cost[i][s2s] = del_cost;
		dir[i][s2s] = 1;
	}
	for(int j = 1; j < s2s; j++)
	{
		cost[s1s][j] = del_cost;
		dir[s1s][j] = 2;
	}
	for(int i = s1s-1; i >= 0; i--)
	{
		for(int j = s2s-1; j >= 0; j--)
		{
			int mcost = cost[i+1][j+1] + (s1[i] != s2[j] ? diff_cost : 0);
			int d1cost = cost[i+1][j] + del_cost;
			int d2cost = cost[i][j+1] + del_cost;
			if (mcost <= d1cost && mcost <= d2cost)
			{
				dir[i][j] = 0;
				cost[i][j] = mcost;
			}
			else if (d1cost <= d2cost)
			{
				dir[i][j] = 1;
				cost[i][j] = d1cost;
			}
			else
			{
				dir[i][j] = 2;
				cost[i][j] = d2cost;
			}
		}
	}

	int i = 0;
	int j = 0;

	m_seqs.resize(2);
	dna_del_seq& o1 = m_seqs[0];
	dna_del_seq& o2 = m_seqs[1];

	while(i < s1s || j < s2s)
	{
		if (dir[i][j] == 0)
		{
			o1.push_back(dna_del_base((char) s1[i]));
			o2.push_back(dna_del_base((char) s2[j]));
			i++; j++;
		}
		else if (dir[i][j] == 1)
		{
			o1.push_back(dna_del_base((char) s1[i]));
			o2.push_back(dna_del_base('.'));
			i++;
		}
		else
		{
			o1.push_back(dna_del_base('.'));
			o2.push_back(dna_del_base((char) s2[j]));
			j++;
		}
	}
}

std::string dna_multiseq::get_string(size_t which)
{
	dna_del_seq& seq = m_seqs[which];
	std::string out;
	for(size_t i = 0; i < seq.size(); i++)
		out += (char) seq[i];
	return out;
}


//==================================================================================================
/*!
  @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_SIMD_FUNCTION_EXPONENTBITS_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_EXPONENTBITS_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/detail/constant/maxexponent.hpp>
#include <boost/simd/constant/nbmantissabits.hpp>
#include <boost/simd/function/bitwise_and.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;
   BOOST_DISPATCH_OVERLOAD_IF(exponentbits_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_<bd::floating_<A0>, X>
                          )
   {
      using result = bd::as_integer_t<A0, signed>;
      BOOST_FORCEINLINE result operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        using s_type = bd::scalar_of_t<A0>;
        using sint_type = bd::scalar_of_t<result>;
        const sint_type me = Maxexponent<s_type>();
        const sint_type nmb= Nbmantissabits<s_type>();
        const result Mask =result((2*me+1)<<nmb);
        return (bitwise_and(Mask, a0));
      }
   };

} } }

#endif


/*    Copyright (c) 2010-2018, 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.
 */

#define BOOST_TEST_MAIN

#include <string>

#include <boost/make_shared.hpp>
#include <boost/test/unit_test.hpp>

#include "Tudat/Mathematics/BasicMathematics/linearAlgebra.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/physicalConstants.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/orbitalElementConversions.h"

#include "Tudat/External/SpiceInterface/spiceInterface.h"
#include "Tudat/Mathematics/NumericalIntegrators/rungeKuttaCoefficients.h"
#include "Tudat/Mathematics/Interpolators/lagrangeInterpolator.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/accelerationModel.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/keplerPropagator.h"
#include "Tudat/InputOutput/basicInputOutput.h"

#include "Tudat/Astrodynamics/BasicAstrodynamics/orbitalElementConversions.h"
#include "Tudat/SimulationSetup/EnvironmentSetup/body.h"
#include "Tudat/Astrodynamics/Propagators/nBodyCowellStateDerivative.h"
#include "Tudat/SimulationSetup/PropagationSetup/dynamicsSimulator.h"
#include "Tudat/Mathematics/NumericalIntegrators/createNumericalIntegrator.h"
#include "Tudat/SimulationSetup/EnvironmentSetup/createBodies.h"
#include "Tudat/SimulationSetup/EstimationSetup/createNumericalSimulator.h"
#include "Tudat/SimulationSetup/EnvironmentSetup/defaultBodies.h"

namespace tudat
{

namespace unit_tests
{

//Using declarations.
using namespace tudat::ephemerides;
using namespace tudat::interpolators;
using namespace tudat::numerical_integrators;
using namespace tudat::spice_interface;
using namespace tudat::simulation_setup;
using namespace tudat::basic_astrodynamics;
using namespace tudat::orbital_element_conversions;
using namespace tudat::propagators;
using namespace tudat;

BOOST_AUTO_TEST_SUITE( test_forwards_backwards_rpopagation )

template< typename TimeType >
std::shared_ptr< IntegratorSettings< TimeType > > getIntegrationSettings(
        const int integratorCase, const int initialTime, const bool propagateForwards )
{
    double initialTimeMultiplier = ( propagateForwards ? 1.0 : -1.0 );
    std::shared_ptr< IntegratorSettings< TimeType > > integratorSettings;
    if( integratorCase == 0 )
    {
        integratorSettings = std::make_shared< IntegratorSettings< TimeType > >
                ( rungeKutta4, initialTime, initialTimeMultiplier * 300.0 );
    }
    else if( integratorCase < 5 )
    {
        RungeKuttaCoefficients::CoefficientSets coefficientSet = RungeKuttaCoefficients::undefinedCoefficientSet;
        if( integratorCase == 1 )
        {
            coefficientSet = RungeKuttaCoefficients::rungeKuttaFehlberg45;
        }
        else if( integratorCase == 2 )
        {
            coefficientSet = RungeKuttaCoefficients::rungeKuttaFehlberg56;

        }
        else if( integratorCase == 3 )
        {
            coefficientSet = RungeKuttaCoefficients::rungeKuttaFehlberg78;

        }
        else if( integratorCase == 4 )
        {
            coefficientSet = RungeKuttaCoefficients::rungeKutta87DormandPrince;

        }
        integratorSettings = std::make_shared< RungeKuttaVariableStepSizeSettings< TimeType > >
                ( initialTime, initialTimeMultiplier * 300.0, coefficientSet, 1.0E-3, 3600.0 );
    }
    return integratorSettings;
}

//! Test to ensure that forward and backward in time integration are properly and consistently performed using different
//! integrators. The state of the Moon is numerically propagated forward, and the back to the original time. The test checks
//! if the state at the midpoint is sufficiently close for the foprward and backward intgrations.
template< typename TimeType = double, typename StateScalarType = double >
Eigen::Matrix< StateScalarType, 6, 1 > propagateForwardBackwards( const int integratorCase )
{
    //Load spice kernels.
    spice_interface::loadStandardSpiceKernels( );

    // Define bodies in simulation.
    std::vector< std::string > bodyNames;
    bodyNames.push_back( "Earth" );
    bodyNames.push_back( "Moon" );
    bodyNames.push_back( "Sun" );
    bodyNames.push_back( "Mars" );
    bodyNames.push_back( "Venus" );

    // Specify initial time
    double initialEphemerisTime = 1.0E7;
    double finalEphemerisTime = initialEphemerisTime + 86400.0;
    double buffer = 3600.0;

    // Create bodies needed in simulation
    std::map< std::string, std::shared_ptr< BodySettings > > bodySettings =
            getDefaultBodySettings( bodyNames, initialEphemerisTime - 2.0 * buffer, finalEphemerisTime + 2.0 * buffer );
    std::dynamic_pointer_cast< InterpolatedSpiceEphemerisSettings >( bodySettings[ "Moon" ]->ephemerisSettings )->
            resetFrameOrigin( "Earth" );
    NamedBodyMap bodyMap = createBodies( bodySettings );
    setGlobalFrameBodyEphemerides( bodyMap, "Earth", "ECLIPJ2000" );

    // Set accelerations between bodies that are to be taken into account.
    SelectedAccelerationMap accelerationMap;
    std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > accelerationsOfMoon;
    accelerationsOfMoon[ "Earth" ].push_back( std::make_shared< AccelerationSettings >( central_gravity ) );
    accelerationsOfMoon[ "Sun" ].push_back( std::make_shared< AccelerationSettings >( central_gravity ) );
    accelerationMap[ "Moon" ] = accelerationsOfMoon;

    // Propagate the moon only
    std::vector< std::string > bodiesToIntegrate;
    bodiesToIntegrate.push_back( "Moon" );
    std::vector< std::string > centralBodies;
    centralBodies.push_back( "Earth" );

    // Define settings for numerical integrator.
    std::shared_ptr< IntegratorSettings< TimeType > > integratorSettings = getIntegrationSettings< TimeType >(
                integratorCase, initialEphemerisTime, true );

    // Propagate forwards
    {

        // Create acceleration models and propagation settings.
        Eigen::Matrix< StateScalarType, 6, 1  > systemInitialState =
                spice_interface::getBodyCartesianStateAtEpoch(
                    bodiesToIntegrate[ 0 ], centralBodies[ 0 ], "ECLIPJ2000", "NONE", initialEphemerisTime ).
                template cast< StateScalarType >( );
        AccelerationMap accelerationModelMap = createAccelerationModelsMap(
                    bodyMap, accelerationMap, bodiesToIntegrate, centralBodies );
        std::shared_ptr< TranslationalStatePropagatorSettings< StateScalarType > > propagatorSettings =
                std::make_shared< TranslationalStatePropagatorSettings< StateScalarType > >
                ( centralBodies, accelerationModelMap, bodiesToIntegrate, systemInitialState, finalEphemerisTime + buffer );

        // Create dynamics simulation object.
        SingleArcDynamicsSimulator< StateScalarType, TimeType > dynamicsSimulator(
                    bodyMap, integratorSettings, propagatorSettings, true, true, true );
    }

    double testTime = initialEphemerisTime + ( finalEphemerisTime - initialEphemerisTime ) / 2.0;
    Eigen::Vector6d forwardState = bodyMap.at( "Moon" )->getEphemeris( )->getCartesianState( testTime );

    // Re-define settings for numerical integrator.
    integratorSettings = getIntegrationSettings< TimeType >( integratorCase, finalEphemerisTime, false );

    // Propagate backwards
    {
        // Create acceleration models and propagation settings.
        Eigen::Matrix< StateScalarType, 6, 1  > systemInitialState =
                spice_interface::getBodyCartesianStateAtEpoch(
                    bodiesToIntegrate[ 0 ], centralBodies[ 0 ], "ECLIPJ2000", "NONE", finalEphemerisTime ).
                template cast< StateScalarType >( );
        AccelerationMap accelerationModelMap = createAccelerationModelsMap(
                    bodyMap, accelerationMap, bodiesToIntegrate, centralBodies );
        std::shared_ptr< TranslationalStatePropagatorSettings< StateScalarType > > propagatorSettings =
                std::make_shared< TranslationalStatePropagatorSettings< StateScalarType > >
                ( centralBodies, accelerationModelMap, bodiesToIntegrate, systemInitialState, initialEphemerisTime - buffer );

        // Create dynamics simulation object.
        SingleArcDynamicsSimulator< StateScalarType, TimeType > dynamicsSimulator(
                    bodyMap, integratorSettings, propagatorSettings, true, true, true );
    }

    Eigen::Vector6d backwardState = bodyMap.at( "Moon" )->getEphemeris( )->getCartesianState( testTime );

    return forwardState - backwardState;
}

BOOST_AUTO_TEST_CASE( testCowellPropagatorKeplerCompare )
{
    for( unsigned int j = 0; j < 5; j++ )
    {
        Eigen::Vector6d stateDifference = propagateForwardBackwards< double, double >( j );
        for( int i = 0; i < 3; i++ )
        {
            BOOST_CHECK_SMALL( std::fabs( stateDifference( i ) ), 0.1 );
            BOOST_CHECK_SMALL( std::fabs( stateDifference( i + 3 ) ), 1.0E-4 );
        }
    }
}

BOOST_AUTO_TEST_SUITE_END( )

}

}


/*!@file
 * @copyright This code is licensed under the 3-clause BSD license.
 *   Copyright ETH Zurich, Laboratory of Physical Chemistry, Reiher Group.
 *   See LICENSE.txt for details.
 */

#include "Molassembler/Shapes/InertialMoments.h"

#include "Molassembler/Shapes/CoordinateSystemTransformation.h"
#include "Molassembler/Shapes/ContinuousMeasures.h"
#include <Eigen/Eigenvalues>

#include "Molassembler/Temple/Functional.h"
#include "Molassembler/Temple/Adaptors/Iota.h"

namespace Scine {
namespace Molassembler {
namespace Shapes {
namespace Detail {

//! Determine degeneracy of intertial moments
unsigned degeneracy(const Eigen::Vector3d& inertialMoments) {
  constexpr double degeneracyEpsilon = 0.05;
  unsigned mdeg = 0;
  if(
    std::fabs(
      (inertialMoments(2) - inertialMoments(1)) / inertialMoments(2)
    ) <= degeneracyEpsilon
  ) {
    mdeg += 1;
  }
  if(
    std::fabs(
      (inertialMoments(1) - inertialMoments(0)) / inertialMoments(2)
    ) <= degeneracyEpsilon
  ) {
    mdeg += 2;
  }

  return 1 + (mdeg + 1) / 2;
}

} // namespace Detail

InertialMoments principalInertialMoments(
  const InertialPositionsType& normalizedPositions
) {
  Eigen::Matrix3d inertialMatrix = Eigen::Matrix3d::Zero(3, 3);
  const unsigned N = normalizedPositions.cols();

  for(unsigned i = 0; i < N; ++i) {
    const auto& vec = normalizedPositions.col(i);
    inertialMatrix(0, 0) += vec.y() * vec.y() + vec.z() * vec.z();
    inertialMatrix(1, 1) += vec.x() * vec.x() + vec.z() * vec.z();
    inertialMatrix(2, 2) += vec.x() * vec.x() + vec.y() * vec.y();
    inertialMatrix(1, 0) -= vec.x() * vec.y(); // xy
    inertialMatrix(2, 0) -= vec.x() * vec.z(); // xz
    inertialMatrix(2, 1) -= vec.y() * vec.z(); // yz
  }

  // Decompose the inertial matrix to get principal axes and inertial moments
  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> decomposition(inertialMatrix);

  InertialMoments result;
  result.moments = decomposition.eigenvalues();
  result.axes = decomposition.eigenvectors();
  return result;
}

Top standardizeTop(Eigen::Ref<InertialPositionsType> normalizedPositions) {
  const unsigned N = normalizedPositions.cols();
  assert(N > 1);

  InertialMoments moments = principalInertialMoments(normalizedPositions);

  const unsigned degeneracy = Detail::degeneracy(moments.moments);

  auto rotateEverything = [&](const CoordinateSystem& sourceSystem) {
    const CoordinateSystem defaultCoordinateSystem {};
    assert(sourceSystem.isRightHanded());
    const auto R = rotationMatrix(sourceSystem, defaultCoordinateSystem);
    // Rotate coordinates
    normalizedPositions = R * normalizedPositions;
    // Rotate inertial moment axes
    moments.axes = R * moments.axes;
  };

  if(moments.moments(0) < 0.1 && degeneracy == 2) {
    // The top is linear: If IA << IB = IC and IA ~ 0. We rotate IA to z
    const CoordinateSystem inertialMomentSystem {
      moments.axes.col(1),
      moments.axes.col(2)
    };
    rotateEverything(inertialMomentSystem);
    assert(moments.axes.col(0).cwiseAbs().isApprox(Eigen::Vector3d::UnitZ(), 1e-10));
    return Top::Line;
  }

  if(degeneracy == 1) {
    /* The top is asymmetric. Rotate the axis with the
     * highest moment of inertia to coincide with z, and the one with second most
     * to coincide with x.
     *
     * To better define orientation, we could look for Cn axes. This is done in
     * another function. No need to burden this function with that here.
     */
    CoordinateSystem inertialMomentSystem {
      moments.axes.col(1), // second highest becomes x
      moments.axes.col(2).cross(moments.axes.col(1)) // y = z.cross(x)
    };
    assert(inertialMomentSystem.z.isApprox(moments.axes.col(2), 1e-10));
    rotateEverything(inertialMomentSystem);
    // Make sure rotation went as intended
    assert(moments.axes.col(2).cwiseAbs().isApprox(Eigen::Vector3d::UnitZ(), 1e-10));
    assert(moments.axes.col(1).cwiseAbs().isApprox(Eigen::Vector3d::UnitX(), 1e-10));

    return Top::Asymmetric;
  }

  if(degeneracy == 2) {
    /* The top is symmetric. The subsets are:
     * - Oblate (disc): IA = IB < IC
     * - Prolate (rugby football): IA < IB = IC
     *
     * We rotate the unique axis to coincide with z (it's probably the site of
     * the highest-order Cn or Sn, and one of the degenerate axes to coincide
     * with x. There could be a C2 on x.
     *
     * This is most likely rare and should occur only for largely undistorted
     * structures. Perhaps we can flowchart point groups here?
     */
    // Calculate Ray's asymmetry parameter
    const double A = 1 / moments.moments(0);
    const double B = 1 / moments.moments(1);
    const double C = 1 / moments.moments(2);
    const double kappa = (2 * B - A - C) / (A - C);
    assert(-1 <= kappa && kappa <= 1);
    if(kappa < 0) {
      // Prolate top. IA is unique
      CoordinateSystem inertialMomentSystem {
        moments.axes.col(1),
        moments.axes.col(2)
      };
      rotateEverything(inertialMomentSystem);
      assert(moments.axes.col(0).cwiseAbs().isApprox(Eigen::Vector3d::UnitZ(), 1e-10));
      return Top::Prolate;
    }

    // Oblate top. IC is unique
    CoordinateSystem inertialMomentSystem {
      moments.axes.col(0),
      moments.axes.col(1)
    };
    rotateEverything(inertialMomentSystem);
    assert(moments.axes.col(2).cwiseAbs().isApprox(Eigen::Vector3d::UnitZ(), 1e-10));
    return Top::Oblate;
  }

  assert(degeneracy == 3);
  /* The top is spherical (IA = IB = IC).
   *
   * Note that there is no reason to rotate anything on the basis of the axes
   * from the inertial moment analysis since any choice of axes gives the
   * spherical symmetry. We can't use those to rotate the system.
   *
   * Rotate an arbitrary position to +z instead (good for Td
   * and Oh octahedral, less so for Oh cubic and Ih, which should be less
   * common)
   */
  unsigned selectedIndex = 0;
  for(; selectedIndex < N; ++selectedIndex) {
    /* As long as the position isn't close to the centroid and it's not exactly
     * the -z vector, we can rotate it
     */
    if(
      normalizedPositions.col(selectedIndex).norm() > 0.2
      && !normalizedPositions.col(selectedIndex).normalized().isApprox(
        -Eigen::Vector3d::UnitZ(),
        1e-10
      )
    ) {
      break;
    }
  }
  assert(selectedIndex != N);

  // Determine axis of rotation as sum of z and position coordinate
  const Eigen::Vector3d rotationAxis = (
    normalizedPositions.col(selectedIndex).normalized()
    + Eigen::Vector3d::UnitZ()
  ).normalized();
  const Eigen::Matrix3d rotationMatrix = Eigen::AngleAxisd(M_PI, rotationAxis).toRotationMatrix();

  // Rotate all coordinates
  for(unsigned i = 0; i < N; ++i) {
    normalizedPositions.col(i) = rotationMatrix * normalizedPositions.col(i);
  }
  // Check that everything went as planned
  assert(normalizedPositions.col(selectedIndex).normalized().cwiseAbs().isApprox(Eigen::Vector3d::UnitZ(), 1e-10));

  return Top::Spherical;
}

unsigned reorientAsymmetricTop(Eigen::Ref<InertialPositionsType> normalizedPositions) {
  const unsigned P = normalizedPositions.cols();
  const auto& axes = Eigen::Matrix3d::Identity();

  struct AxisBest {
    unsigned order = 1;
    double csm = 1; // This functions much like a detection threshold below
    unsigned axisIndex;

    AxisBest(unsigned index) : axisIndex(index) {}

    bool operator < (const AxisBest& other) const {
      return order > other.order;
    }
  };

  auto orderedAxisBest = Temple::sorted(
    Temple::map(
      Temple::iota<unsigned>(3),
      [&](const unsigned axisIndex) -> AxisBest {
        const Eigen::Vector3d axis = axes.col(axisIndex);
        AxisBest best {axisIndex};
        for(unsigned n = 2; n <= P; ++n) {
          const double axisCSM = Continuous::Fixed::element(normalizedPositions, Elements::Rotation::Cn(axis, n));
          if(axisCSM < best.csm) {
            best.order = n;
            best.csm = axisCSM;
          }
        }
        return best;
      }
    )
  );

  if(orderedAxisBest.front().order > 1) {
    /* Only mess with the coordinate frame if any sort of axis was found.
     * We want the second-highest order axis on x, highest order axis along z,
     * doesn't really matter if +z or -z
     */
    const CoordinateSystem highestOrderSystem {
      axes.col(orderedAxisBest.at(1).axisIndex),
      axes.col(orderedAxisBest.back().axisIndex)
    };

    normalizedPositions = rotationMatrix(highestOrderSystem, {}) * normalizedPositions;
  }

  return orderedAxisBest.front().order;
}

} // namespace Shapes
} // namespace Molassembler
} // namespace Scine


#include <iostream>
#include <fstream>
#include <string>
#include <Eigen/Cholesky>
#include "../headers/autodiff.h"
#include <chrono>

DECLARE_DIFFSCALAR_BASE();
using namespace std;
using namespace std::chrono;
int main(int argc, char **argv)
{
   typedef Eigen::Matrix<double, 1, 1> Gradient;

   string output_filename = argv[1];
   cout << output_filename << endl;

   typedef DScalar1<double, Gradient> DScalar;

   int num_params = 4010;
   int num_vars = 1;

   Eigen::VectorXd args(num_params * num_vars);
   Eigen::VectorXd ders(num_params * num_vars);

   std::ifstream file("./tests/utils/params.txt");
   int i = 0;
   for (std::string line; std::getline(file, line);)
   {
       args(i) = stod(line.c_str());
       i++;
   }
   file.close();

   ofstream outfile;
   outfile.open(output_filename);

   auto start = high_resolution_clock::now();
   for (int index = 0; index < num_params; index++)
   {
       /* There are two independent variables */
       DiffScalarBase::setVariableCount(1);
		DScalar k(0, args[index * 1 + 0]);
		DScalar Fx = ((k*k+3*k)-k/4)/k+k*k*k*k+k*k*(22/7*k)+k*k*k*k*k*k*k*k*k;
		ders[index * 1 + 0] = Fx.getGradient()(0);
   }

   auto stop = high_resolution_clock::now();
   auto duration = duration_cast<microseconds>(stop - start);
   outfile << (double)duration.count() / 1000000.0 << " ";
   for (int i = 0; i < num_params * num_vars; i++)
   {
        ostringstream ss;
        ss.precision(3);
       ss << fixed << ders[i];
       outfile << ss.str() << " ";
   }

   outfile.close();

   return 0;
}


#include "ceiling.hpp"
#include "ast.hpp"
#include "parsers.hpp"
#include "types.hpp"
#include <boost/variant.hpp>
#include <stdexcept>
namespace HT
{
    void ceiling(PASTNode astnode, ParsersHelper& ph)
    {
        auto myParserHelper(ph);
        if (astnode->ch.size()!=2)
          throw std::runtime_error("ceiling can only have one parameter");
        auto & secondCh = *astnode->ch.rbegin();
        ph.parse(secondCh);
        if (secondCh->token.tokenType != Complex || ! boost::get<ComplexType>(secondCh->token.info).isReal())
          throw std::runtime_error("The argument of ceiling must be real");
        auto cast = boost::get<ComplexType>(secondCh->token.info);

        astnode->type = Simple;
        astnode->token.tokenType = Complex;
        if (!cast.exact())
        astnode->token.info = ComplexType(
                    std::ceil(
                        cast.toinexact().getRealD()
                        ));
        else
        {
            auto rat = cast.getRealR();
            auto ans = rat.getUp() / rat.getDown();
            if (rat.getSign())
              if (rat.getUp() % rat.getDown() != 0)
                astnode->token.info = ComplexType( ans +1);
            else
              astnode->token.info = ComplexType(ans);
            else
              astnode->token.info = ComplexType( (ans).setSign(false) );
        }

        astnode->remove();
    }

}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2014 MetaScale SAS
//
//          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
//==============================================================================
#ifndef BOOST_SIMD_ARITHMETIC_FUNCTIONS_GENERIC_MOD_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_GENERIC_MOD_HPP_INCLUDED
#include <boost/simd/arithmetic/functions/mod.hpp>
#include <boost/simd/include/functions/simd/selsub.hpp>
#include <boost/simd/include/functions/simd/is_nez.hpp>
#include <boost/simd/include/functions/simd/divfloor.hpp>
#include <boost/simd/include/functions/simd/idivfloor.hpp>
#include <boost/simd/include/functions/simd/multiplies.hpp>
#include <boost/dispatch/attributes.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( mod_, tag::cpu_
                                    , (A0)
                                    , (generic_<floating_<A0> >)
                                      (generic_<floating_<A0> >)
                                    )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return selsub(boost::simd::is_nez(a1),a0,
                    divfloor(a0,a1)*a1
                   );
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( mod_, tag::cpu_
                                    , (A0)
                                    , (generic_<arithmetic_<A0> >)
                                      (generic_<arithmetic_<A0> >)
                                    )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return selsub(is_nez(a1),a0,
                    boost::simd::multiplies(idivfloor(a0,a1), a1)
                   );

    }
  };

} } }

#endif


#pragma once
#ifndef __Matrix_H_
#define __Matrix_H_

#define _USE_MATH_DEFINES
#include <math.h>
#include <vector>

#include <Eigen/Dense>

#define P_X	0
#define P_Y	1
#define P_Z	2

#define degToRad(angleInDegrees) ((angleInDegrees) * M_PI / 180.0)
#define radToDeg(angleInRadians) ((angleInRadians) * 180.0 / M_PI)

using namespace Eigen;

typedef struct
{
	std::vector<int> point;
	std::vector<Eigen::Vector2f> point2D;
	std::vector<Eigen::Vector3f> point3D;
} sp_point;

std::vector<Vector3f> slope_3d(Vector3f alpha, Vector3f omega);
Vector3f unit_vector(Vector3f vec);
Matrix4f coor(float roll, float pitch, float yaw, Vector3f point);

std::vector<Vector3f> slope_3d(Vector3f alpha, Vector3f omega);
Vector4f plane_vector(Vector3f a, Vector3f b, Vector3f c);
Vector3f nose_projection(Vector4f p_vector, std::vector<Vector3f> nose);

#else
#endif


#ifndef INTERSIM_DATA_STATE_H
#define INTERSIM_DATA_STATE_H

#include <Core_Defines.h>

#include <Eigen/Dense>
#include <Eigen/Sparse>

#include <spdlog/spdlog.h>

#include <memory>
#include <vector>

namespace InterSim
{
	typedef std::shared_ptr<Eigen::Matrix<scalar, Eigen::Dynamic, Eigen::Dynamic>> field;
	typedef Eigen::SparseMatrix<scalar> sparse_matrix;
}

/*
State
The State struct is passed around in an application to make the
simulation's state available.
The State contains all necessary system information and
provides a few utilities (pointers) to commonly used contents.
*/
struct State
{
	int nx;
	int ny;
	int nz;

	std::vector<InterSim::field> data;
	std::shared_ptr<InterSim::sparse_matrix> matrix;

	// Log
	std::shared_ptr<spdlog::logger> log;
};

#endif


// Demonstration of calculating Elmore delay with a graph
// to accompany "Analyzing On-Chip Interconnect with Modern C++"
// Author: Jeff Trull <edaskel@att.net>

/*
Copyright (c) 2014 Jeffrey E. Trull

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/

#include <iostream>
#include <fstream>

#include <boost/graph/visitors.hpp>
#include <boost/graph/undirected_dfs.hpp>
#include <boost/graph/graphviz.hpp>
#include <boost/units/systems/si/time.hpp>

#include "ckt_graph.h"

// For Elmore we need two visitors:
// 1) To calculate total capacitance at and "below" each node
// 2) To sum resistor delays to each node from a chosen input

template<typename Graph>
struct cap_summing_visitor : boost::default_dfs_visitor {
    typedef typename boost::graph_traits<Graph>::edge_descriptor edge_t;
    typedef typename boost::graph_traits<Graph>::vertex_descriptor vertex_t;

    // This edge variant visitor returns the correct contribution for an edge
    // to a downstream node: the value on the edge, if a capacitor, or
    // the supplied downstream capacitance, if a resistor.
    struct cap_summer : boost::static_visitor<capacitor_value_t> {
        capacitor_value_t operator()(resistor_value_t const&) const {
            return downstream_;   // count downstream, not edge
        }
        capacitor_value_t operator()(capacitor_value_t const& c) const {
            return c;             // count edge but not downstream (i.e., lump to gnd)
        }
        cap_summer(capacitor_value_t downstream) : downstream_(downstream) {}
    private:
        capacitor_value_t downstream_;
    };

    void tree_edge(edge_t e, Graph const& g) {
        // remember the predecessor so later we can avoid summing up capacitance from back edges
        predecessors_[target(e, g)] = source(e, g);
    }

    void finish_vertex(vertex_t u, Graph const& g) {
        // We want to sum only *downstream* capacitance
        // However, circuits are undirected, and the downstreamness of our search
        // is only due to the order in which we encountered vertices
        // a "tree edge" is downstream, and a "back edge" is upstream

        downstream_caps_[u] = capacitor_value_t();
        for (auto e : boost::make_iterator_range(out_edges(u, g))) {
            // detect back edge, i.e., an edge to an already-visited node
            // this won't work correctly for circuits with resistive loops
            if ((predecessors_[target(e, g)] == u) || (target(e, g) == g.gnd())) {
                // our first time at this node, OR the node is gnd, in which case this is normal
                capacitor_value_t contr = boost::apply_visitor(cap_summer(downstream_caps_[target(e, g)]), g[e]);
                downstream_caps_[u] += contr;
            }
        }
    }

    // finish_edge never seems to get called

    cap_summing_visitor(std::vector<capacitor_value_t>& capsvec)
        : downstream_caps_(capsvec), predecessors_(capsvec.size()) {}

private:
    std::vector<capacitor_value_t>& downstream_caps_;
    std::vector<vertex_t> predecessors_;
};

typedef quantity<si::time> delay_t;

// adds up delays.  To be run on filtered (R-only) graph
template<typename Graph>
struct delay_calculating_visitor : boost::default_dfs_visitor {
    typedef typename boost::graph_traits<Graph>::edge_descriptor edge_t;
    typedef typename boost::graph_traits<Graph>::vertex_descriptor vertex_t;

    void start_vertex(vertex_t u, Graph const&) {
        // this hook is called once at the beginning;
        // we can use it to initialize the delay from the tree root
        delays_[u] = delay_t();
    }

    struct res_summer : boost::static_visitor<resistor_value_t> {
        resistor_value_t operator()(resistor_value_t const& r) const {
            return r;
        }
        resistor_value_t operator()(capacitor_value_t const&) const {
            assert(0);   // program bug - we should operate on a filtered graph
            return resistor_value_t();
        }
    };

    void tree_edge(edge_t e, Graph const& g) {
        delays_[target(e, g)] = delays_[source(e, g)] +
            boost::apply_visitor(res_summer(), g[e]) *
            downstream_caps_[target(e, g)];
    }

    void back_edge(edge_t, Graph const&) {
        // Resistive loops will break this algorithm - you could put a check here
    }

    delay_calculating_visitor(std::vector<capacitor_value_t>& capsvec,
                              std::vector<delay_t>& delays)
        : downstream_caps_(capsvec), delays_(delays) {}

private:
    std::vector<capacitor_value_t>& downstream_caps_;   // bottom-up
    std::vector<delay_t>&          delays_;            // top-down
};

int main() {
    using namespace boost;
    using namespace std;

    quantity<si::resistance> kohm(1.0 * units::si::kilo  * units::si::ohms);
    quantity<si::capacitance> ff(1.0 * units::si::femto * units::si::farads);

    // Coupling test case
    ckt_graph_t coupling_test;
    auto gnd = coupling_test.gnd();

    auto vagg = add_vertex("vagg", coupling_test);   // driver voltage source
    auto n1   = add_vertex("n1",   coupling_test);
    add_edge(vagg, n1, 0.1*kohm,   coupling_test);   // driver impedance
    auto n2   = add_vertex("n2",   coupling_test);   // central node - where coupling occurs
    add_edge(n1, n2, 1.0*kohm,     coupling_test);   // first "pi" model, aggressor side
    add_edge(n1, gnd, 50.0*ff,     coupling_test);   // caps for first "pi"
    add_edge(n2, gnd, 50.0*ff,     coupling_test);
    auto n3   = add_vertex("n3",   coupling_test);   // aggressor-side receiver
    add_edge(n2, n3, 1.0*kohm,     coupling_test);   // second "pi" model, aggressor side
    add_edge(n2, gnd, 50.0*ff,     coupling_test);   // caps for first "pi"
    add_edge(n3, gnd, 50.0*ff,     coupling_test);
    add_edge(n3, gnd, 20.0*ff,     coupling_test);   // aggressor side receiver load

    // then the same thing over again for the victim side
    auto vvic = add_vertex("vvic", coupling_test);   // driver voltage source
    auto n5   = add_vertex("n5",   coupling_test);
    add_edge(vvic, n5, 0.1*kohm,   coupling_test);   // driver impedance
    auto n6   = add_vertex("n6",   coupling_test);   // central node
    add_edge(n5, n6, 1.0*kohm,     coupling_test);   // first "pi" model
    add_edge(n5, gnd, 50.0*ff,     coupling_test);
    add_edge(n6, gnd, 50.0*ff,     coupling_test);
    auto n7   = add_vertex("n7",   coupling_test);   // victim-side receiver
    add_edge(n6, n7, 1.0*kohm,     coupling_test);   // second "pi" model
    add_edge(n6, gnd, 50.0*ff,     coupling_test);
    add_edge(n7, gnd, 50.0*ff,     coupling_test);
    add_edge(n7, gnd, 20.0*ff,     coupling_test);   // victim side receiver load

    // coupling capacitor between the two signal traces
    add_edge(n2, n6, 100.0*ff,     coupling_test);

    // debug output via Dot
    /*
    ofstream dbg("/tmp/coupling_test.dot");
    write_graphviz(dbg, coupling_test,
                   make_label_writer(get(vertex_bundle, coupling_test)),
                   make_label_writer(get(edge_bundle, coupling_test)));
    */

    // calculate Elmore delay
    // Create visitors
    vector<capacitor_value_t> downstream_caps(num_vertices(coupling_test));
    cap_summing_visitor<ckt_graph_t> capvis(downstream_caps);

    // undirected DFS requires edge color map
    map<ckt_graph_t::edge_descriptor, default_color_type> edge_colors;
    auto cpmap = make_assoc_property_map(edge_colors);

    // perform first pass and sum capacitances
    undirected_dfs(coupling_test, edge_color_map(cpmap).visitor(capvis).root_vertex(vagg));

    typedef filtered_graph<ckt_graph_t, resistors_only> resonly_graph_t;
    resonly_graph_t res_graph(coupling_test, resistors_only(&coupling_test));

    vector<delay_t>          delays(num_vertices(coupling_test));
    delay_calculating_visitor<resonly_graph_t>
                                     delvis(downstream_caps, delays);

    // create *vertex* color map for depth_first_visit
    auto vindex_map = typed_identity_property_map<size_t>();
    vector<default_color_type> colorvec(num_vertices(coupling_test));   // underlying storage
    auto cvpmap = make_iterator_property_map(colorvec.begin(), vindex_map);

    depth_first_visit(res_graph, vagg, delvis, cvpmap);

    cout << "Elmore delay of aggressor net: " << delays.at(n3) << endl;

    // If you want to calculate the delay from a different node you have to recalculate
    // both capacitance and delays starting with that new node
}