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

#include <algorithm>
#include <Eigen/Core>
#include <Eigen/Eigen>
#include <cmath>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <memory>
#include <object_detection/object_polygon_detail.hpp>
#include <string>
#include <vector>

namespace autoware
{
namespace rviz_plugins
{
namespace object_detection
{
namespace detail
{
using Marker = visualization_msgs::msg::Marker;

visualization_msgs::msg::Marker::SharedPtr get_path_confidence_marker_ptr(
  const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path,
  const std_msgs::msg::ColorRGBA & path_confidence_color)
{
  auto marker_ptr = std::make_shared<Marker>();
  marker_ptr->type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
  marker_ptr->ns = std::string("path confidence");
  marker_ptr->action = visualization_msgs::msg::Marker::MODIFY;
  marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.2);
  marker_ptr->scale.x = 0.5;
  marker_ptr->scale.y = 0.5;
  marker_ptr->scale.z = 0.5;
  marker_ptr->pose = initPose();
  marker_ptr->color = path_confidence_color;
  marker_ptr->pose.position = predicted_path.path.back().position;
  marker_ptr->text = std::to_string(predicted_path.confidence);
  marker_ptr->color.a = std::max(
    static_cast<double>(std::min(static_cast<double>(predicted_path.confidence), 0.999)), 0.5);
  return marker_ptr;
}

visualization_msgs::msg::Marker::SharedPtr get_predicted_path_marker_ptr(
  const autoware_auto_perception_msgs::msg::Shape & shape,
  const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path,
  const std_msgs::msg::ColorRGBA & predicted_path_color)
{
  auto marker_ptr = std::make_shared<Marker>();
  marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST;
  marker_ptr->ns = std::string("path");
  marker_ptr->action = visualization_msgs::msg::Marker::MODIFY;
  marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.2);
  marker_ptr->pose = initPose();
  marker_ptr->color = predicted_path_color;
  marker_ptr->color.a = std::max(
    static_cast<double>(std::min(static_cast<double>(predicted_path.confidence), 0.999)), 0.5);
  marker_ptr->scale.x = 0.03 * marker_ptr->color.a;
  calc_path_line_list(predicted_path, marker_ptr->points);
  for (size_t k = 0; k < marker_ptr->points.size(); ++k) {
    marker_ptr->points.at(k).z -= shape.dimensions.z / 2.0;
  }
  return marker_ptr;
}

visualization_msgs::msg::Marker::SharedPtr get_twist_marker_ptr(
  const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance,
  const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance)
{
  auto marker_ptr = std::make_shared<Marker>();
  marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST;
  marker_ptr->ns = std::string("twist");
  marker_ptr->scale.x = 0.03;
  marker_ptr->action = visualization_msgs::msg::Marker::MODIFY;
  marker_ptr->pose = pose_with_covariance.pose;

  geometry_msgs::msg::Point pt_s;
  pt_s.x = 0.0;
  pt_s.y = 0.0;
  pt_s.z = 0.0;
  marker_ptr->points.push_back(pt_s);

  geometry_msgs::msg::Point pt_e;
  pt_e.x = twist_with_covariance.twist.linear.x;
  pt_e.y = twist_with_covariance.twist.linear.y;
  pt_e.z = twist_with_covariance.twist.linear.z;
  marker_ptr->points.push_back(pt_e);

  marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.2);
  marker_ptr->color.a = 0.999;
  marker_ptr->color.r = 1.0;
  marker_ptr->color.g = 0.0;
  marker_ptr->color.b = 0.0;

  return marker_ptr;
}

visualization_msgs::msg::Marker::SharedPtr get_velocity_text_marker_ptr(
  const geometry_msgs::msg::Twist & twist, const geometry_msgs::msg::Point & vis_pos,
  const std_msgs::msg::ColorRGBA & color_rgba)
{
  auto marker_ptr = std::make_shared<Marker>();
  marker_ptr->type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
  marker_ptr->ns = std::string("velocity");
  marker_ptr->scale.x = 0.5;
  marker_ptr->scale.z = 0.5;

  double vel = std::sqrt(
    twist.linear.x * twist.linear.x + twist.linear.y * twist.linear.y +
    twist.linear.z * twist.linear.z);
  marker_ptr->text = std::to_string(static_cast<int>(vel * 3.6)) + std::string("[km/h]");
  marker_ptr->action = visualization_msgs::msg::Marker::MODIFY;
  marker_ptr->pose.position = vis_pos;
  marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.2);
  marker_ptr->color = color_rgba;
  return marker_ptr;
}

visualization_msgs::msg::Marker::SharedPtr get_pose_with_covariance_marker_ptr(
  const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance)
{
  auto marker_ptr = std::make_shared<Marker>();
  marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST;
  marker_ptr->ns = std::string("position covariance");
  marker_ptr->scale.x = 0.03;
  marker_ptr->action = visualization_msgs::msg::Marker::MODIFY;
  marker_ptr->pose = pose_with_covariance.pose;
  marker_ptr->pose.orientation.x = 0.0;
  marker_ptr->pose.orientation.y = 0.0;
  marker_ptr->pose.orientation.z = 0.0;
  marker_ptr->pose.orientation.w = 1.0;
  geometry_msgs::msg::Point point;
  Eigen::Matrix2d eigen_pose_with_covariance;
  eigen_pose_with_covariance << pose_with_covariance.covariance[0],
    pose_with_covariance.covariance[1], pose_with_covariance.covariance[6],
    pose_with_covariance.covariance[7];
  Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> solver(eigen_pose_with_covariance);
  double sigma1 = 2.448 * std::sqrt(solver.eigenvalues().x());  // 2.448 sigma is 95%
  double sigma2 = 2.448 * std::sqrt(solver.eigenvalues().y());  // 2.448 sigma is 95%
  Eigen::Vector2d e1 = solver.eigenvectors().col(0);
  Eigen::Vector2d e2 = solver.eigenvectors().col(1);
  point.x = -e1.x() * sigma1;
  point.y = -e1.y() * sigma1;
  point.z = 0;
  marker_ptr->points.push_back(point);
  point.x = e1.x() * sigma1;
  point.y = e1.y() * sigma1;
  point.z = 0;
  marker_ptr->points.push_back(point);
  point.x = -e2.x() * sigma2;
  point.y = -e2.y() * sigma2;
  point.z = 0;
  marker_ptr->points.push_back(point);
  point.x = e2.x() * sigma2;
  point.y = e2.y() * sigma2;
  point.z = 0;
  marker_ptr->points.push_back(point);
  marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.2);
  marker_ptr->color.a = 0.999;
  marker_ptr->color.r = 1.0;
  marker_ptr->color.g = 1.0;
  marker_ptr->color.b = 1.0;
  return marker_ptr;
}

visualization_msgs::msg::Marker::SharedPtr get_uuid_marker_ptr(
  const std::string & uuid, const geometry_msgs::msg::Point & centroid,
  const std_msgs::msg::ColorRGBA & color_rgba)
{
  auto marker_ptr = std::make_shared<Marker>();
  marker_ptr->type = Marker::TEXT_VIEW_FACING;
  marker_ptr->ns = std::string("uuid");
  marker_ptr->text = uuid.substr(0, 4);
  marker_ptr->action = Marker::MODIFY;
  marker_ptr->scale.z = 0.5;
  marker_ptr->color = color_rgba;
  marker_ptr->pose.position = centroid;
  return marker_ptr;
}

visualization_msgs::msg::Marker::SharedPtr get_label_marker_ptr(
  const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation,
  const std::string label, const std_msgs::msg::ColorRGBA & color_rgba)
{
  auto marker_ptr = std::make_shared<Marker>();
  marker_ptr->type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
  marker_ptr->ns = std::string("label");
  marker_ptr->scale.x = 0.5;
  marker_ptr->scale.z = 0.5;
  marker_ptr->text = label;
  marker_ptr->action = visualization_msgs::msg::Marker::MODIFY;
  marker_ptr->pose = marker_ptr->pose = to_pose(centroid, orientation);
  marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.2);
  marker_ptr->color = color_rgba;
  return marker_ptr;
}

visualization_msgs::msg::Marker::SharedPtr get_shape_marker_ptr(
  const autoware_auto_perception_msgs::msg::Shape & shape_msg,
  const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation,
  const std_msgs::msg::ColorRGBA & color_rgba)
{
  auto marker_ptr = std::make_shared<Marker>();
  marker_ptr->ns = std::string("shape");

  using autoware_auto_perception_msgs::msg::Shape;
  if (shape_msg.type == Shape::BOUNDING_BOX) {
    marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST;
    calc_bounding_box_line_list(shape_msg, marker_ptr->points);
  } else if (shape_msg.type == Shape::CYLINDER) {
    marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST;
    calc_cylinder_line_list(shape_msg, marker_ptr->points);
  } else if (shape_msg.type == Shape::POLYGON) {
    marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST;
    calc_polygon_line_list(shape_msg, marker_ptr->points);
  } else {
    marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST;
    calc_polygon_line_list(shape_msg, marker_ptr->points);
  }

  marker_ptr->action = visualization_msgs::msg::Marker::MODIFY;
  marker_ptr->pose = to_pose(centroid, orientation);
  marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.2);
  marker_ptr->scale.x = 0.03;
  marker_ptr->color = color_rgba;

  return marker_ptr;
}

void calc_bounding_box_line_list(
  const autoware_auto_perception_msgs::msg::Shape & shape,
  std::vector<geometry_msgs::msg::Point> & points)
{
  geometry_msgs::msg::Point point;
  point.x = shape.dimensions.x / 2.0;
  point.y = shape.dimensions.y / 2.0;
  point.z = shape.dimensions.z / 2.0;
  points.push_back(point);
  point.x = shape.dimensions.x / 2.0;
  point.y = shape.dimensions.y / 2.0;
  point.z = -shape.dimensions.z / 2.0;
  points.push_back(point);

  point.x = shape.dimensions.x / 2.0;
  point.y = -shape.dimensions.y / 2.0;
  point.z = shape.dimensions.z / 2.0;
  points.push_back(point);
  point.x = shape.dimensions.x / 2.0;
  point.y = -shape.dimensions.y / 2.0;
  point.z = -shape.dimensions.z / 2.0;
  points.push_back(point);

  point.x = -shape.dimensions.x / 2.0;
  point.y = shape.dimensions.y / 2.0;
  point.z = shape.dimensions.z / 2.0;
  points.push_back(point);
  point.x = -shape.dimensions.x / 2.0;
  point.y = shape.dimensions.y / 2.0;
  point.z = -shape.dimensions.z / 2.0;
  points.push_back(point);

  point.x = -shape.dimensions.x / 2.0;
  point.y = -shape.dimensions.y / 2.0;
  point.z = shape.dimensions.z / 2.0;
  points.push_back(point);
  point.x = -shape.dimensions.x / 2.0;
  point.y = -shape.dimensions.y / 2.0;
  point.z = -shape.dimensions.z / 2.0;
  points.push_back(point);

  // up surface
  point.x = shape.dimensions.x / 2.0;
  point.y = shape.dimensions.y / 2.0;
  point.z = shape.dimensions.z / 2.0;
  points.push_back(point);
  point.x = -shape.dimensions.x / 2.0;
  point.y = shape.dimensions.y / 2.0;
  point.z = shape.dimensions.z / 2.0;
  points.push_back(point);

  point.x = shape.dimensions.x / 2.0;
  point.y = shape.dimensions.y / 2.0;
  point.z = shape.dimensions.z / 2.0;
  points.push_back(point);
  point.x = shape.dimensions.x / 2.0;
  point.y = -shape.dimensions.y / 2.0;
  point.z = shape.dimensions.z / 2.0;
  points.push_back(point);

  point.x = -shape.dimensions.x / 2.0;
  point.y = shape.dimensions.y / 2.0;
  point.z = shape.dimensions.z / 2.0;
  points.push_back(point);
  point.x = -shape.dimensions.x / 2.0;
  point.y = -shape.dimensions.y / 2.0;
  point.z = shape.dimensions.z / 2.0;
  points.push_back(point);

  point.x = shape.dimensions.x / 2.0;
  point.y = -shape.dimensions.y / 2.0;
  point.z = shape.dimensions.z / 2.0;
  points.push_back(point);
  point.x = -shape.dimensions.x / 2.0;
  point.y = -shape.dimensions.y / 2.0;
  point.z = shape.dimensions.z / 2.0;
  points.push_back(point);

  // down surface
  point.x = shape.dimensions.x / 2.0;
  point.y = shape.dimensions.y / 2.0;
  point.z = -shape.dimensions.z / 2.0;
  points.push_back(point);
  point.x = -shape.dimensions.x / 2.0;
  point.y = shape.dimensions.y / 2.0;
  point.z = -shape.dimensions.z / 2.0;
  points.push_back(point);

  point.x = shape.dimensions.x / 2.0;
  point.y = shape.dimensions.y / 2.0;
  point.z = -shape.dimensions.z / 2.0;
  points.push_back(point);
  point.x = shape.dimensions.x / 2.0;
  point.y = -shape.dimensions.y / 2.0;
  point.z = -shape.dimensions.z / 2.0;
  points.push_back(point);

  point.x = -shape.dimensions.x / 2.0;
  point.y = shape.dimensions.y / 2.0;
  point.z = -shape.dimensions.z / 2.0;
  points.push_back(point);
  point.x = -shape.dimensions.x / 2.0;
  point.y = -shape.dimensions.y / 2.0;
  point.z = -shape.dimensions.z / 2.0;
  points.push_back(point);

  point.x = shape.dimensions.x / 2.0;
  point.y = -shape.dimensions.y / 2.0;
  point.z = -shape.dimensions.z / 2.0;
  points.push_back(point);
  point.x = -shape.dimensions.x / 2.0;
  point.y = -shape.dimensions.y / 2.0;
  point.z = -shape.dimensions.z / 2.0;
  points.push_back(point);
}

void calc_cylinder_line_list(
  const autoware_auto_perception_msgs::msg::Shape & shape,
  std::vector<geometry_msgs::msg::Point> & points)
{
  const double radius = shape.dimensions.x * 0.5;
  {
    constexpr int n = 20;
    geometry_msgs::msg::Point center;
    center.x = 0.0;
    center.y = 0.0;
    center.z = shape.dimensions.z * 0.5;
    calc_circle_line_list(center, radius, points, n);
    center.z = -shape.dimensions.z * 0.5;
    calc_circle_line_list(center, radius, points, n);
  }
  {
    constexpr int n = 4;
    for (int i = 0; i < n; ++i) {
      geometry_msgs::msg::Point point;
      point.x = std::cos(
        (static_cast<double>(i) / static_cast<double>(n)) * 2.0 * M_PI +
        M_PI / static_cast<double>(n)) *
        radius;
      point.y = std::sin(
        (static_cast<double>(i) / static_cast<double>(n)) * 2.0 * M_PI +
        M_PI / static_cast<double>(n)) *
        radius;
      point.z = shape.dimensions.z * 0.5;
      points.push_back(point);
      point.x = std::cos(
        (static_cast<double>(i) / static_cast<double>(n)) * 2.0 * M_PI +
        M_PI / static_cast<double>(n)) *
        radius;
      point.y = std::sin(
        (static_cast<double>(i) / static_cast<double>(n)) * 2.0 * M_PI +
        M_PI / static_cast<double>(n)) *
        radius;
      point.z = -shape.dimensions.z * 0.5;
      points.push_back(point);
    }
  }
}

void calc_circle_line_list(
  const geometry_msgs::msg::Point center, const double radius,
  std::vector<geometry_msgs::msg::Point> & points, const int n)
{
  for (int i = 0; i < n; ++i) {
    geometry_msgs::msg::Point point;
    point.x = std::cos(
      (static_cast<double>(i) / static_cast<double>(n)) * 2.0 * M_PI +
      M_PI / static_cast<double>(n)) *
      radius +
      center.x;
    point.y = std::sin(
      (static_cast<double>(i) / static_cast<double>(n)) * 2.0 * M_PI +
      M_PI / static_cast<double>(n)) *
      radius +
      center.y;
    point.z = center.z;
    points.push_back(point);
    point.x = std::cos(
      (static_cast<double>(i + 1.0) / static_cast<double>(n)) * 2.0 * M_PI +
      M_PI / static_cast<double>(n)) *
      radius +
      center.x;
    point.y = std::sin(
      (static_cast<double>(i + 1.0) / static_cast<double>(n)) * 2.0 * M_PI +
      M_PI / static_cast<double>(n)) *
      radius +
      center.y;
    point.z = center.z;
    points.push_back(point);
  }
}

void calc_polygon_line_list(
  const autoware_auto_perception_msgs::msg::Shape & shape,
  std::vector<geometry_msgs::msg::Point> & points)
{
  if (shape.footprint.points.size() < 2) {
    RCLCPP_WARN(
      rclcpp::get_logger("ObjectPolygonDisplayBase"),
      "there are no enough footprint to visualize polygon");
    return;
  }
  for (size_t i = 0; i < shape.footprint.points.size(); ++i) {
    geometry_msgs::msg::Point point;
    point.x = shape.footprint.points.at(i).x;
    point.y = shape.footprint.points.at(i).y;
    point.z = shape.dimensions.z / 2.0;
    points.push_back(point);
    point.x = shape.footprint.points
      .at(static_cast<int>(i + 1) % static_cast<int>(shape.footprint.points.size()))
      .x;
    point.y = shape.footprint.points
      .at(static_cast<int>(i + 1) % static_cast<int>(shape.footprint.points.size()))
      .y;
    point.z = shape.dimensions.z / 2.0;
    points.push_back(point);
  }
  for (size_t i = 0; i < shape.footprint.points.size(); ++i) {
    geometry_msgs::msg::Point point;
    point.x = shape.footprint.points.at(i).x;
    point.y = shape.footprint.points.at(i).y;
    point.z = -shape.dimensions.z / 2.0;
    points.push_back(point);
    point.x = shape.footprint.points
      .at(static_cast<int>(i + 1) % static_cast<int>(shape.footprint.points.size()))
      .x;
    point.y = shape.footprint.points
      .at(static_cast<int>(i + 1) % static_cast<int>(shape.footprint.points.size()))
      .y;
    point.z = -shape.dimensions.z / 2.0;
    points.push_back(point);
  }
  for (size_t i = 0; i < shape.footprint.points.size(); ++i) {
    geometry_msgs::msg::Point point;
    point.x = shape.footprint.points.at(i).x;
    point.y = shape.footprint.points.at(i).y;
    point.z = shape.dimensions.z / 2.0;
    points.push_back(point);
    point.x = shape.footprint.points.at(i).x;
    point.y = shape.footprint.points.at(i).y;
    point.z = -shape.dimensions.z / 2.0;
    points.push_back(point);
  }
}

void calc_path_line_list(
  const autoware_auto_perception_msgs::msg::PredictedPath & paths,
  std::vector<geometry_msgs::msg::Point> & points)
{
  for (int i = 0; i < static_cast<int>(paths.path.size()) - 1; ++i) {
    geometry_msgs::msg::Point point;
    point.x = paths.path.at(i).position.x;
    point.y = paths.path.at(i).position.y;
    point.z = paths.path.at(i).position.z;
    points.push_back(point);
    point.x = paths.path.at(i + 1).position.x;
    point.y = paths.path.at(i + 1).position.y;
    point.z = paths.path.at(i + 1).position.z;
    points.push_back(point);
    calc_circle_line_list(point, 0.25, points, 10);
  }
}

}  // namespace detail
}  // namespace object_detection
}  // namespace rviz_plugins
}  // namespace autoware


///////////////////////////////////////////////////////////////////////////
//
// Copyright (c) 1998-2011, Industrial Light & Magic, a division of Lucas
// Digital Ltd. LLC
//
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
// *       Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
// *       Redistributions in binary form must reproduce the above
// copyright notice, this list of conditions and the following disclaimer
// in the documentation and/or other materials provided with the
// distribution.
// *       Neither the name of Industrial Light & Magic nor the names of
// its contributors may be used to endorse or promote products derived
// from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
///////////////////////////////////////////////////////////////////////////

#include "PyIlmBaseConfigInternal.h"

#define BOOST_PYTHON_MAX_ARITY 17

#include "PyImathMatrix.h"
#include "PyImathExport.h"
#include "PyImathDecorators.h"
#include <Python.h>
#include <boost/python.hpp>
#include <boost/python/make_constructor.hpp>
#include <boost/format.hpp>
#include <boost/python/tuple.hpp>
#include <boost/python/dict.hpp>
#include <boost/python/raw_function.hpp>
#include "PyImath.h"
#include "PyImathVec.h"
#include "PyImathMathExc.h"
#include <ImathVec.h>
#include <ImathMatrixAlgo.h>
#include <Iex.h>

namespace PyImath {

template<> const char *PyImath::M22fArray::name() { return "M22fArray"; }
template<> const char *PyImath::M22dArray::name() { return "M22dArray"; }

using namespace boost::python;
using namespace IMATH_NAMESPACE;

template <class T, int len>
struct MatrixRow {
    explicit MatrixRow(T *data) : _data(data) {}
    T & operator [] (int i) { return _data[i]; }
    T *_data;

    static const char *name;
    static void register_class()
    {
        typedef PyImath::StaticFixedArray<MatrixRow,T,len> MatrixRow_helper;
        class_<MatrixRow> matrixRow_class(name,no_init);
        matrixRow_class
            .def("__len__", MatrixRow_helper::len)
            .def("__getitem__", MatrixRow_helper::getitem,return_value_policy<copy_non_const_reference>())
            .def("__setitem__", MatrixRow_helper::setitem)
            ;
    }
};

template <> const char *MatrixRow<float,2>::name = "M22fRow";
template <> const char *MatrixRow<double,2>::name = "M22dRow";

template <class Container, class Data, int len>
struct IndexAccessMatrixRow {
    typedef MatrixRow<Data,len> result_type;
    static MatrixRow<Data,len> apply(Container &c, int i) { return MatrixRow<Data,len>(c[i]); }
};

template <class T> struct Matrix22Name { static const char *value; };
template<> const char *Matrix22Name<float>::value  = "M22f";
template<> const char *Matrix22Name<double>::value = "M22d";

template <class T>
static std::string Matrix22_str(const Matrix22<T> &v)
{
    std::stringstream stream;
    stream << Matrix22Name<T>::value << "(";
    for (int row = 0; row < 2; row++)
    {
        stream << "(";
	for (int col = 0; col < 2; col++)
	{
	    stream << v[row][col];
            stream << (col != 1 ? ", " : "");
	}
        stream << ")" << (row != 1 ? ", " : "");
    }
    stream << ")";
    return stream.str();
}

// Non-specialized repr is same as str
template <class T>
static std::string Matrix22_repr(const Matrix22<T> &v)
{
    return Matrix22_str(v);
}

// Specialization for float to full precision
template <>
std::string Matrix22_repr(const Matrix22<float> &v)
{
    return (boost::format("%s((%.9g, %.9g), (%.9g, %.9g))")
                        % Matrix22Name<float>::value
                        % v[0][0] % v[0][1]
                        % v[1][0] % v[1][1]).str();
}

// Specialization for double to full precision
template <>
std::string Matrix22_repr(const Matrix22<double> &v)
{
    return (boost::format("%s((%.17g, %.17g), (%.17g, %.17g))")
                        % Matrix22Name<double>::value
                        % v[0][0] % v[0][1]
                        % v[1][0] % v[1][1]).str();
}

template <class T>
static const Matrix22<T> &
invert22 (Matrix22<T> &m, bool singExc = true)
{
    MATH_EXC_ON;
    return m.invert(singExc);
}

template <class T>
static Matrix22<T>
inverse22 (Matrix22<T> &m, bool singExc = true)
{
    MATH_EXC_ON;
    return m.inverse(singExc);
}

template <class T, class U>
static const Matrix22<T> &
iadd22(Matrix22<T> &m, const Matrix22<U> &m2)
{
    MATH_EXC_ON;
    Matrix22<T> m3;
    m3.setValue (m2);
    return m += m3;
}

template <class T>
static const Matrix22<T> &
iadd22T(Matrix22<T> &mat, T a)
{
    MATH_EXC_ON;
    return mat += a;
}

template <class T>
static Matrix22<T>
add22(Matrix22<T> &m, const Matrix22<T> &m2)
{
    MATH_EXC_ON;
    return m + m2;
}

template <class T, class U>
static const Matrix22<T> &
isub22(Matrix22<T> &m, const Matrix22<U> &m2)
{
    MATH_EXC_ON;
    Matrix22<T> m3;
    m3.setValue (m2);
    return m -= m3;
}

template <class T>
static const Matrix22<T> &
isub22T(Matrix22<T> &mat, T a)
{
    MATH_EXC_ON;
    return mat -= a;
}

template <class T>
static Matrix22<T>
sub22(Matrix22<T> &m, const Matrix22<T> &m2)
{
    MATH_EXC_ON;
    return m - m2;
}

template <class T>
static const Matrix22<T> &
negate22 (Matrix22<T> &m)
{
    MATH_EXC_ON;
    return m.negate();
}

template <class T>
static Matrix22<T>
neg22 (Matrix22<T> &m)
{
    MATH_EXC_ON;
    return -m;
}

template <class T>
static const Matrix22<T> &
imul22T(Matrix22<T> &m, const T &t)
{
    MATH_EXC_ON;
    return m *= t;
}

template <class T>
static Matrix22<T>
mul22T(Matrix22<T> &m, const T &t)
{
    MATH_EXC_ON;
    return m * t;
}

template <class T>
static Matrix22<T>
rmul22T(Matrix22<T> &m, const T &t)
{
    MATH_EXC_ON;
    return t * m;
}

template <class T>
static const Matrix22<T> &
idiv22T(Matrix22<T> &m, const T &t)
{
    MATH_EXC_ON;
    return m /= t;
}

template <class T>
static Matrix22<T>
div22T(Matrix22<T> &m, const T &t)
{
    MATH_EXC_ON;
    return m / t;
}

template <class T>
void
outerProduct22(Matrix22<T> &mat, const Vec2<T> &a, const Vec2<T> &b)
{
    MATH_EXC_ON;
    mat = IMATH_NAMESPACE::outerProduct(a,b);
}

template <class TV,class TM>
static void
multDirMatrix22(Matrix22<TM> &mat, const Vec2<TV> &src, Vec2<TV> &dst)
{
    MATH_EXC_ON;
    mat.multDirMatrix(src, dst);
}

template <class TV,class TM>
static Vec2<TV>
multDirMatrix22_return_value(Matrix22<TM> &mat, const Vec2<TV> &src)
{
    MATH_EXC_ON;
    Vec2<TV> dst;
    mat.multDirMatrix(src, dst);
    return dst;
}

template <class TV,class TM>
static FixedArray<Vec2<TV> >
multDirMatrix22_array(Matrix22<TM> &mat, const FixedArray<Vec2<TV> >&src)
{
    MATH_EXC_ON;
    size_t len = src.len();
    FixedArray<Vec2<TV> > dst(len);
    for (size_t i=0; i<len; ++i) mat.multDirMatrix(src[i], dst[i]);
    return dst;
}

template <class T>
static const Matrix22<T> &
rotate22(Matrix22<T> &mat, const T &r)
{
    MATH_EXC_ON;
    return mat.rotate(r);
}

template <class T>
static void
extractEuler(Matrix22<T> &mat, Vec2<T> &dstObj)
{
    MATH_EXC_ON;
    T dst;
    IMATH_NAMESPACE::extractEuler(mat, dst);
    dstObj.setValue(dst, T (0));
}

template <class T>
static const Matrix22<T> &
scaleSc22(Matrix22<T> &mat, const T &s)
{
    MATH_EXC_ON;
    Vec2<T> sVec(s, s);
    return mat.scale(sVec);
}

template <class T>
static const Matrix22<T> &
scaleV22(Matrix22<T> &mat, const Vec2<T> &s)
{
    MATH_EXC_ON;
    return mat.scale(s);
}

template <class T>
static const Matrix22<T> &
scale22Tuple(Matrix22<T> &mat, const tuple &t)
{
    MATH_EXC_ON;
    if(t.attr("__len__")() == 2)
    {
        Vec2<T> s;
        s.x = extract<T>(t[0]);
        s.y = extract<T>(t[1]);

        return mat.scale(s);
    }
    else
        THROW(IEX_NAMESPACE::LogicExc, "m.scale needs tuple of length 2");
}

template <class T>
static const Matrix22<T> &
setRotation22(Matrix22<T> &mat, const T &r)
{
    MATH_EXC_ON;
    return mat.setRotation(r);
}

template <class T>
static const Matrix22<T> &
setScaleSc22(Matrix22<T> &mat, const T &s)
{
    MATH_EXC_ON;
    Vec2<T> sVec(s, s);
    return mat.setScale(sVec);
}

template <class T>
static const Matrix22<T> &
setScaleV22(Matrix22<T> &mat, const Vec2<T> &s)
{
    MATH_EXC_ON;
    return mat.setScale(s);
}

template <class T>
static const Matrix22<T> &
setScale22Tuple(Matrix22<T> &mat, const tuple &t)
{
    MATH_EXC_ON;
    if(t.attr("__len__")() == 2)
    {
        Vec2<T> s;
        s.x = extract<T>(t[0]);
        s.y = extract<T>(t[1]);

        return mat.setScale(s);
    }
    else
        THROW(IEX_NAMESPACE::LogicExc, "m.setScale needs tuple of length 2");
}

template <class T>
static void
setValue22(Matrix22<T> &mat, const Matrix22<T> &value)
{
    MATH_EXC_ON;
    mat.setValue(value);
}

template <class T>
static Matrix22<T>
subtractTL22(Matrix22<T> &mat, T a)
{
    MATH_EXC_ON;
    Matrix22<T> m(mat.x);
    for(int i = 0; i < 2; ++i)
        for(int j = 0; j < 2; ++j)
            m.x[i][j] -= a;

    return m;
}

template <class T>
static Matrix22<T>
subtractTR22(Matrix22<T> &mat, T a)
{
    MATH_EXC_ON;
    Matrix22<T> m(mat.x);
    for(int i = 0; i < 2; ++i)
        for(int j = 0; j < 2; ++j)
            m.x[i][j] = a - m.x[i][j];

    return m;
}

template <class T>
static Matrix22<T>
add22T(Matrix22<T> &mat, T a)
{
    MATH_EXC_ON;
    Matrix22<T> m(mat.x);
    for(int i = 0; i < 2; ++i)
        for(int j = 0; j < 2; ++j)
            m.x[i][j] += a;

    return m;
}

template <class S, class T>
static Matrix22<T>
mul22(Matrix22<T> &mat1, Matrix22<S> &mat2)
{
    MATH_EXC_ON;
    Matrix22<T> mat2T;
    mat2T.setValue (mat2);
    return mat1 * mat2T;
}

template <class S, class T>
static Matrix22<T>
rmul22(Matrix22<T> &mat2, Matrix22<S> &mat1)
{
    MATH_EXC_ON;
    Matrix22<T> mat1T;
    mat1T.setValue (mat1);
    return mat1T * mat2;
}

template <class S, class T>
static const Matrix22<T> &
imul22(Matrix22<T> &mat1, Matrix22<S> &mat2)
{
    MATH_EXC_ON;
    Matrix22<T> mat2T;
    mat2T.setValue (mat2);
    return mat1 *= mat2T;
}

template <class T>
static bool
lessThan22(Matrix22<T> &mat1, const Matrix22<T> &mat2)
{
    for(int i = 0; i < 2; ++i){
        for(int j = 0; j < 2; ++j){
            if(mat1[i][j] > mat2[i][j]){
                return false;
            }
        }
    }

    return (mat1 != mat2);
}

template <class T>
static bool
lessThanEqual22(Matrix22<T> &mat1, const Matrix22<T> &mat2)
{
    for(int i = 0; i < 2; ++i){
        for(int j = 0; j < 2; ++j){
            if(mat1[i][j] > mat2[i][j]){
                return false;
            }
        }
    }

    return true;
}

template <class T>
static bool
greaterThan22(Matrix22<T> &mat1, const Matrix22<T> &mat2)
{
    for(int i = 0; i < 2; ++i){
        for(int j = 0; j < 2; ++j){
            if(mat1[i][j] < mat2[i][j]){
                std::cout << mat1[i][j] << " " << mat2[i][j] << std::endl;
                return false;
            }
        }
    }

    return (mat1 != mat2);
}

template <class T>
static bool
greaterThanEqual22(Matrix22<T> &mat1, const Matrix22<T> &mat2)
{
    for(int i = 0; i < 2; ++i){
        for(int j = 0; j < 2; ++j){
            if(mat1[i][j] < mat2[i][j]){
                return false;
            }
        }
    }

    return true;
}

BOOST_PYTHON_FUNCTION_OVERLOADS(invert22_overloads, invert22, 1, 2);
BOOST_PYTHON_FUNCTION_OVERLOADS(inverse22_overloads, inverse22, 1, 2);
BOOST_PYTHON_FUNCTION_OVERLOADS(outerProduct22_overloads, outerProduct22, 3, 3);

template <class T>
static Matrix22<T> * Matrix2_tuple_constructor(const tuple &t0, const tuple &t1)
{
  if(t0.attr("__len__")() == 2 && t1.attr("__len__")() == 2)
  {
      return new Matrix22<T>(extract<T>(t0[0]),  extract<T>(t0[1]),
                             extract<T>(t1[0]),  extract<T>(t1[1]));
  }
  else
      THROW(IEX_NAMESPACE::LogicExc, "Matrix22 takes 2 tuples of length 2");
}

template <class T, class S>
static Matrix22<T> *Matrix2_matrix_constructor(const Matrix22<S> &mat)
{
    Matrix22<T> *m = new Matrix22<T>;

    for(int i = 0; i < 2; ++i)
        for(int j = 0; j < 2; ++j)
            m->x[i][j] = T (mat.x[i][j]);

    return m;
}

template <class T>
class_<Matrix22<T> >
register_Matrix22()
{
    typedef PyImath::StaticFixedArray<Matrix22<T>,T,2,IndexAccessMatrixRow<Matrix22<T>,T,2> > Matrix22_helper;

    MatrixRow<T,2>::register_class();
    class_<Matrix22<T> > matrix22_class(Matrix22Name<T>::value, Matrix22Name<T>::value,init<Matrix22<T> >("copy construction"));
    matrix22_class
        .def(init<>("initialize to identity"))
        .def(init<T>("initialize all entries to a single value"))
        .def(init<T,T,T,T>("make from components"))
        .def("__init__", make_constructor(Matrix2_tuple_constructor<T>))
        .def("__init__", make_constructor(Matrix2_matrix_constructor<T,float>))
        .def("__init__", make_constructor(Matrix2_matrix_constructor<T,double>))

	//.def_readwrite("x00", &Matrix22<T>::x[0][0])
	//.def_readwrite("x01", &Matrix22<T>::x[0][1])
	//.def_readwrite("x02", &Matrix22<T>::x[0][2])
	//.def_readwrite("x10", &Matrix22<T>::x[1][0])
	//.def_readwrite("x11", &Matrix22<T>::x[1][1])
	//.def_readwrite("x12", &Matrix22<T>::x[1][2])
	//.def_readwrite("x20", &Matrix22<T>::x[2][0])
	//.def_readwrite("x21", &Matrix22<T>::x[2][1])
	//.def_readwrite("x22", &Matrix22<T>::x[2][2])
        .def("baseTypeEpsilon", &Matrix22<T>::baseTypeEpsilon,"baseTypeEpsilon() epsilon value of the base type of the vector")
        .staticmethod("baseTypeEpsilon")
        .def("baseTypeMax", &Matrix22<T>::baseTypeMax,"baseTypeMax() max value of the base type of the vector")
        .staticmethod("baseTypeMax")
        .def("baseTypeMin", &Matrix22<T>::baseTypeMin,"baseTypeMin() min value of the base type of the vector")
        .staticmethod("baseTypeMin")
        .def("baseTypeSmallest", &Matrix22<T>::baseTypeSmallest,"baseTypeSmallest() smallest value of the base type of the vector")
        .staticmethod("baseTypeSmallest")
        .def("equalWithAbsError", &Matrix22<T>::equalWithAbsError,"m1.equalWithAbsError(m2,e) true if the elements "
             "of v1 and v2 are the same with an absolute error of no more than e, "
             "i.e., abs(m1[i] - m2[i]) <= e")
        .def("equalWithRelError", &Matrix22<T>::equalWithRelError,"m1.equalWithAbsError(m2,e) true if the elements "
             "of m1 and m2 are the same with an absolute error of no more than e, "
             "i.e., abs(m1[i] - m2[i]) <= e * abs(m1[i])")
        // need a different version for matrix data access
        .def("__len__", Matrix22_helper::len)
        .def("__getitem__", Matrix22_helper::getitem)
	//.def("__setitem__", Matrix22_helper::setitem)
        .def("makeIdentity",&Matrix22<T>::makeIdentity,"makeIdentity() make this matrix the identity matrix")
        .def("transpose",&Matrix22<T>::transpose,return_internal_reference<>(),"transpose() transpose this matrix")
        .def("transposed",&Matrix22<T>::transposed,"transposed() return a transposed copy of this matrix")
        .def("invert",&invert22<T>,invert22_overloads("invert() invert this matrix")[return_internal_reference<>()])
        .def("inverse",&inverse22<T>,inverse22_overloads("inverse() return an inverted copy of this matrix"))
        .def("determinant",&Matrix22<T>::determinant,"determinant() return the determinant of this matrix")
        .def(self == self) // NOSONAR - suppress SonarCloud bug report.
        .def(self != self) // NOSONAR - suppress SonarCloud bug report.
        .def("__iadd__", &iadd22<T, float>,return_internal_reference<>())
        .def("__iadd__", &iadd22<T, double>,return_internal_reference<>())
        .def("__iadd__", &iadd22T<T>,return_internal_reference<>())
        .def("__add__", &add22<T>)
        .def("__isub__", &isub22<T, float>,return_internal_reference<>())
        .def("__isub__", &isub22<T, double>,return_internal_reference<>())
        .def("__isub__", &isub22T<T>,return_internal_reference<>())
        .def("__sub__", &sub22<T>)
        .def("negate",&negate22<T>,return_internal_reference<>(),"negate() negate all entries in this matrix")
        .def("__neg__", &neg22<T>)
        .def("__imul__", &imul22T<T>,return_internal_reference<>())
        .def("__mul__", &mul22T<T>)
        .def("__rmul__", &rmul22T<T>)
        .def("__idiv__", &idiv22T<T>,return_internal_reference<>())
        .def("__itruediv__", &idiv22T<T>,return_internal_reference<>())
        .def("__div__", &div22T<T>)
        .def("__truediv__", &div22T<T>)
        .def("__add__", &add22T<T>)
        .def("__radd__", &add22T<T>)
        .def("__sub__", &subtractTL22<T>)
        .def("__rsub__", &subtractTR22<T>)
        .def("__mul__", &mul22<float, T>)
        .def("__mul__", &mul22<double, T>)
        .def("__rmul__", &rmul22<float, T>)
        .def("__rmul__", &rmul22<double, T>)
        .def("__imul__", &imul22<float, T>,return_internal_reference<>())
        .def("__imul__", &imul22<double, T>,return_internal_reference<>())
        .def("__lt__", &lessThan22<T>)
        .def("__le__", &lessThanEqual22<T>)
        .def("__gt__", &greaterThan22<T>)
        .def("__ge__", &greaterThanEqual22<T>)
	//.def(self_ns::str(self))
        .def("__str__",&Matrix22_str<T>)
        .def("__repr__",&Matrix22_repr<T>)

         .def("extractEuler", &extractEuler<T>,
              "M.extractEuler(r) -- extracts the "
			  "rotation component of M into r. "
              "Assumes that M contains no shear or "
              "non-uniform scaling; results are "
              "meaningless if it does.")

         .def("multDirMatrix", &multDirMatrix22<double,T>, "mult matrix")
         .def("multDirMatrix", &multDirMatrix22_return_value<double,T>, "mult matrix")
         .def("multDirMatrix", &multDirMatrix22_array<double,T>, "mult matrix")
         .def("multDirMatrix", &multDirMatrix22<float,T>, "mult matrix")
         .def("multDirMatrix", &multDirMatrix22_return_value<float,T>, "mult matrix")
         .def("multDirMatrix", &multDirMatrix22_array<float,T>, "mult matrix")

         .def("rotate", &rotate22<T>, return_internal_reference<>(),"rotate matrix")

         .def("scale", &scaleSc22<T>, return_internal_reference<>(),"scale matrix")
         .def("scale", &scaleV22<T>, return_internal_reference<>(),"scale matrix")
         .def("scale", &scale22Tuple<T>, return_internal_reference<>(),"scale matrix")

         .def("setRotation", &setRotation22<T>, return_internal_reference<>(),"setRotation()")
         .def("setScale", &setScaleSc22<T>, return_internal_reference<>(),"setScale()")
         .def("setScale", &setScaleV22<T>, return_internal_reference<>(),"setScale()")
         .def("setScale", &setScale22Tuple<T>, return_internal_reference<>(),"setScale()")

         .def("setValue", &setValue22<T>, "setValue()")
         ;

    decoratecopy(matrix22_class);

    return matrix22_class;
/*
    const Matrix22 &	operator = (const Matrix22 &v);
    const Matrix22 &	operator = (T a);
    T *			getValue ();
    const T *		getValue () const;
    template <class S> void getValue (Matrix22<S> &v) const;
    template <class S> Matrix22 & setValue (const Matrix22<S> &v);
    template <class S> Matrix22 & setTheMatrix (const Matrix22<S> &v);
    template <class S> void multVecMatrix(const Vec2<S> &src, Vec2<S> &dst) const;
    template <class S> void multDirMatrix(const Vec2<S> &src, Vec2<S> &dst) const;
    template <class S> const Matrix22 &	setRotation (S r);
    template <class S> const Matrix22 &	rotate (S r);
    const Matrix22 &	setScale (T s);
    template <class S> const Matrix22 &	setScale (const Vec2<S> &s);
    template <class S> const Matrix22 &	scale (const Vec2<S> &s);
    template <class S> const Matrix22 &	setTranslation (const Vec2<S> &t);
    Vec2<T>		translation () const;
    template <class S> const Matrix22 &	translate (const Vec2<S> &t);
    template <class S> const Matrix22 &	setShear (const S &h);
    template <class S> const Matrix22 &	setShear (const Vec2<S> &h);
    template <class S> const Matrix22 &	shear (const S &xy);
    template <class S> const Matrix22 &	shear (const Vec2<S> &h);
*/
}

template <class T>
static void
setM22ArrayItem(FixedArray<IMATH_NAMESPACE::Matrix22<T> > &ma,
                Py_ssize_t index,
                const IMATH_NAMESPACE::Matrix22<T> &m)
{
    ma[ma.canonical_index(index)] = m;
}

template <class T>
class_<FixedArray<IMATH_NAMESPACE::Matrix22<T> > >
register_M22Array()
{
    class_<FixedArray<IMATH_NAMESPACE::Matrix22<T> > > matrixArray_class = FixedArray<IMATH_NAMESPACE::Matrix22<T> >::register_("Fixed length array of IMATH_NAMESPACE::Matrix22");
    matrixArray_class
         .def("__setitem__", &setM22ArrayItem<T>)
        ;
    return matrixArray_class;
}

template PYIMATH_EXPORT class_<IMATH_NAMESPACE::Matrix22<float> > register_Matrix22<float>();
template PYIMATH_EXPORT class_<IMATH_NAMESPACE::Matrix22<double> > register_Matrix22<double>();

template PYIMATH_EXPORT class_<FixedArray<IMATH_NAMESPACE::Matrix22<float> > > register_M22Array<float>();
template PYIMATH_EXPORT class_<FixedArray<IMATH_NAMESPACE::Matrix22<double> > > register_M22Array<double>();

template<> PYIMATH_EXPORT IMATH_NAMESPACE::Matrix22<float> FixedArrayDefaultValue<IMATH_NAMESPACE::Matrix22<float> >::value() { return IMATH_NAMESPACE::Matrix22<float>(); }
template<> PYIMATH_EXPORT IMATH_NAMESPACE::Matrix22<double> FixedArrayDefaultValue<IMATH_NAMESPACE::Matrix22<double> >::value() { return IMATH_NAMESPACE::Matrix22<double>(); }
}


#include <skelly_sim.hpp>

#include <rng.hpp>

#include <Eigen/Core>
#include <fstream>
#include <unordered_map>

#include <body.hpp>
#include <fiber.hpp>
#include <params.hpp>
#include <parse_util.hpp>
#include <periphery.hpp>
#include <solver_hydro.hpp>
#include <system.hpp>

#include <mpi.h>
#include <sys/mman.h>

#include <spdlog/cfg/env.h>
#include <spdlog/sinks/null_sink.h>
#include <spdlog/sinks/stdout_color_sinks.h>
#include <spdlog/spdlog.h>

namespace System {
Params params_;                    ///< Simulation input parameters
FiberContainer fc_;                ///< Fibers
BodyContainer bc_;                 ///< Bodies
std::unique_ptr<Periphery> shell_; ///< Periphery
std::ofstream ofs_;                ///< Trajectory output file stream. Opened at initialization

FiberContainer fc_bak_;   ///< Copy of fibers for timestep reversion
BodyContainer bc_bak_;    ///< Copy of bodies for timestep reversion
int rank_;                ///< MPI rank
int size_;                ///< MPI size
toml::value param_table_; ///< Parsed input table

/// @brief Time varying system properties that are extrinsic to the physical objects
struct {
    double dt;         ///< Current timestep size
    double time = 0.0; ///< Current system time
} properties;

/// @brief Structure for trajectory output via msgpack
///
/// This can be extended easily, so long as you update the corresponding input_map_t and potentially the System::write()
/// function if you can't use a reference in the output_map for some reason.
typedef struct output_map_t {
    double &time = properties.time;                          ///< System::properties
    double &dt = properties.dt;                              ///< System::properties
    FiberContainer &fibers = fc_;                            ///< System::fc_
    BodyContainer &bodies = bc_;                             ///< System::bc_
    std::pair<std::string, std::string> rng_state;           ///< string representation of split/unsplit state in RNG
    MSGPACK_DEFINE_MAP(time, dt, rng_state, fibers, bodies); ///< Helper routine to specify serialization
} output_map_t;
output_map_t output_map; ///< Output data for msgpack dump

/// @brief Structure for importing frame of trajectory into the simulation
///
/// We can't use output_map_t here, but rather a similar struct which uses copies of the member
/// variables (rather than references) which are then used to update the System variables.
typedef struct input_map_t {
    double time;                                             ///< System::properties
    double dt;                                               ///< System::properties
    FiberContainer fibers;                                   ///< System::fc_
    BodyContainer bodies;                                    ///< System::bc_
    std::pair<std::string, std::string> rng_state;           ///< string representation of split/unsplit state in RNG
    MSGPACK_DEFINE_MAP(time, dt, rng_state, fibers, bodies); ///< Helper routine to specify serialization
} input_map_t;

/// Flush current simulation state to trajectory file.
void write() {
    output_map.rng_state = RNG::dump_state();
    msgpack::pack(ofs_, output_map);
    ofs_.flush();
}

/// @brief Set system state to last state found in trajectory files
///
/// @param[in] if_file input file name of trajectory file for this rank
void resume_from_trajectory(std::string if_file) {
    int fd = open(if_file.c_str(), O_RDONLY);
    if (fd == -1)
        throw std::runtime_error("Unable to open trajectory file " + if_file + " for resume.");

    struct stat sb;
    if (fstat(fd, &sb) == -1)
        throw std::runtime_error("Error statting " + if_file + " for resume.");

    std::size_t buflen = sb.st_size;

    const char *addr = static_cast<const char *>(mmap(NULL, buflen, PROT_READ, MAP_PRIVATE, fd, 0u));
    if (addr == MAP_FAILED)
        throw std::runtime_error("Error mapping " + if_file + " for resume.");

    std::size_t offset = 0;
    msgpack::object_handle oh;
    // FIXME: There is probably a way to not have to read the entire trajectory
    while (offset != buflen)
        msgpack::unpack(oh, addr, buflen, offset);

    // FIXME: add assertion that system time is the same across all ranks to resume functionality

    // FIXME: This is a bug-prone way to unpack. Should make proper clone functions that merge
    // the minimum representation with existing data automatically (node data, etc)
    msgpack::object obj = oh.get();
    input_map_t const &min_state = obj.as<input_map_t>();
    output_map.time = min_state.time;
    output_map.dt = min_state.dt;
    fc_.fibers.clear();
    for (const auto &min_fib : min_state.fibers.fibers) {
        Fiber new_fib = min_fib;
        new_fib.init(params_.eta);
        new_fib.x_ = min_fib.x_;
        fc_.fibers.push_back(new_fib);
    }
    for (int i = 0; i < bc_.bodies.size(); ++i) {
        bc_.bodies[i]->position_ = min_state.bodies.bodies[i]->position_;
        bc_.bodies[i]->orientation_ = min_state.bodies.bodies[i]->orientation_;
    }
    output_map.rng_state = min_state.rng_state;
    RNG::init(output_map.rng_state);
}

// TODO: Refactor all preprocess stuff. It's awful

/// @brief Convert fiber initial positions/orientations to full coordinate representation
/// @param[out] fiber_table Explicitly instantiated element of fiber config
/// @param[in] origin origin of coordinate system for fiber
void resolve_fiber_position(toml::value &fiber_table, Eigen::Vector3d &origin) {
    int64_t n_nodes = toml::find_or<int64_t>(fiber_table, "n_nodes", -1);
    double length = toml::find<double>(fiber_table, "length");

    Eigen::VectorXd x_array = parse_util::convert_array<>(toml::find_or<toml::array>(fiber_table, "x", {}));
    Eigen::VectorXd x_0 = parse_util::convert_array<>(toml::find_or<toml::array>(fiber_table, "relative_position", {}));
    Eigen::VectorXd u = parse_util::convert_array<>(toml::find_or<toml::array>(fiber_table, "orientation", {}));

    if (n_nodes == -1) {
        n_nodes = x_array.size() / 3;
        if (n_nodes == 0)
            throw std::runtime_error("Attempt to initialize fiber without point count or positions.");
    }

    if ((x_array.size() && x_0.size()) || (x_array.size() && u.size()))
        throw std::runtime_error("Fiber supplied 'relative_position' or 'orientation' with node positions 'x', "
                                 "ambiguous initialization.");

    // FIXME: Make test for this case
    if (x_0.size() == 3 && u.size() == 3) {
        Eigen::MatrixXd x(3, n_nodes);
        Eigen::ArrayXd s = Eigen::ArrayXd::LinSpaced(n_nodes, 0, length).transpose();
        for (int i = 0; i < 3; ++i)
            x.row(i) = origin(i) + x_0(i) + u(i) * s;

        fiber_table.as_table()["x"] = toml::array();
        toml::array &x_tab = fiber_table.at("x").as_array();
        for (int i = 0; i < x.size(); ++i) {
            x_tab.push_back(x.data()[i]);
        }
    }
}

/// @brief Generate uniformly distributed point on unit sphere
Eigen::Vector3d uniform_on_sphere() {
    const double u = 2 * (RNG::uniform_unsplit()) - 1;
    const double theta = 2 * M_PI * RNG::uniform_unsplit();
    const double factor = sqrt(1 - u * u);
    return Eigen::Vector3d{factor * cos(theta), factor * sin(theta), u};
}

/// @brief Update config to appropriately fill nucleation sites with fibers
/// @param[out] fiber_array Updated fiber array with pointers to nucleation sites
/// @param[out] body_array Updated body array with nucleation sites fixed to current fiber position
void resolve_nucleation_sites(toml::array &fiber_array, toml::array &body_array) {
    using std::make_pair;
    const int n_bodies = body_array.size();
    std::map<std::pair<int, int>, int> occupied;
    std::vector<int> n_sites(n_bodies);

    // Pass through fibers for assigned bodies
    for (size_t i_fib = 0; i_fib < fiber_array.size(); ++i_fib) {
        toml::value &fiber_table = fiber_array.at(i_fib);
        int i_body = toml::find_or<int>(fiber_table, "parent_body", -1);
        int i_site = toml::find_or<int>(fiber_table, "parent_site", -1);

        if (i_body >= n_bodies) {
            std::cerr << "Invalid body reference " << i_body << " on fiber " << i_fib << ": \n"
                      << fiber_table << std::endl;
            throw std::runtime_error("Invalid body reference.\n");
        }

        // unattached or site find automated. move to next fiber
        if (i_body < 0 || i_site < 0)
            continue;

        // check for duplicate assignment
        auto site_pair = make_pair(i_body, i_site);
        if (occupied.count(site_pair) > 0) {
            const int j_fib = occupied[site_pair];
            // clang-format off
                std::cerr << "Multiple fibers bound to site: ("
                          << i_body << ", " << i_site << ")"
                          << " from fibers (" << i_fib << ", " << j_fib << ") " << ".\n"
                          << fiber_table << "\n\n" << fiber_array.at(j_fib) << "\n";
                throw std::runtime_error("Multiple fibers bound to site.\n");
            // clang-format on
        }

        occupied[site_pair] = i_fib;
    }

    // Pass through fibers for unassigned bodies
    std::vector<int> test_site(n_bodies); //< current site to try for insertion
    for (size_t i_fib = 0; i_fib < fiber_array.size(); ++i_fib) {
        toml::value &fiber_table = fiber_array.at(i_fib);
        int i_body = toml::find_or<int>(fiber_table, "parent_body", -1);
        int i_site = toml::find_or<int>(fiber_table, "parent_site", -1);

        // unattached or site already assigned. move to next fiber
        if (i_body < 0 || i_site >= 0) {
            Eigen::Vector3d origin{0.0, 0.0, 0.0};
            if (i_body < 0) // Initialize free fiber
                resolve_fiber_position(fiber_table, origin);
            continue;
        }

        // Find an empty site
        while (occupied.count(make_pair(i_body, test_site[i_body])) != 0)
            test_site[i_body]++;

        // Store in map so body knows where to grab
        occupied[make_pair(i_body, test_site[i_body])] = i_fib;
    }

    std::vector<std::map<int, Eigen::Vector3d>> nucleation_sites(n_bodies);
    for (const auto &site_map : occupied) {
        auto [i_body, i_site] = site_map.first;
        auto i_fib = site_map.second;
        toml::value &fiber_table = fiber_array.at(i_fib);
        toml::array &body_position = body_array.at(i_body).at("position").as_array();

        fiber_table["parent_body"] = i_body;
        fiber_table["parent_site"] = i_site;
        Eigen::Vector3d origin = parse_util::convert_array<>(body_position);

        resolve_fiber_position(fiber_table, origin);

        if (fiber_table.contains("x")) {
            Eigen::VectorXd x_array = parse_util::convert_array<>(fiber_table.at("x").as_array());
            nucleation_sites[i_body][i_site] = x_array.segment(0, 3) - origin;
        }
    }

    for (int i_body = 0; i_body < n_bodies; ++i_body) {
        toml::value &body_table = body_array.at(i_body);
        if (!body_table.contains("nucleation_sites"))
            body_table["nucleation_sites"] = toml::array();

        int n_nucleation_sites = toml::find_or<int>(body_table, "n_nucleation_sites", nucleation_sites[i_body].size());
        double radius = body_table["radius"].as_floating();
        // FIXME: add min_separation parameter
        const double min_separation2 = pow(params_.dynamic_instability.min_separation, 2);

        for (int i = 0; i < n_nucleation_sites; ++i) {
            if (nucleation_sites[i_body].count(i))
                continue;

            bool collision = false;
            do {
                collision = false;
                nucleation_sites[i_body][i] = radius * uniform_on_sphere();
                Eigen::Vector3d &r_i = nucleation_sites[i_body][i];

                for (auto &j_pair : nucleation_sites[i_body]) {
                    const int &j = j_pair.first;
                    const Eigen::Vector3d &r_j = j_pair.second;
                    if (i == j)
                        continue;
                    Eigen::Vector3d dr = r_j - r_i;
                    double dr2 = dr.dot(dr);
                    if (dr2 < min_separation2) {
                        collision = true;
                        break;
                    }
                }
            } while (collision);
        }

        toml::array &x = body_table["nucleation_sites"].as_array();
        for (auto &[site, xvec] : nucleation_sites[i_body])
            for (int i = 0; i < 3; ++i)
                x.push_back(xvec[i]);
    }

    for (const auto &site_map : occupied) {
        auto [i_body, i_site] = site_map.first;
        auto i_fib = site_map.second;
        toml::value &fiber_table = fiber_array.at(i_fib);
        toml::array &body_position = body_array.at(i_body).at("position").as_array();
        toml::array &nucleation_sites = body_array.at(i_body).at("nucleation_sites").as_array();

        fiber_table["parent_body"] = i_body;
        fiber_table["parent_site"] = i_site;
        Eigen::Vector3d origin = parse_util::convert_array<>(body_position);
        Eigen::VectorXd sites = parse_util::convert_array<>(nucleation_sites);

        if (!fiber_table.contains("x")) {
            Eigen::Vector3d site_pos = Eigen::Map<Eigen::Vector3d>(sites.data() + i_site * 3, 3);
            Eigen::Vector3d fiber_origin = site_pos;
            Eigen::Vector3d fiber_orientation = fiber_origin.normalized();
            fiber_table["relative_position"] = {fiber_origin[0], fiber_origin[1], fiber_origin[2]};
            fiber_table["orientation"] = {fiber_orientation[0], fiber_orientation[1], fiber_orientation[2]};

            resolve_fiber_position(fiber_table, origin);
        }
    }
}

/// @file
/// @brief Initialize classes with proper interdependencies

/// @brief Patch up input toml config to go from implicit to explicit instantiations
///
/// Some classes have interdependencies, but in order to avoid the objects having explicit
/// dependencies on each other through initialization while maintaining a simple config format,
/// we preprocess the toml config to have TOML communicate these dependencies by explicitly
/// instantiating config values.
///
/// The main example of the utility of this approach is Fiber-Body connections through
/// "nucleation sites". The body nucleation site is _constrained_ to be at the same position as
/// its associated Fiber minus end. Ultimately the Body doesn't care what Fiber it's attached
/// to, just that it has nucleation sites that rotate/move with it. The Fibers, however, have
/// to know which Body they're attached to to constrain their motion. Rather than forcing the
/// user to supply the Fiber minus end and which (body,site) combo it sits at on the Fiber
/// object to the body object, and the nucleation site coordinates in an ordered array on the
/// body object, it's much easier for the user to simply supply a fiber and a body it's attached
/// to, and then automatically sort out the interconnections programmatically.
///
/// This presents an ordering problem for initialization though, since the FiberContainer needs
/// to communicate the nucleation positions to the BodyContainer, but the BodyContainer needs
/// to communicate what (body,site) combo the Fiber is attached to back to the
/// FiberContainer. If you initialize the bodies first, they won't know how to initialize
/// nucleation sites. If you initialize fibers first, they have to communicate the bodies
/// they're attached to the body objects and then get a response on what site they are
/// assigned. This initialization cycle can be removed by just preprocessing the config so that
/// all data on all MPI ranks is consistent at system initialization time, and all objects know
/// the relevant data to fully initialize at object creation time.
///
/// @param[out] config global toml config after initial parsing
void preprocess(toml::value &config) {
    spdlog::info("Preprocessing config file");

    // Body-fiber interactions through nucleation sites
    if (config.contains("fibers") && config.contains("bodies"))
        resolve_nucleation_sites(config["fibers"].as_array(), config["bodies"].as_array());
    else if (config.contains("fibers")) {
        toml::array &fiber_array = config["fibers"].as_array();
        for (size_t i_fib = 0; i_fib < fiber_array.size(); ++i_fib) {
            toml::value &fiber_table = fiber_array.at(i_fib);
            Eigen::Vector3d origin{0.0, 0.0, 0.0};
            resolve_fiber_position(fiber_table, origin);
        }
    }
}

/// @brief Calculate forces/torques on the bodies and velocities on the fibers due to attachment constraints
/// @param[in] fibers_xt [4 x num_fiber_nodes_local] Vector of fiber node positions and tensions on current rank.
/// Ordering is [fib1.nodes.x, fib1.nodes.y, fib1.nodes.z, fib1.T, fib2.nodes.x, ...]
/// @param[in] body_velocities [6 x n_bodies] Matrix of body center of mass velocities and angular velocities
std::pair<Eigen::MatrixXd, Eigen::MatrixXd> calculate_body_fiber_link_conditions(VectorRef &fibers_xt,
                                                                                MatrixRef &body_velocities) {
    using Eigen::ArrayXXd;
    using Eigen::MatrixXd;
    using Eigen::Vector3d;

    auto &fc = fc_;
    auto &bc = bc_;

    Eigen::MatrixXd force_torque_on_bodies = MatrixXd::Zero(6, bc.get_global_count());
    Eigen::MatrixXd velocities_on_fiber = MatrixXd::Zero(7, fc.get_local_count());

    int xt_offset = 0;
    int i_fib = 0;
    for (const auto &fib : fc.fibers) {
        const auto &fib_mats = fib.matrices_.at(fib.n_nodes_);
        const int n_pts = fib.n_nodes_;

        auto &[i_body, i_site] = fib.binding_site_;
        if (i_body < 0)
            continue;

        Vector3d site_pos = bc.get_nucleation_site(i_body, i_site) - bc.at(i_body).get_position();
        MatrixXd x_new(3, fib.n_nodes_);
        x_new.row(0) = fibers_xt.segment(xt_offset + 0 * fib.n_nodes_, fib.n_nodes_);
        x_new.row(1) = fibers_xt.segment(xt_offset + 1 * fib.n_nodes_, fib.n_nodes_);
        x_new.row(2) = fibers_xt.segment(xt_offset + 2 * fib.n_nodes_, fib.n_nodes_);

        double T_new_0 = fibers_xt(xt_offset + 3 * n_pts);

        Vector3d xs_0 = fib.xs_.col(0);
        Eigen::MatrixXd xss_new = pow(2.0 / fib.length_, 2) * x_new * fib_mats.D_2_0;
        Eigen::MatrixXd xsss_new = pow(2.0 / fib.length_, 3) * x_new * fib_mats.D_3_0;
        Vector3d xss_new_0 = xss_new.col(0);
        Vector3d xsss_new_0 = xsss_new.col(0);

        // FIRST FROM FIBER ON-TO BODY
        // Force by fiber on body at s = 0, Fext = -F|s=0 = -(EXsss - TXs)
        // Bending term + Tension term:
        Vector3d F_body = -fib.bending_rigidity_ * xsss_new_0 + xs_0 * T_new_0;

        // Torque by fiber on body at s = 0
        // Lext = (L + link_loc x F) = -E(Xss x Xs) - link_loc x (EXsss - TXs)
        // bending contribution :
        Vector3d L_body = -fib.bending_rigidity_ * site_pos.cross(xsss_new_0);

        // tension contribution :
        L_body += site_pos.cross(xs_0) * T_new_0;

        // fiber's torque L:
        L_body += fib.bending_rigidity_ * xs_0.cross(xss_new_0);

        // Store the contribution of each fiber in this array
        force_torque_on_bodies.col(i_body).segment(0, 3) += F_body;
        force_torque_on_bodies.col(i_body).segment(3, 3) += L_body;

        // SECOND FROM BODY ON-TO FIBER
        // Translational and angular velocities at the attachment point are calculated
        Vector3d v_body = body_velocities.block(0, i_body, 3, 1);
        Vector3d w_body = body_velocities.block(3, i_body, 3, 1);

        // dx/dt = U + Omega x link_loc (move to LHS so -U -Omega x link_loc)
        Vector3d v_fiber = -v_body - w_body.cross(site_pos);

        // tension condition = -(xs*vx + ys*vy + zs*wz)
        double tension_condition = -xs_0.dot(v_body) + (xs_0.cross(site_pos)).dot(w_body);

        // Rotational velocity condition on fiber
        // FIXME: Fiber torque assumes body is a sphere :(
        Vector3d w_fiber = site_pos.normalized().cross(w_body);

        velocities_on_fiber.block(0, i_fib, 3, 1) = v_fiber;
        velocities_on_fiber(3, i_fib) = tension_condition;
        velocities_on_fiber.block(4, i_fib, 3, 1) = w_fiber;

        i_fib++;
        xt_offset += 4 * n_pts;
    }

    // Sum up fiber contributions from all other ranks to body torques
    MPI_Allreduce(MPI_IN_PLACE, force_torque_on_bodies.data(), force_torque_on_bodies.size(), MPI_DOUBLE, MPI_SUM,
                  MPI_COMM_WORLD);

    return std::make_pair(force_torque_on_bodies, velocities_on_fiber);
}

/// @brief Get number of physical nodes local to MPI rank for each object type [fibers, shell, bodies]
std::tuple<int, int, int> get_local_node_counts() {
    return std::make_tuple(fc_.get_local_node_count(), shell_->get_local_node_count(), bc_.get_local_node_count());
}

/// @brief Get GMRES solution size local to MPI rank for each object type [fibers, shell, bodies]
std::tuple<int, int, int> get_local_solution_sizes() {
    return std::make_tuple(fc_.get_local_solution_size(), shell_->get_local_solution_size(),
                           bc_.get_local_solution_size());
}

/// @brief Map 1D array data to a three-tuple of Vector Maps [fibers, shell, bodies]
std::tuple<VectorMap, VectorMap, VectorMap> get_solution_maps(double *x) {
    using Eigen::Map;
    using Eigen::VectorXd;
    auto [fib_sol_size, shell_sol_size, body_sol_size] = System::get_local_solution_sizes();
    return std::make_tuple(VectorMap(x, fib_sol_size), VectorMap(x + fib_sol_size, shell_sol_size),
                           VectorMap(x + fib_sol_size + shell_sol_size, body_sol_size));
}

/// @brief Map 1D array data to a three-tuple of const Vector Maps [fibers, shell, bodies]
std::tuple<CVectorMap, CVectorMap, CVectorMap> get_solution_maps(const double *x) {
    using Eigen::Map;
    using Eigen::VectorXd;
    auto [fib_sol_size, shell_sol_size, body_sol_size] = System::get_local_solution_sizes();
    return std::make_tuple(CVectorMap(x, fib_sol_size), CVectorMap(x + fib_sol_size, shell_sol_size),
                           CVectorMap(x + fib_sol_size + shell_sol_size, body_sol_size));
}

/// @brief Map Eigen Matrix node data to a three-tuple of Matrix Block references (use like view)
///
/// @param[in] x [3 x n_nodes_local] Matrix where you want the views
/// @return Three-tuple of node data [3 x n_fiber_nodes, 3 x n_bodiy_nodes, 3 x n_periphery_nodes]
template <typename Derived>
std::tuple<Eigen::Block<Derived>, Eigen::Block<Derived>, Eigen::Block<Derived>>
get_node_maps(Eigen::MatrixBase<Derived> &x) {
    auto [fib_nodes, shell_nodes, body_nodes] = get_local_node_counts();
    return std::make_tuple(Eigen::Block<Derived>(x.derived(), 0, 0, 3, fib_nodes),
                           Eigen::Block<Derived>(x.derived(), 0, fib_nodes, 3, shell_nodes),
                           Eigen::Block<Derived>(x.derived(), 0, fib_nodes + shell_nodes, 3, body_nodes));
}

/// @brief Apply and return preconditioner results from fibers/body/shell
///
/// \f[ P^{-1} * x = y \f]
/// @param [in] x [local_solution_size] Vector to apply preconditioner on
/// @return [local_solution_size] Preconditioned input vector
Eigen::VectorXd apply_preconditioner(VectorRef &x) {
    const auto [fib_sol_size, shell_sol_size, body_sol_size] = get_local_solution_sizes();
    const int sol_size = fib_sol_size + shell_sol_size + body_sol_size;
    assert(sol_size == x.size());
    Eigen::VectorXd res(sol_size);

    auto [x_fibers, x_shell, x_bodies] = get_solution_maps(x.data());
    auto [res_fibers, res_shell, res_bodies] = get_solution_maps(res.data());

    res_fibers = fc_.apply_preconditioner(x_fibers);
    res_shell = shell_->apply_preconditioner(x_shell);
    res_bodies = bc_.apply_preconditioner(x_bodies);

    return res;
}

/// @brief Nucleate/grow/destroy Fibers based on dynamic instability rules. See white paper for details
///
/// Modifies:
/// - FiberContainer::fibers [for nucleation/catastrophe]
/// - Fiber::v_growth_
/// - Fiber::length_
/// - Fiber::length_prev_
void dynamic_instability() {
    const double dt = properties.dt;
    FiberContainer &fc = fc_;
    BodyContainer &bc = bc_;
    Params &params = params_;
    if (params_.dynamic_instability.n_nodes == 0)
        return;

    size_t n_sites = 0;
    std::vector<int> body_offsets(bc.bodies.size() + 1);
    int i_body = 0;
    for (const auto &body : bc.bodies) {
        n_sites += body->nucleation_sites_.cols();
        body_offsets[i_body + 1] = body_offsets[i_body] + body->nucleation_sites_.cols();
        i_body++;
    }

    auto site_index = [&body_offsets](std::pair<int, int> binding_site) -> int {
        assert(binding_site.first >= 0 && binding_site.second >= 0);
        return body_offsets[binding_site.first] + binding_site.second;
    };

    auto binding_site_from_index = [&body_offsets](int index) -> std::pair<int, int> {
        for (size_t i_body = 0; i_body < body_offsets.size() - 1; ++i_body) {
            if (index < body_offsets[i_body + 1]) {
                return {i_body, index - body_offsets[i_body]};
            }
        }
        return {-1, -1};
    };

    std::vector<uint8_t> occupied_flat(bc.get_global_site_count(), 0);
    auto fib = fc.fibers.begin();
    while (fib != fc.fibers.end()) {
        fib->v_growth_ = params.dynamic_instability.v_growth;
        double f_cat = params.dynamic_instability.f_catastrophe;
        if (fib->near_periphery) {
            fib->v_growth_ *= params.dynamic_instability.v_grow_collision_scale;
            f_cat *= params.dynamic_instability.f_catastrophe_collision_scale;
        }

        if (RNG::uniform() > exp(-dt * f_cat)) {
            fib = fc.fibers.erase(fib);
        } else {
            if (fib->attached_to_body())
                occupied_flat[site_index(fib->binding_site_)] = 1;
            fib->length_prev_ = fib->length_;
            fib->length_ += dt * fib->v_growth_;
            fib++;
        }
    }

    MPI_Reduce(rank_ == 0 ? MPI_IN_PLACE : occupied_flat.data(), occupied_flat.data(), occupied_flat.size(), MPI_BYTE,
               MPI_LOR, 0, MPI_COMM_WORLD);

    std::unordered_map<int, bool> active_sites;
    std::unordered_map<int, bool> inactive_sites;
    std::vector<std::pair<int, int>> to_nucleate;
    if (rank_ == 0) {
        for (size_t i = 0; i < occupied_flat.size(); ++i) {
            if (occupied_flat[i])
                active_sites[i] = true;
            else
                inactive_sites[i] = true;
        }

        // FIXME: Is this right? I feel like the nucleation rate should be proportional to the
        // sites, or the area, or something rather than a global parameter
        int n_to_nucleate = std::min(RNG::poisson_int(dt * params.dynamic_instability.nucleation_rate),
                                     static_cast<int>(inactive_sites.size()));
        int n_trials = 100;
        while (n_to_nucleate && n_trials) {
            int passive_site_index =
                std::next(inactive_sites.begin(), RNG::uniform_int(0, inactive_sites.size()))->first;

            auto [i_body, i_site] = binding_site_from_index(passive_site_index);
            Eigen::Vector3d site_pos_i = bc.at(i_body).nucleation_sites_.col(i_site);
            bool valid_site = true;
            const double min_ds2 = pow(params.dynamic_instability.min_separation, 2);
            for (auto &[active_site_index, dum] : active_sites) {
                auto [j_body, j_site] = binding_site_from_index(active_site_index);
                Eigen::Vector3d site_pos_j = bc.at(j_body).nucleation_sites_.col(j_site);

                if ((site_pos_j - site_pos_i).squaredNorm() < min_ds2) {
                    valid_site = false;
                    break;
                }
            }

            if (valid_site) {
                n_trials = 100;
                active_sites[passive_site_index] = true;
                inactive_sites.erase(passive_site_index);
                to_nucleate.push_back({i_body, i_site});
                n_to_nucleate--;
            } else {
                n_trials--;
            }
        }
    }

    int n_fibers = fc.fibers.size();
    std::vector<int> fiber_counts(rank_ == 0 ? size_ : 0);
    MPI_Gather(&n_fibers, 1, MPI_INT, fiber_counts.data(), 1, MPI_INT, 0, MPI_COMM_WORLD);

    using fiber_struct = struct {
        int rank;
        std::pair<int, int> binding_site;
    };

    std::vector<fiber_struct> new_fibers;
    for (const auto &binding_site : to_nucleate) {
        const int fiber_rank = std::min_element(fiber_counts.begin(), fiber_counts.end()) - fiber_counts.begin();
        new_fibers.push_back({fiber_rank, binding_site});
        spdlog::info("Queueing fiber insertion to rank {} to site [{}, {}]", fiber_rank, binding_site.first,
                     binding_site.second);
        fiber_counts[fiber_rank]++;
    }

    int n_new = new_fibers.size();
    MPI_Bcast(&n_new, 1, MPI_INT, 0, MPI_COMM_WORLD);
    new_fibers.resize(n_new);
    MPI_Bcast(new_fibers.data(), sizeof(fiber_struct) * new_fibers.size(), MPI_CHAR, 0, MPI_COMM_WORLD);
    if (n_new)
        spdlog::info("Sent {} fibers to nucleate", new_fibers.size());

    for (const auto &min_fib : new_fibers) {
        if (min_fib.rank == rank_) {
            Fiber fib(params.dynamic_instability.n_nodes, params.dynamic_instability.bending_rigidity, params.eta);
            fib.length_ = params.dynamic_instability.min_length;
            fib.length_prev_ = params.dynamic_instability.min_length;
            fib.v_growth_ = 0.0;
            fib.binding_site_ = min_fib.binding_site;

            Eigen::MatrixXd x(3, fib.n_nodes_);
            Eigen::ArrayXd s = Eigen::ArrayXd::LinSpaced(fib.n_nodes_, 0, fib.length_).transpose();
            Eigen::Vector3d origin = bc_.get_nucleation_site(fib.binding_site_.first, fib.binding_site_.second);
            Eigen::Vector3d u = (origin - bc_.bodies[fib.binding_site_.first]->position_).normalized();

            for (int i = 0; i < 3; ++i)
                fib.x_.row(i) = origin(i) + u(i) * s;

            fc.fibers.push_back(fib);
            spdlog::get("SkellySim global")
                ->info("Inserted fiber on rank {} at site [{}, {}]", rank_, min_fib.binding_site.first,
                       min_fib.binding_site.second);
        }
    }
}

/// @brief Apply and return entire operator on system state vector for fibers/body/shell
///
/// \f[ A * x = y \f]
/// @param [in] x [local_solution_size] Vector to apply matvec on
/// @return [local_solution_size] Vector y, the result of the operator applied to x.
Eigen::VectorXd apply_matvec(VectorRef &x) {
    using Eigen::Block;
    using Eigen::MatrixXd;
    const FiberContainer &fc = fc_;
    const Periphery &shell = *shell_;
    const BodyContainer &bc = bc_;
    const double eta = params_.eta;

    const auto [fib_node_count, shell_node_count, body_node_count] = get_local_node_counts();
    const int total_node_count = fib_node_count + shell_node_count + body_node_count;

    const auto [fib_sol_size, shell_sol_size, body_sol_size] = get_local_solution_sizes();
    const int sol_size = fib_sol_size + shell_sol_size + body_sol_size;
    assert(sol_size == x.size());
    Eigen::VectorXd res(sol_size);

    MatrixXd r_all(3, total_node_count), v_all(3, total_node_count);
    auto [r_fibers, r_shell, r_bodies] = get_node_maps(r_all);
    auto [v_fibers, v_shell, v_bodies] = get_node_maps(v_all);
    r_fibers = fc.get_local_node_positions();
    r_shell = shell.get_local_node_positions();
    r_bodies = bc.get_local_node_positions();

    auto [x_fibers, x_shell, x_bodies] = get_solution_maps(x.data());
    auto [res_fibers, res_shell, res_bodies] = get_solution_maps(res.data());

    // calculate fiber-fiber velocity
    MatrixXd fw = fc.apply_fiber_force(x_fibers);
    Block<MatrixXd> r_shellbody = r_all.block(0, r_fibers.cols(), 3, r_shell.cols() + r_bodies.cols());
    MatrixXd v_fib2all = fc.flow(fw, r_shellbody, eta);

    MatrixXd r_fibbody(3, r_fibers.cols() + r_bodies.cols());
    r_fibbody.block(0, 0, 3, r_fibers.cols()) = r_fibers;
    r_fibbody.block(0, r_fibers.cols(), 3, r_bodies.cols()) = r_bodies;
    MatrixXd v_shell2fibbody = shell.flow(r_fibbody, x_shell, eta);

    MatrixXd body_velocities, body_densities, v_fib_boundary, force_torque_bodies;
    std::tie(body_velocities, body_densities) = bc.unpack_solution_vector(x_bodies);
    std::tie(force_torque_bodies, v_fib_boundary) =
        System::calculate_body_fiber_link_conditions(x_fibers, body_velocities);

    v_all = v_fib2all;
    v_fibers += v_shell2fibbody.block(0, 0, 3, r_fibers.cols());
    v_bodies += v_shell2fibbody.block(0, r_fibers.cols(), 3, r_bodies.cols());
    v_all += bc.flow(r_all, body_densities, force_torque_bodies, eta);

    res_fibers = fc.matvec(x_fibers, v_fibers, v_fib_boundary);
    res_shell = shell.matvec(x_shell, v_shell);
    res_bodies = bc.matvec(v_bodies, body_densities, body_velocities);

    return res;
}

/// @brief Generate next trial system state for the current System::properties::dt
///
/// @note Modifies anything that evolves in time.
/// @return If the Matrix solver converged to the requested tolerance with no issue.
bool step() {
    using Eigen::MatrixXd;
    Params &params = params_;
    Periphery &shell = *shell_;
    FiberContainer &fc = fc_;
    BodyContainer &bc = bc_;
    const double eta = params.eta;
    const double dt = properties.dt;

    // Since DI can change size of fiber containers, must call first.
    System::dynamic_instability();

    const auto [fib_node_count, shell_node_count, body_node_count] = get_local_node_counts();

    MatrixXd r_trg_external(3, shell_node_count + body_node_count);
    r_trg_external.block(0, 0, 3, shell_node_count) = shell.get_local_node_positions();
    r_trg_external.block(0, shell_node_count, 3, body_node_count) = bc.get_local_node_positions();

    fc.update_cache_variables(dt, eta);

    MatrixXd f_on_fibers = fc.generate_constant_force();
    MatrixXd v_all = fc.flow(f_on_fibers, r_trg_external, eta);

    bc.update_cache_variables(eta);

    // Check for an add external body forces
    Eigen::MatrixXd force_torque_bodies = Eigen::MatrixXd::Zero(6, bc.bodies.size());
    for (size_t i = 0; i < bc.bodies.size(); ++i) {
        const auto &body = bc.bodies[i];
        force_torque_bodies.col(i).segment(0, 3) += body->external_force_;
    }

    if (force_torque_bodies.any()) {
        const int total_node_count = fib_node_count + shell_node_count + body_node_count;
        MatrixXd r_all(3, total_node_count);
        auto [r_fibers, r_shell, r_bodies] = get_node_maps(r_all);
        r_fibers = fc.get_local_node_positions();
        r_shell = shell.get_local_node_positions();
        r_bodies = bc.get_local_node_positions();

        v_all += bc.flow(r_all, Eigen::MatrixXd::Zero(r_bodies.rows(), r_bodies.cols()), force_torque_bodies, eta);
    }

    bc.update_RHS(v_all.block(0, fib_node_count + shell_node_count, 3, body_node_count));

    fc.update_RHS(dt, v_all.block(0, 0, 3, fib_node_count), f_on_fibers);
    fc.update_boundary_conditions(shell, params.periphery_binding_flag);
    fc.apply_bc_rectangular(dt, v_all.block(0, 0, 3, fib_node_count), f_on_fibers);

    shell.update_RHS(v_all.block(0, fib_node_count, 3, shell_node_count));

    Solver<P_inv_hydro, A_fiber_hydro> solver_;
    solver_.set_RHS();
    bool converged = solver_.solve();
    CVectorMap sol = solver_.get_solution();

    double residual = solver_.get_residual();
    spdlog::info("Residual: {}", residual);

    auto [fiber_sol, shell_sol, body_sol] = get_solution_maps(sol.data());

    size_t offset = 0;
    for (auto &fib : fc.fibers) {
        for (int i = 0; i < 3; ++i)
            fib.x_.row(i) = sol.segment(offset + i * fib.n_nodes_, fib.n_nodes_);
        offset += 4 * fib.n_nodes_;
    }

    Eigen::MatrixXd body_velocities, body_densities;
    std::tie(body_velocities, body_densities) = bc.unpack_solution_vector(body_sol);
    for (int i = 0; i < body_velocities.cols(); ++i) {
        std::stringstream ss;
        ss << body_velocities.col(i).transpose();
        spdlog::debug("body velocities {}: [{}]", i, ss.str());
    }

    for (int i = 0; i < bc.bodies.size(); ++i) {
        auto &body = bc.bodies[i];
        Eigen::Vector3d x_new = body->position_ + body_velocities.col(i).segment(0, 3) * dt;
        Eigen::Vector3d phi = body_velocities.col(i).segment(3, 3) * dt;
        double phi_norm = phi.norm();
        Eigen::Quaterniond orientation_new = body->orientation_;
        if (phi_norm) {
            double s = std::cos(0.5 * phi_norm);
            Eigen::Vector3d p = std::sin(0.5 * phi_norm) * phi / phi_norm;
            orientation_new = Eigen::Quaterniond(s, p[0], p[1], p[2]) * body->orientation_;
        }
        std::stringstream ss;
        ss << x_new.transpose();
        spdlog::debug("Moving body {}: [{}]", i, ss.str());

        body->move(x_new, orientation_new);
    }

    // Re-pin fibers to bodies
    for (auto &fib : fc.fibers) {
        if (fib.binding_site_.first >= 0) {
            Eigen::Vector3d delta =
                bc.get_nucleation_site(fib.binding_site_.first, fib.binding_site_.second) - fib.x_.col(0);
            fib.x_.colwise() += delta;
        }
    }

    return converged;
}

/// @brief store copies of Fiber and Body containers in case time step is rejected
void backup() {
    fc_bak_ = fc_;
    bc_bak_ = bc_;
}

/// @brief restore copies of Fiber and Body containers to the state when last backed up
void restore() {
    fc_ = fc_bak_;
    bc_ = bc_bak_;
}

/// @brief Run the simulation!
void run() {
    Params &params = params_;

    System::write();
    while (properties.time < params.t_final) {
        System::backup();
        bool converged = System::step();
        double fiber_error = 0.0;
        for (const auto &fib : fc_.fibers) {
            const auto &mats = fib.matrices_.at(fib.n_nodes_);
            const Eigen::MatrixXd xs = std::pow(2.0 / fib.length_, 1) * fib.x_ * mats.D_1_0;
            for (int i = 0; i < fib.n_nodes_; ++i)
                fiber_error = std::max(fabs(xs.col(i).norm() - 1.0), fiber_error);
        }
        MPI_Allreduce(MPI_IN_PLACE, &fiber_error, 1, MPI_DOUBLE, MPI_MAX, MPI_COMM_WORLD);

        double dt_new = properties.dt;
        bool accept = false;
        if (converged && fiber_error <= params.fiber_error_tol) {
            accept = true;
            const double tol_window = 0.9 * params.fiber_error_tol;
            if (fiber_error <= tol_window)
                dt_new = std::min(params.dt_max, properties.dt * params.beta_up);
        } else {
            dt_new = properties.dt * params.beta_down;
            accept = false;
        }

        if (converged && System::check_collision()) {
            spdlog::info("Collision detected, rejecting solution and taking a smaller timestep");
            dt_new = properties.dt * 0.5;
            accept = false;
        }

        if (dt_new < params.dt_min) {
            spdlog::info("System time, dt, fiber_error: {}, {}, {}", properties.time, dt_new, fiber_error);
            spdlog::critical("Timestep smaller than minimum allowed");
            throw std::runtime_error("Timestep smaller than dt_min");
        }

        if (accept) {
            spdlog::info("Accepting timestep and advancing time");
            properties.time += properties.dt;
            double &dt_write = params_.dt_write;
            if ((int)(properties.time / dt_write) > (int)((properties.time - properties.dt) / dt_write))
                System::write();
        } else {
            spdlog::info("Rejecting timestep");
            System::restore();
        }
        properties.dt = dt_new;
        spdlog::info("System time, dt, fiber_error: {}, {}, {}", properties.time, dt_new, fiber_error);
    }

    System::write();
}

/// @brief Check for any collisions between objects
bool check_collision() {
    BodyContainer &bc = bc_;
    FiberContainer &fc = fc_;
    Periphery &shell = *shell_;
    const double threshold = 0.0;
    using Eigen::VectorXd;

    char collided = false;
    for (const auto &body : bc.bodies)
        if (!collided && body->check_collision(shell, threshold))
            collided = true;

    for (const auto &fiber : fc.fibers)
        if (!collided && shell.check_collision(fiber.x_, threshold))
            collided = true;

    for (auto &body1 : bc.bodies)
        for (auto &body2 : bc.bodies)
            if (!collided && body1 != body2 && body1->check_collision(*body2, threshold))
                collided = true;

    MPI_Allreduce(MPI_IN_PLACE, &collided, 1, MPI_CHAR, MPI_LOR, MPI_COMM_WORLD);

    return collided;
}

/// @brief Return copy of fiber container's RHS
Eigen::VectorXd get_fiber_RHS() { return fc_.get_RHS(); }
/// @brief Return copy of body container's RHS
Eigen::VectorXd get_body_RHS() { return bc_.get_RHS(); }
/// @brief Return copy of shell's RHS
Eigen::VectorXd get_shell_RHS() { return shell_->get_RHS(); }

/// @brief get pointer to params struct
Params *get_params() { return &params_; }
/// @brief get pointer to body container
BodyContainer *get_body_container() { return &bc_; }
/// @brief get pointer to fiber container
FiberContainer *get_fiber_container() { return &fc_; }
/// @brief get pointer to shell
Periphery *get_shell() { return shell_.get(); }
/// @brief get pointer to param table struct
toml::value *get_param_table() { return &param_table_; }

/// @brief Initialize entire system. Needs to be called once at the beginning of the program execution
/// @param[in] input_file String of toml config file specifying system parameters and initial conditions
/// @param[in] resume_flag true if simulation is resuming from prior execution state, false otherwise.
void init(const std::string &input_file, bool resume_flag) {
    MPI_Comm_rank(MPI_COMM_WORLD, &rank_);
    MPI_Comm_size(MPI_COMM_WORLD, &size_);
    spdlog::logger sink = rank_ == 0
                              ? spdlog::logger("SkellySim", std::make_shared<spdlog::sinks::ansicolor_stdout_sink_st>())
                              : spdlog::logger("SkellySim", std::make_shared<spdlog::sinks::null_sink_st>());
    spdlog::set_default_logger(std::make_shared<spdlog::logger>(sink));
    spdlog::stdout_color_mt("STKFMM");
    spdlog::stdout_color_mt("Belos");
    spdlog::stdout_color_mt("SkellySim global");
    spdlog::cfg::load_env_levels();

    param_table_ = toml::parse(input_file);
    params_ = Params(param_table_.at("params"));
    RNG::init(params_.seed);
    preprocess(param_table_);

    if (param_table_.contains("fibers"))
        fc_ = FiberContainer(param_table_.at("fibers").as_array(), params_);

    if (param_table_.contains("periphery")) {
        const toml::value &periphery_table = param_table_.at("periphery");
        if (!params_.shell_precompute_file.length())
            throw std::runtime_error("Periphery specified, but no precompute file. Set [params] shell_precompute_file "
                                     "in your input config and run the precompute script.");
        if (toml::find_or(periphery_table, "shape", "") == std::string("sphere"))
            shell_ = std::make_unique<SphericalPeriphery>(params_.shell_precompute_file, periphery_table, params_);
        else {
            throw std::runtime_error("Unknown shape for periphery set in input file. Valid values are 'sphere'");
        }
    } else {
        shell_ = std::make_unique<Periphery>();
    }

    if (param_table_.contains("bodies"))
        bc_ = BodyContainer(param_table_.at("bodies").as_array(), params_);
    properties.dt = params_.dt_initial;

    std::string filename = "skelly_sim.out." + std::to_string(rank_);
    if (resume_flag) {
        resume_from_trajectory(filename);
        ofs_ = std::ofstream(filename, std::ofstream::out | std::ofstream::binary | std::ofstream::app);
    } else {
        ofs_ = std::ofstream(filename, std::ofstream::out | std::ofstream::binary);
    }
}
} // namespace System


/*
 * Copyright (c) 2011, Mattia Penati <mattia.penati@gmail.com>
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright notice,
 *       this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright notice,
 *       this list of conditions and the following disclaimer in the documentation
 *       and/or other materials provided with the distribution.
 *     * Neither the name of the Politecnico di Milano nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
 * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef AMA_TENSOR_IEXP_SUM_HPP
#define AMA_TENSOR_IEXP_SUM_HPP 1

#include <ama/tensor/iexp/iexp_copy.hpp>
#include <ama/tensor/detail/copy.hpp>
#include <boost/mpl/assert.hpp>
#include <boost/mpl/equal_to.hpp>
#include <boost/mpl/fold.hpp>
#include <boost/mpl/insert.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/mpl/size.hpp>

namespace ama
{
  namespace tensor_
  {

    namespace mpl = ::boost::mpl;

    /* apply the sum: a trick to remove the last operation */
    template <typename T> T sum_op(T const & a, T const & b) { return a + b; }
    template <typename T> T sum_op(T const & a, mpl::na const &) { return a; }

    /* this struct implement the sum */
    template <
          typename D               /* the dimensione of tensor */
        , typename O               /* the order of tensor */
        , typename I = first<O>    /* the first multi-index */
        , typename R = mpl::false_ /* this type is true for the last multi-index */
        >
    struct sum
    {
      template <typename IMAP, typename ISUM, typename IEXP>
      static
      typename IEXP::value_type
      apply(IEXP const & iexp)
      {
        BOOST_MPL_ASSERT_MSG(
              (mpl::equal_to< mpl::size<I> , mpl::size<ISUM> >::value)
            , YOU_GIVE_A_INCORRECT_LIST_OF_INDICES
            , (I, ISUM));

        /* append to map */
        typedef typename mpl::fold<
              IMAP
            , typename make_imap<ISUM, I>::type
            , mpl::insert<mpl::_1, mpl::_2>
            >::type imap;

        /* increment the multi-index */
        typedef typename increment<D,I>::type increment_type;

        /* the first is the multi-index incremented */
        typedef typename mpl::first<increment_type>::type i;
        /* the second is a boolean flag toidentify the last multi-index */
        typedef typename mpl::second<increment_type>::type r;

        /* return */
        return sum_op(iexp.template at<imap>(),
                      sum<D,O,i,r>::template apply<IMAP, ISUM>(iexp));
      }
    };

    /* partial specialization, end the iterative call */
    template <typename D, typename O, typename I>
    struct sum<D,O,I,mpl::true_>
    {
      template <typename IMAP, typename ISUM, typename IEXP>
      static mpl::na apply(IEXP const &) { return mpl::na(); }
    };

  }
}

#endif /* AMA_TENSOR_IEXP_SUM_HPP */