/**
 * @file
 * @brief Implementation of charge propagation module with transient behavior simulation
 * @copyright Copyright (c) 2019-2020 CERN and the Allpix Squared authors.
 * This software is distributed under the terms of the MIT License, copied verbatim in the file "LICENSE.md".
 * In applying this license, CERN does not waive the privileges and immunities granted to it by virtue of its status as an
 * Intergovernmental Organization or submit itself to any jurisdiction.
 */

#include "TransientPropagationModule.hpp"

#include <map>
#include <memory>
#include <string>
#include <utility>

#include <Eigen/Core>

#include "core/utils/distributions.h"
#include "core/utils/log.h"
#include "objects/PixelCharge.hpp"
#include "objects/PropagatedCharge.hpp"
#include "tools/runge_kutta.h"

using namespace allpix;
using namespace ROOT::Math;

TransientPropagationModule::TransientPropagationModule(Configuration& config,
                                                       Messenger* messenger,
                                                       std::shared_ptr<Detector> detector)
    : Module(config, detector), messenger_(messenger), detector_(std::move(detector)) {
    // Enable multithreading of this module if multithreading is enabled
    allow_multithreading();

    // Save detector model
    model_ = detector_->getModel();

    // Require deposits message for single detector:
    messenger_->bindSingle<DepositedChargeMessage>(this, MsgFlags::REQUIRED);

    // Set default value for config variables
    config_.setDefault<double>("timestep", Units::get(0.01, "ns"));
    config_.setDefault<double>("integration_time", Units::get(25, "ns"));
    config_.setDefault<unsigned int>("charge_per_step", 10);

    // Models:
    config_.setDefault<std::string>("mobility_model", "jacoboni");
    config_.setDefault<std::string>("recombination_model", "none");

    config_.setDefault<double>("temperature", 293.15);
    config_.setDefault<bool>("output_plots", false);
    config_.setDefault<unsigned int>("distance", 1);
    config_.setDefault<bool>("ignore_magnetic_field", false);

    // Copy some variables from configuration to avoid lookups:
    temperature_ = config_.get<double>("temperature");
    timestep_ = config_.get<double>("timestep");
    integration_time_ = config_.get<double>("integration_time");
    distance_ = config_.get<unsigned int>("distance");
    charge_per_step_ = config_.get<unsigned int>("charge_per_step");

    output_plots_ = config_.get<bool>("output_plots");
    boltzmann_kT_ = Units::get(8.6173e-5, "eV/K") * temperature_;

    // Parameter for charge transport in magnetic field (approximated from graphs:
    // http://www.ioffe.ru/SVA/NSM/Semicond/Si/electric.html) FIXME
    electron_Hall_ = 1.15;
    hole_Hall_ = 0.9;
}

void TransientPropagationModule::initialize() {

    auto detector = getDetector();

    // Check for electric field
    if(!detector->hasElectricField()) {
        LOG(WARNING) << "This detector does not have an electric field.";
    }

    if(!detector_->hasWeightingPotential()) {
        throw ModuleError("This module requires a weighting potential.");
    }

    if(detector_->getElectricFieldType() == FieldType::LINEAR) {
        throw ModuleError("This module cannot be used with linear electric fields.");
    }

    // Prepare mobility model
    try {
        mobility_ = Mobility(config_.get<std::string>("mobility_model"), temperature_, detector->hasDopingProfile());
    } catch(ModelError& e) {
        throw InvalidValueError(config_, "mobility_model", e.what());
    }

    // Prepare recombination model
    try {
        recombination_ = Recombination(config_.get<std::string>("recombination_model"), detector->hasDopingProfile());
    } catch(ModelError& e) {
        throw InvalidValueError(config_, "recombination_model", e.what());
    }

    // Check for magnetic field
    has_magnetic_field_ = detector->hasMagneticField();
    if(has_magnetic_field_) {
        if(config_.get<bool>("ignore_magnetic_field")) {
            has_magnetic_field_ = false;
            LOG(WARNING) << "A magnetic field is switched on, but is set to be ignored for this module.";
        } else {
            LOG(DEBUG) << "This detector sees a magnetic field.";
            magnetic_field_ = detector_->getMagneticField();
        }
    }

    if(output_plots_) {
        potential_difference_ = CreateHistogram<TH1D>(
            "potential_difference",
            "Weighting potential difference between two steps;#left|#Delta#phi_{w}#right| [a.u.];events",
            500,
            0,
            1);
        induced_charge_histo_ = CreateHistogram<TH1D>("induced_charge_histo",
                                                      "Induced charge per time, all pixels;Drift time [ns];charge [e]",
                                                      static_cast<int>(integration_time_ / timestep_),
                                                      0,
                                                      static_cast<double>(Units::convert(integration_time_, "ns")));
        induced_charge_e_histo_ =
            CreateHistogram<TH1D>("induced_charge_e_histo",
                                  "Induced charge per time, electrons only, all pixels;Drift time [ns];charge [e]",
                                  static_cast<int>(integration_time_ / timestep_),
                                  0,
                                  static_cast<double>(Units::convert(integration_time_, "ns")));
        induced_charge_h_histo_ =
            CreateHistogram<TH1D>("induced_charge_h_histo",
                                  "Induced charge per time, holes only, all pixels;Drift time [ns];charge [e]",
                                  static_cast<int>(integration_time_ / timestep_),
                                  0,
                                  static_cast<double>(Units::convert(integration_time_, "ns")));
        step_length_histo_ =
            CreateHistogram<TH1D>("step_length_histo",
                                  "Step length;length [#mum];integration steps",
                                  100,
                                  0,
                                  static_cast<double>(Units::convert(0.25 * model_->getSensorSize().z(), "um")));

        drift_time_histo_ = CreateHistogram<TH1D>("drift_time_histo",
                                                  "Drift time;Drift time [ns];charge carriers",
                                                  static_cast<int>(Units::convert(integration_time_, "ns") * 5),
                                                  0,
                                                  static_cast<double>(Units::convert(integration_time_, "ns")));

        recombine_histo_ =
            CreateHistogram<TH1D>("recombination_histo",
                                  "Fraction of recombined charge carriers;recombination [N / N_{total}] ;number of events",
                                  100,
                                  0,
                                  1);
    }
}

void TransientPropagationModule::run(Event* event) {
    auto deposits_message = messenger_->fetchMessage<DepositedChargeMessage>(this, event);

    // Create vector of propagated charges to output
    std::vector<PropagatedCharge> propagated_charges;
    unsigned int propagated_charges_count = 0;
    unsigned int recombined_charges_count = 0;

    // Loop over all deposits for propagation
    LOG(TRACE) << "Propagating charges in sensor";
    for(const auto& deposit : deposits_message->getData()) {

        // Only process if within requested integration time:
        if(deposit.getLocalTime() > integration_time_) {
            LOG(DEBUG) << "Skipping charge carriers deposited beyond integration time: "
                       << Units::display(deposit.getGlobalTime(), "ns") << " global / "
                       << Units::display(deposit.getLocalTime(), {"ns", "ps"}) << " local";
            continue;
        }

        // Loop over all charges in the deposit
        unsigned int charges_remaining = deposit.getCharge();

        LOG(DEBUG) << "Set of charge carriers (" << deposit.getType() << ") on "
                   << Units::display(deposit.getLocalPosition(), {"mm", "um"});

        auto charge_per_step = charge_per_step_;
        while(charges_remaining > 0) {
            // Define number of charges to be propagated and remove charges of this step from the total
            if(charge_per_step > charges_remaining) {
                charge_per_step = charges_remaining;
            }
            charges_remaining -= charge_per_step;
            std::map<Pixel::Index, Pulse> px_map;

            // Get position and propagate through sensor
            auto [local_position, time, alive] = propagate(
                event, deposit.getLocalPosition(), deposit.getType(), charge_per_step, deposit.getLocalTime(), px_map);

            // Create a new propagated charge and add it to the list
            auto global_position = detector_->getGlobalPosition(local_position);
            PropagatedCharge propagated_charge(local_position,
                                               global_position,
                                               deposit.getType(),
                                               std::move(px_map),
                                               deposit.getLocalTime() + time,
                                               deposit.getGlobalTime() + time,
                                               &deposit);

            LOG(DEBUG) << " Propagated " << charge_per_step << " to " << Units::display(local_position, {"mm", "um"})
                       << " in " << Units::display(time, "ns") << " time, induced "
                       << Units::display(propagated_charge.getCharge(), {"e"});

            propagated_charges.push_back(std::move(propagated_charge));

            if(alive) {
                propagated_charges_count += charge_per_step;
            } else {
                recombined_charges_count += charge_per_step;
            }

            if(output_plots_) {
                drift_time_histo_->Fill(static_cast<double>(Units::convert(time, "ns")), charge_per_step);
            }
        }
    }

    if(output_plots_) {
        recombine_histo_->Fill(static_cast<double>(recombined_charges_count) /
                               (propagated_charges_count + recombined_charges_count));
    }

    // Create a new message with propagated charges
    auto propagated_charge_message = std::make_shared<PropagatedChargeMessage>(std::move(propagated_charges), detector_);

    // Dispatch the message with propagated charges
    messenger_->dispatchMessage(this, propagated_charge_message, event);
}

/**
 * Propagation is simulated using a parameterization for the electron mobility. This is used to calculate the electron
 * velocity at every point with help of the electric field map of the detector. A Runge-Kutta integration is applied in
 * multiple steps, adding a random diffusion to the propagating charge every step.
 */
std::tuple<ROOT::Math::XYZPoint, double, bool>
TransientPropagationModule::propagate(Event* event,
                                      const ROOT::Math::XYZPoint& pos,
                                      const CarrierType& type,
                                      const unsigned int charge,
                                      const double initial_time,
                                      std::map<Pixel::Index, Pulse>& pixel_map) {
    Eigen::Vector3d position(pos.x(), pos.y(), pos.z());

    // Define a function to compute the diffusion
    auto carrier_diffusion = [&](double efield_mag, double doping, double timestep) -> Eigen::Vector3d {
        double diffusion_constant = boltzmann_kT_ * mobility_(type, efield_mag, doping);
        double diffusion_std_dev = std::sqrt(2. * diffusion_constant * timestep);

        // Compute the independent diffusion in three
        allpix::normal_distribution<double> gauss_distribution(0, diffusion_std_dev);
        Eigen::Vector3d diffusion;
        for(int i = 0; i < 3; ++i) {
            diffusion[i] = gauss_distribution(event->getRandomEngine());
        }
        return diffusion;
    };

    // Survival probability of this charge carrier package, evaluated at every step
    std::uniform_real_distribution<double> survival(0, 1);

    // Define lambda functions to compute the charge carrier velocity with or without magnetic field
    std::function<Eigen::Vector3d(double, const Eigen::Vector3d&)> carrier_velocity_noB =
        [&](double, const Eigen::Vector3d& cur_pos) -> Eigen::Vector3d {
        auto raw_field = detector_->getElectricField(static_cast<ROOT::Math::XYZPoint>(cur_pos));
        Eigen::Vector3d efield(raw_field.x(), raw_field.y(), raw_field.z());

        auto doping = detector_->getDopingConcentration(static_cast<ROOT::Math::XYZPoint>(cur_pos));

        return static_cast<int>(type) * mobility_(type, efield.norm(), doping) * efield;
    };

    std::function<Eigen::Vector3d(double, const Eigen::Vector3d&)> carrier_velocity_withB =
        [&](double, const Eigen::Vector3d& cur_pos) -> Eigen::Vector3d {
        auto raw_field = detector_->getElectricField(static_cast<ROOT::Math::XYZPoint>(cur_pos));
        Eigen::Vector3d efield(raw_field.x(), raw_field.y(), raw_field.z());

        Eigen::Vector3d velocity;
        Eigen::Vector3d bfield(magnetic_field_.x(), magnetic_field_.y(), magnetic_field_.z());

        auto doping = detector_->getDopingConcentration(static_cast<ROOT::Math::XYZPoint>(cur_pos));

        auto mob = mobility_(type, efield.norm(), doping);
        auto exb = efield.cross(bfield);

        Eigen::Vector3d term1;
        double hallFactor = (type == CarrierType::ELECTRON ? electron_Hall_ : hole_Hall_);
        term1 = static_cast<int>(type) * mob * hallFactor * exb;

        Eigen::Vector3d term2 = mob * mob * hallFactor * hallFactor * efield.dot(bfield) * bfield;

        auto rnorm = 1 + mob * mob * hallFactor * hallFactor * bfield.dot(bfield);
        return static_cast<int>(type) * mob * (efield + term1 + term2) / rnorm;
    };

    // Create the runge kutta solver with an RKF5 tableau
    auto runge_kutta = make_runge_kutta(
        tableau::RK5, (has_magnetic_field_ ? carrier_velocity_withB : carrier_velocity_noB), timestep_, position);

    // Continue propagation until the deposit is outside the sensor
    Eigen::Vector3d last_position = position;
    bool within_sensor = true;
    bool is_alive = true;
    while(within_sensor && (initial_time + runge_kutta.getTime()) < integration_time_ && is_alive) {
        // Save previous position and time
        last_position = position;

        // Execute a Runge Kutta step
        auto step = runge_kutta.step();

        // Get the current result
        position = runge_kutta.getValue();

        // Get electric field at current position and fall back to empty field if it does not exist
        auto efield = detector_->getElectricField(static_cast<ROOT::Math::XYZPoint>(position));
        auto doping = detector_->getDopingConcentration(static_cast<ROOT::Math::XYZPoint>(position));

        // Apply diffusion step
        auto diffusion = carrier_diffusion(std::sqrt(efield.Mag2()), doping, timestep_);
        position += diffusion;
        runge_kutta.setValue(position);

        // Check if charge carrier is still alive:
        is_alive = !recombination_(type,
                                   detector_->getDopingConcentration(static_cast<ROOT::Math::XYZPoint>(position)),
                                   survival(event->getRandomEngine()),
                                   timestep_);

        // Update step length histogram
        if(output_plots_) {
            step_length_histo_->Fill(static_cast<double>(Units::convert(step.value.norm(), "um")));
        }

        // Check for overshooting outside the sensor and correct for it:
        if(!detector_->getModel()->isWithinSensor(static_cast<ROOT::Math::XYZPoint>(position))) {
            LOG(TRACE) << "Carrier outside sensor: " << Units::display(static_cast<ROOT::Math::XYZPoint>(position), {"nm"});
            // within_sensor = false;

            auto check_position = position;
            check_position.z() = last_position.z();
            // Correct for position in z by interpolation to increase precision:
            if(detector_->getModel()->isWithinSensor(static_cast<ROOT::Math::XYZPoint>(check_position))) {
                // FIXME this currently depends in the direction of the drift
                if(position.z() > 0 && type == CarrierType::HOLE) {
                    LOG(DEBUG) << "Not stopping carrier " << type << " at "
                               << Units::display(static_cast<ROOT::Math::XYZPoint>(position), {"um"});
                } else if(position.z() < 0 && type == CarrierType::ELECTRON) {
                    LOG(DEBUG) << "Not stopping carrier " << type << " at "
                               << Units::display(static_cast<ROOT::Math::XYZPoint>(position), {"um"});
                } else {
                    within_sensor = false;
                }

                // Carrier left sensor on top or bottom surface, interpolate
                auto z_cur_border = std::fabs(position.z() - model_->getSensorSize().z() / 2.0);
                auto z_last_border = std::fabs(model_->getSensorSize().z() / 2.0 - last_position.z());
                auto z_total = z_cur_border + z_last_border;
                position = (z_last_border / z_total) * position + (z_cur_border / z_total) * last_position;
                LOG(TRACE) << "Moved carrier to: " << Units::display(static_cast<ROOT::Math::XYZPoint>(position), {"nm"});
            } else {
                within_sensor = false;
            }
        }

        // Find the nearest pixel - before and after the step
        auto [xpixel, ypixel] = model_->getPixelIndex(static_cast<ROOT::Math::XYZPoint>(position));
        auto [last_xpixel, last_ypixel] = model_->getPixelIndex(static_cast<ROOT::Math::XYZPoint>(last_position));
        auto idx = Pixel::Index(static_cast<unsigned int>(xpixel), static_cast<unsigned int>(ypixel));
        auto neighbors = model_->getNeighbors(idx, distance_);

        // If the charge carrier crossed pixel boundaries, ensure that we always calculate the induced current for both of
        // them by extending the induction matrix temporarily. Otherwise we end up doing "double-counting" because we would
        // only jump "into" a pixel but never "out". At the border of the induction matrix, this would create an imbalance.
        if(last_xpixel != xpixel || last_ypixel != ypixel) {
            auto last_idx = Pixel::Index(static_cast<unsigned int>(last_xpixel), static_cast<unsigned int>(last_ypixel));
            neighbors.merge(model_->getNeighbors(last_idx, distance_));
            LOG(TRACE) << "Carrier crossed boundary from pixel "
                       << Pixel::Index(static_cast<unsigned int>(last_xpixel), static_cast<unsigned int>(last_ypixel))
                       << " to pixel " << Pixel::Index(static_cast<unsigned int>(xpixel), static_cast<unsigned int>(ypixel));
        }
        LOG(TRACE) << "Moving carriers below pixel "
                   << Pixel::Index(static_cast<unsigned int>(xpixel), static_cast<unsigned int>(ypixel)) << " from "
                   << Units::display(static_cast<ROOT::Math::XYZPoint>(last_position), {"um", "mm"}) << " to "
                   << Units::display(static_cast<ROOT::Math::XYZPoint>(position), {"um", "mm"}) << ", "
                   << Units::display(initial_time + runge_kutta.getTime(), "ns");

        for(const auto& pixel_index : neighbors) {
            auto ramo = detector_->getWeightingPotential(static_cast<ROOT::Math::XYZPoint>(position), pixel_index);
            auto last_ramo = detector_->getWeightingPotential(static_cast<ROOT::Math::XYZPoint>(last_position), pixel_index);

            // Induced charge on electrode is q_int = q * (phi(x1) - phi(x0))
            auto induced = charge * (ramo - last_ramo) * static_cast<std::underlying_type<CarrierType>::type>(type);
            LOG(TRACE) << "Pixel " << pixel_index << " dPhi = " << (ramo - last_ramo) << ", induced " << type
                       << " q = " << Units::display(induced, "e");

            // Create pulse if it doesn't exist. Store induced charge in the returned pulse iterator
            auto pixel_map_iterator = pixel_map.emplace(pixel_index, Pulse(timestep_));
            pixel_map_iterator.first->second.addCharge(induced, initial_time + runge_kutta.getTime());

            if(output_plots_) {
                potential_difference_->Fill(std::fabs(ramo - last_ramo));
                induced_charge_histo_->Fill(initial_time + runge_kutta.getTime(), induced);
                if(type == CarrierType::ELECTRON) {
                    induced_charge_e_histo_->Fill(initial_time + runge_kutta.getTime(), induced);
                } else {
                    induced_charge_h_histo_->Fill(initial_time + runge_kutta.getTime(), induced);
                }
            }
        }
    }

    // Return the final position of the propagated charge
    return std::make_tuple(static_cast<ROOT::Math::XYZPoint>(position), initial_time + runge_kutta.getTime(), is_alive);
}

void TransientPropagationModule::finalize() {
    if(output_plots_) {
        potential_difference_->Write();
        step_length_histo_->Write();
        drift_time_histo_->Write();
        recombine_histo_->Write();
        induced_charge_histo_->Write();
        induced_charge_e_histo_->Write();
        induced_charge_h_histo_->Write();
    }
}


#include "stiffness_checker/Stiffness.h"
#include "stiffness_checker/Util.h"
#include "stiffness_checker/SharedConst.h"
#include "stiffness_checker/StiffnessSolver.h"
#include "stiffness_checker/StiffnessIO.h"

#include <cstdlib>
#include <cmath>
#include <set>
#include <algorithm>
#include <numeric>
#include <chrono>
#include <ctime>

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

namespace{
const std::string PathSeparator =
#ifdef _WIN32
"\\";
#else
"/";
#endif
}

namespace conmech
{
namespace stiffness_checker
{

Stiffness Stiffness::create(const Eigen::MatrixXd& V, const Eigen::MatrixXi& E, const Eigen::MatrixXi& Fixities,
                            const std::vector<conmech::material::Material>& materials,
                            const bool& verbose, const std::string& model_type, const bool& output_json)
{
  return Stiffness(V, E, Fixities, materials, verbose, model_type, output_json);
}

Stiffness::Stiffness(const std::string& file_path,
                     const bool& verbose, const std::string& model_type, const bool& output_json)
{
  Eigen::MatrixXd V;
  Eigen::MatrixXi E;
  Eigen::MatrixXi Fixities;
  std::vector<conmech::material::Material> mats;
  parseFrameJson(file_path, V, E, Fixities, mats);

  init(V, E, Fixities, mats, verbose, model_type, output_json);
}

Stiffness::Stiffness(const Eigen::MatrixXd& V, const Eigen::MatrixXi& E, const Eigen::MatrixXi& Fixities,
                     const std::vector<conmech::material::Material>& materials,
                     const bool& verbose, const std::string& model_type, const bool& output_json)
{
  init(V, E, Fixities, materials, verbose, model_type, output_json);
}

Stiffness::Stiffness(const Eigen::MatrixXd& V, const Eigen::MatrixXi& E, const Eigen::MatrixXi& Fixities,
                     const std::vector<nlohmann::json>& material_jsons,
                     const bool& verbose, const std::string& model_type, const bool& output_json)
{
  using namespace conmech::material;
  std::vector<Material> materials;
  for (const auto& m_json : material_jsons)
  {
    Material m(m_json);
    materials.push_back(m);
  }
  init(V, E, Fixities, materials, verbose, model_type, output_json);
}

Stiffness::~Stiffness()
{
}

bool Stiffness::init(const Eigen::MatrixXd& V, const Eigen::MatrixXi& E, const Eigen::MatrixXi& Fixities,
                     const std::vector<conmech::material::Material>& materials,
                     const bool& verbose, const std::string& model_type, const bool& output_json)
{
  Vertices_ = V;
  Elements_ = E;
  Fixities_ = Fixities;
  materials_ = materials;
  verbose_ = verbose;
  model_type_ = model_type;
  write_result_ = output_json,
  stiff_solver_.timing_ = verbose;

  // default settings
  is_init_ = false;
  include_self_weight_load_ = false;
  has_stored_deformation_ = false;
  stored_compliance_ = -1.0;
  transl_tol_ = 1e-3;
  rot_tol_ = 3 * (3.14 / 180);
  output_json_file_name_ = "";
  output_json_file_path_ = "";

  // init starts
  dim_ = int(this->Vertices_.cols());

  // TODO: generalize to 2D
  ASSERT(3 == dim_, "only support 3D structure now!");

  if (3 == dim_) {
    gravity_direction_ = Eigen::VectorXd(3);
    gravity_direction_ << 0, 0, -1;
  } else {
    gravity_direction_ = Eigen::VectorXd(2);
    gravity_direction_ << 0, -1;
  }

  // set up dimension, node_dof
  if (3 == dim_)
  {
    // 3D case, x, y, z, xx, yy, zz
    full_node_dof_ = 6;
    if (model_type_ == "frame")
    {
      node_dof_ = 6;
      xyz_dof_id_ = Eigen::VectorXi::LinSpaced(node_dof_ * 2, 0, node_dof_ * 2 - 1);
      e_react_dof_id_ = Eigen::VectorXi::LinSpaced(node_dof_ * 2, 0, node_dof_ * 2 - 1);
    }
    if (model_type_ == "truss")
    {
      node_dof_ = 3;
      xyz_dof_id_ = Eigen::VectorXi(6);
      xyz_dof_id_ << 0, 1, 2, 6, 7, 8;

      e_react_dof_id_ = Eigen::VectorXi(2);
      e_react_dof_id_ << 0, 6;
    }
  }
  else
  {
    // 2D case, x, y, theta
    full_node_dof_ = 3;
    if (model_type_ == "frame")
    {
      node_dof_ = 3;
      xyz_dof_id_ = Eigen::VectorXi::LinSpaced(node_dof_ * 2, 0, node_dof_ * 2 - 1);
      e_react_dof_id_ = Eigen::VectorXi::LinSpaced(node_dof_ * 2, 0, node_dof_ * 2 - 1);
    }
    if (model_type_ == "truss")
    {
      node_dof_ = 2;
      xyz_dof_id_ = Eigen::VectorXi(4);
      xyz_dof_id_ << 0, 1, 3, 4;

      e_react_dof_id_ = Eigen::VectorXi(2);
      e_react_dof_id_ << 0, 3;
    }
  }
  ASSERT(xyz_dof_id_.size() == node_dof_ * 2, "");

  // init per-element material properties
  precomputeElementStiffnessMatrixList();
  precomputeElementSelfWeightLumpedLoad();

  // create id_map_
  int dof = nV() * node_dof_;
  int N_element = nE();
  int N_node = nV();

  id_map_.resize(N_element, node_dof_ * 2);
  v_id_map_.resize(N_node, node_dof_);
  auto lin_sp_id = Eigen::VectorXi::LinSpaced(node_dof_, 0, node_dof_ - 1); // 0,1,..,5

  for (int i = 0; i < N_node; i++)
  {
    v_id_map_.block(i, 0, 1, node_dof_) =
      (Eigen::VectorXi::Constant(node_dof_, node_dof_ * i) + lin_sp_id).transpose();
  }

  for (int i = 0; i < N_element; i++)
  {
    const auto end_u_id = Elements_(i, 0);
    const auto end_v_id = Elements_(i, 1);

    id_map_.block(i, 0, 1, node_dof_) =
      (Eigen::VectorXi::Constant(node_dof_, node_dof_ * end_u_id) + lin_sp_id).transpose();
    id_map_.block(i, node_dof_, 1, node_dof_) =
      (Eigen::VectorXi::Constant(node_dof_, node_dof_ * end_v_id) + lin_sp_id).transpose();
  }

  // create zero load
  nodal_load_P_ = Eigen::VectorXd::Zero(dof);

  if (verbose_)
  {
    std::cout << "Initialization done" << std::endl;
  }
  is_init_ = true;
  return true;
}

void Stiffness::setNodalDisplacementTolerance(double transl_tol, double rot_tol)
{
  if(transl_tol <= 0 || rot_tol <= 0) {
    throw std::runtime_error("invalid tolerance: tolerance must be bigger than 0!");
  }
  transl_tol_ = transl_tol;
  rot_tol_ = rot_tol;
}

void Stiffness::precomputeElementStiffnessMatrixList()
{
  // complete element stiffness matrix
  // in local frame: [MSA McGuire et al.] P73

  // for all cross section's geometrical properties
  // cross section: m^2
  // Iz, Iy: m^4
  // J: m^4
  // element length L: m
  // E, G: kN/m^2
  // Force: kN
  // Moment: kN-m

  int N_element = nE();

  element_K_list_.clear();
  rot_m_list_.clear();
  element_K_list_.reserve(N_element);
  rot_m_list_.reserve(N_element);

  for (int i = 0; i < N_element; i++)
  {
    int end_u_id = Elements_(i, 0);
    int end_v_id = Elements_(i, 1);
    Eigen::VectorXd end_u, end_v;
    getNodePoints(Vertices_, end_u_id, end_v_id, end_u, end_v);

    // element length, unit: m
    double L = (end_u - end_v).norm();

    // material properties
    double A = materials_[i].cross_sec_area_;
    double Jx = materials_[i].Jx_;
    double Iy = materials_[i].Iy_;
    double Iz = materials_[i].Iz_;

    // E,G: MPa; mu (poisson ratio): unitless
    double E = materials_[i].youngs_modulus_;
    double mu = materials_[i].poisson_ratio_;
    double G = materials_[i].shear_modulus_;

    Eigen::Matrix3d R_LG;
    getGlobal2LocalRotationMatrix(end_u, end_v, R_LG);

    // element stiffness matrix in local frame
    Eigen::MatrixXd K_loc;
    createLocalStiffnessMatrix(L, A, dim_, Jx, Iy, Iz, E, G, mu, K_loc);

    // transform to global frame
    Eigen::MatrixXd R_LG_diag = Eigen::MatrixXd::Zero(full_node_dof_ * 2, full_node_dof_ * 2);

    // 4 block or 2 block
    for (int j = 0; j < (full_node_dof_ / 3) * 2; j++)
    {
      R_LG_diag.block<3, 3>(j * 3, j * 3) = R_LG;
    }

    Eigen::MatrixXd R_temp = Eigen::MatrixXd::Zero(e_react_dof_id_.size(), xyz_dof_id_.size());
    Eigen::MatrixXd K_loc_temp = Eigen::MatrixXd::Zero(e_react_dof_id_.size(), e_react_dof_id_.size());

    for (int k = 0; k < e_react_dof_id_.size(); k++)
    {
      for (int kk = 0; kk < xyz_dof_id_.size(); kk++)
      {
        // assert(e_react_dof_id_(k) < R_LG_diag.rows() && xyz_dof_id_(kk) < R_LG_diag.cols());
        R_temp(k, kk) = R_LG_diag(e_react_dof_id_(k), xyz_dof_id_(kk));
      }
    }
    R_LG_diag = R_temp;

    for (int k = 0; k < e_react_dof_id_.size(); k++)
    {
      for (int kk = 0; kk < e_react_dof_id_.size(); kk++)
      {
        // assert(e_react_dof_id_(k) < K_loc.rows() && e_react_dof_id_(kk) < K_loc.cols());
        K_loc_temp(k, kk) = K_loc(e_react_dof_id_(k), e_react_dof_id_(kk));
      }
    }
    K_loc = K_loc_temp;

    auto R_LG_diagT = R_LG_diag.transpose();
    K_loc = R_LG_diagT * K_loc * R_LG_diag;

    element_K_list_.push_back(K_loc);
    rot_m_list_.push_back(R_LG_diag);
  }
}

void Stiffness::createCompleteGlobalStiffnessMatrix(const std::vector<int> &exist_e_ids)
{
  // std::clock_t c_start = std::clock();

  assert(element_K_list_.size() > 0);
  assert(exist_e_ids.size() > 0);
  assert(id_map_.rows() >= int(exist_e_ids.size()));

  int total_dof = nV() * node_dof_;
  int N_element = nE();

  K_assembled_full_.resize(total_dof, total_dof);
  std::vector<Eigen::Triplet<double>> K_triplets;

  for (const int& e_id : exist_e_ids)
  {
    assert(e_id >= 0 && e_id < nE());
    const auto K_e = element_K_list_[e_id];
    assert(K_e.rows() == 2 * node_dof_ && K_e.cols() == 2 * node_dof_);

    for (int i = 0; i < 2 * node_dof_; i++)
    {
      int row_id = id_map_(e_id, i);

      for (int j = 0; j < 2 * node_dof_; j++)
      {
        // TODO: threhold push_back by checking double_eps
        int col_id = id_map_(e_id, j);
        K_triplets.push_back(Eigen::Triplet<double>(row_id, col_id, K_e(i, j)));
      }
    }
  } // end e_id
  K_assembled_full_.setFromTriplets(K_triplets.begin(), K_triplets.end());

  // std::clock_t c_end = std::clock();
  // std::cout << "create global stiffness matrix: " << 1e3 * (c_end - c_start) / CLOCKS_PER_SEC << " ms" << std::endl;
}

void Stiffness::setLoad(const Eigen::MatrixXd &nodal_forces)
{
  // assert(is_init_);

  int dof = node_dof_ * nV();
  bool is_empty_ext = nodal_forces.isZero(0) || nodal_forces.rows() == 0;

  if (!is_empty_ext)
  {
    createExternalNodalLoad(nodal_forces, nodal_load_P_);
  } else {
    throw std::runtime_error("No load is assigned.");
  }
}

void Stiffness::setUniformlyDistributedLoad(const Eigen::MatrixXd &element_load_density)
{
  // assert(is_init_);
  bool is_empty_ext = element_load_density.isZero(0) || element_load_density.rows() == 0;
  if (!is_empty_ext)
  {
    precomputeElementUniformlyDistributedLumpedLoad(element_load_density);
  } else {
    throw std::runtime_error("No load is assigned.");
  }
}

void Stiffness::setGravityDirection(const Eigen::VectorXd& gravity_direction) {
    if (gravity_direction.size() == dim_) {
      gravity_direction_ = gravity_direction;
      precomputeElementSelfWeightLumpedLoad();
    }
  }

void Stiffness::createExternalNodalLoad(
  const Eigen::MatrixXd &nodal_forces, Eigen::VectorXd &ext_load)
{
  // assert(is_init_);
  // assert(nodal_forces.cols() == node_dof_ + 1);

  int full_dof = node_dof_ * nV();
  ext_load.resize(full_dof);

  for (int i = 0; i < nodal_forces.rows(); i++)
  {
    int v_id = int(nodal_forces(i, 0));

    ext_load.segment(v_id * 6, 6) = nodal_forces.block<1, 6>(i, 1);
  }
}

void Stiffness::computeLumpedUniformlyDistributedLoad(const Eigen::Vector3d &w_G, const Eigen::Matrix3d &R_LG, const double &Le,
  Eigen::VectorXd &fixed_end_lumped_load)
{
  fixed_end_lumped_load = Eigen::VectorXd::Zero(2 * node_dof_);

  // node 0,1 force
  fixed_end_lumped_load.segment(0, 3) = - w_G * Le / 2.0;
  fixed_end_lumped_load.segment(6, 3) = - w_G * Le / 2.0;

  // transform global load density to local density
  Eigen::Vector3d w_l = - R_LG * w_G;

  // node 0, 1 local moment
  Eigen::Vector3d M_0_l(3);
  M_0_l << 0, - w_l(2)*std::pow(Le,2)/12.0, w_l(1)*std::pow(Le,2)/12.0;
  Eigen::Vector3d M_1_l = - M_0_l;

  // transform local moment back to global
  fixed_end_lumped_load.segment(3, 3) = R_LG.transpose() * M_0_l;
  fixed_end_lumped_load.segment(9, 3) = R_LG.transpose() * M_1_l;

  // equivalent nodal load P_e = - P_fixed_end
  fixed_end_lumped_load *= -1;
}

void Stiffness::precomputeElementUniformlyDistributedLumpedLoad(const Eigen::MatrixXd &element_load_density)
{
  // refer [MSA McGuire et al.] P111
  // Loads between nodal points
  int N_vert = nV();
  int N_element = nE();

  element_lumped_nload_list_.resize(N_element);
  for (int i = 0; i < N_element; i++)
  {
    element_lumped_nload_list_[i] = Eigen::VectorXd::Zero(6);
  }

  for (int i = 0; i < element_load_density.rows(); i++)
  {
    int e_id = int(element_load_density(i, 0));
    int end_u_id = Elements_(e_id, 0);
    int end_v_id = Elements_(e_id, 1);
    Eigen::VectorXd end_u, end_v;
    getNodePoints(Vertices_, end_u_id, end_v_id, end_u, end_v);
    double Le = (end_u - end_v).norm();

    Eigen::Vector3d w_g = element_load_density.block<1, 3>(i, 1);

    Eigen::Matrix3d R_LG;
    getGlobal2LocalRotationMatrix(end_u, end_v, R_LG);

    Eigen::VectorXd fixed_end_lumped_load;
    computeLumpedUniformlyDistributedLoad(w_g, R_LG, Le, fixed_end_lumped_load);

    element_lumped_nload_list_[e_id] = fixed_end_lumped_load;
  }
}

void Stiffness::createUniformlyDistributedLumpedLoad(const std::vector<int>& exist_e_ids, Eigen::VectorXd &ext_load)
{
  // solve-time calculation
  int N_vert = nV();
  ext_load = Eigen::VectorXd::Zero(N_vert * node_dof_);

  if (element_lumped_nload_list_.size() != nE())
  {
    return;
  }

  for (const int e_id : exist_e_ids)
  {
    // assert(0 <= e_id && e_id < element_lumped_nload_list_.size());
    auto Qe = element_lumped_nload_list_[e_id];

    for (int j = 0; j < id_map_.cols(); j++)
    {
      ext_load[id_map_(e_id, j)] += Qe[j];
    }
  }
}

void Stiffness::precomputeElementSelfWeightLumpedLoad()
{
  // TODO: reuse element uniformly distributed load?
  // gravity - Precomputation
  // refer [MSA McGuire et al.] P111
  // Loads between nodal points
  int N_vert = nV();
  int N_element = nE();

  element_gravity_nload_list_.clear();
  element_gravity_nload_list_.resize(N_element);

  for (int i = 0; i < N_element; i++)
  {
    int end_u_id = Elements_(i, 0);
    int end_v_id = Elements_(i, 1);
    Eigen::VectorXd end_u, end_v;
    getNodePoints(Vertices_, end_u_id, end_v_id, end_u, end_v);
    double Le = (end_u - end_v).norm();

    // uniform force density along the element
    // due to gravity
    // density kN / m^3 * m^2
    double q_sw = materials_[i].density_ * materials_[i].cross_sec_area_;

    if (model_type_ == "frame")
    {
      if (3 == dim_)
      {
        // Eigen::Vector3d w_g(3);
        // w_g << 0, 0, -q_sw;
        Eigen::Vector3d w_g = gravity_direction_ * q_sw;
        // std::cout << "gravity force: " << w_g.transpose() << std::endl;

        Eigen::Matrix3d R_LG;
        getGlobal2LocalRotationMatrix(end_u, end_v, R_LG);

        Eigen::VectorXd fixed_end_lumped_load;
        computeLumpedUniformlyDistributedLoad(w_g, R_LG, Le, fixed_end_lumped_load);

        element_gravity_nload_list_[i] = fixed_end_lumped_load;
      }
      else
      {
        // TODO
        assert(false && "2D frame gravity not implemented yet.");
      }
    }
    else
    {
      // TODO
      assert(false && "truss gravity not implemented yet.");
    }
  }
}

void Stiffness::createSelfWeightLumpedLoad(const std::vector<int> &exist_e_ids, Eigen::VectorXd &self_weight_load_P)
{
  // solve-time calculation
  // assert(is_init_);
  // assert(id_map_.cols() == 2 * node_dof_);

  int N_vert = nV();
  self_weight_load_P = Eigen::VectorXd::Zero(N_vert * node_dof_);

  for (const int e_id : exist_e_ids)
  {
    // assert(0 <= e_id && e_id < element_gravity_nload_list_.size());
    auto Qe = element_gravity_nload_list_[e_id];
    // assert(Qe.size() == id_map_.cols());

    for (int j = 0; j < id_map_.cols(); j++)
    {
      self_weight_load_P[id_map_(e_id, j)] += Qe[j];
    }
  }
}

bool Stiffness::solve(
  const std::vector<int> &exist_element_ids,
  Eigen::MatrixXd &node_displ,
  Eigen::MatrixXd &fixities_reaction,
  Eigen::MatrixXd &element_reaction,
  const bool &cond_num)
{
  using namespace std;

  int n_Element = nE();
  int n_Node = nV();

  if (verbose_)
  {
    create_k_.Start();
  }

  // start with assuming all dof does not exist (-1)
  int dof = node_dof_ * n_Node;
  Eigen::VectorXi full_f = Eigen::VectorXi::Constant(dof, -1);

  if(!(exist_element_ids.size() > 0 && int(exist_element_ids.size()) <= n_Element))
  {
      throw std::invalid_argument("input existing ids not within range!");
  }

  std::set<int> sub_nodes_set;
  for (int e_id : exist_element_ids)
  {
    // assert(e_id >= 0 && e_id < nE());
    int end_u_id = Elements_(e_id, 0);
    int end_v_id = Elements_(e_id, 1);
    sub_nodes_set.insert(end_u_id);
    sub_nodes_set.insert(end_v_id);

    // turn the existing node's dofs to free(0)
    full_f.segment(node_dof_ * end_u_id, node_dof_) = Eigen::VectorXi::Constant(node_dof_, 0);
    full_f.segment(node_dof_ * end_v_id, node_dof_) = Eigen::VectorXi::Constant(node_dof_, 0);
  }

  // Assemble the list of fixed DOFs fixedList, a matrix of size
  // 1-by-(number of fixities)
  int n_SubFixedNode = 0;
  int n_Fixities = 0;
  for (int i = 0; i < Fixities_.rows(); i++)
  {
    int v_id = Fixities_(i, 0);

    if (sub_nodes_set.end() != sub_nodes_set.find(v_id))
    {
      full_f.segment(node_dof_ * v_id, node_dof_) = (Fixities_.block(i, 1, 1, node_dof_)).transpose();

      n_Fixities += full_f.segment(node_dof_ * v_id, node_dof_).sum();
      n_SubFixedNode++;
    }
  }

  if (0 == n_Fixities)
  {
    if (verbose_){
      std::cout << "Not stable: At least one node needs to be fixed in the considered substructure!" << std::endl;
    }
    return false;
  }

  // count supp_dof and res_dof to init K_slice
  int n_Nexist = node_dof_ * (n_Node - int(sub_nodes_set.size()));
  int n_Free = dof - n_Fixities - n_Nexist;

  // generate permute id map
  int free_tail = 0;
  int fix_tail = n_Free;
  int nexist_tail = n_Free + n_Fixities;
  Eigen::VectorXi id_map_RO = Eigen::VectorXi::LinSpaced(dof, 0, dof - 1);
  for (int i = 0; i < dof; i++)
  {
    if (0 == full_f[i])
    {
      id_map_RO[free_tail] = i;
      free_tail++;
    }
    if (1 == full_f[i])
    {
      id_map_RO[fix_tail] = i;
      fix_tail++;
    }
    if (-1 == full_f[i])
    {
      id_map_RO[nexist_tail] = i;
      nexist_tail++;
    }
  }

  // a row permuatation matrix (multiply left)
  Eigen::SparseMatrix<double> Perm(dof, dof);
  std::vector<Eigen::Triplet<double>> perm_triplets;
  for (int i = 0; i < dof; i++)
  {
    perm_triplets.push_back(Eigen::Triplet<double>(i, id_map_RO[i], 1));
  }
  Perm.setFromTriplets(perm_triplets.begin(), perm_triplets.end());
  // perm operator on the right (column) = inverse of perm operation on the left (row)
  auto Perm_T = Perm.transpose();

  // permute the full stiffness matrix & carve the needed portion out
  createCompleteGlobalStiffnessMatrix(exist_element_ids);

  auto K_perm = Perm * K_assembled_full_ * Perm_T;
  auto K_mm = K_perm.block(0, 0, n_Free, n_Free);
  auto K_fm = K_perm.block(n_Free, 0, n_Fixities, n_Free);

  Eigen::VectorXd nodal_load_P_tmp = nodal_load_P_;

  Eigen::VectorXd element_lumped_load;
  createUniformlyDistributedLumpedLoad(exist_element_ids, element_lumped_load);
  nodal_load_P_tmp += element_lumped_load;

  if (include_self_weight_load_)
  {
    Eigen::VectorXd load_sw;
    createSelfWeightLumpedLoad(exist_element_ids, load_sw);
    nodal_load_P_tmp += load_sw;
  }

  Eigen::VectorXd Q_perm = Perm * nodal_load_P_tmp;
  auto Q_m = Q_perm.segment(0, n_Free);
  auto Q_f = Q_perm.segment(n_Free, n_Fixities);

  if (verbose_)
  {
    create_k_.Stop();
  }

  Eigen::VectorXd U_m(n_Free);
  // TODO: clamping by tolerance here?
  if (nodal_load_P_tmp.isZero())
  {
    U_m.setZero();
  }
  else
  {
    // TODO: what's the best solve strategy?
    // TODO: Conjugate gradient, precondition? https://www.cs.cmu.edu/~baraff/papers/sig98.pdf
    if (!stiff_solver_.solveSparseSimplicialLDLT(K_mm, Q_m, U_m, verbose_))
    {
      if (verbose_)
      {
        std::cout << "ERROR: Stiffness Solver fail! The sub-structure contains mechanism.\n" << std::endl;
      }
      return false;
    }
  }

  stored_compliance_ = 0.5 * U_m.dot(Q_m);

  // reaction force
  Eigen::VectorXd R(dof);
  R.setZero();

  auto R_f = K_fm * U_m - Q_f;
  R.segment(n_Free, n_Fixities) = R_f;
  R = Perm_T * R.eval();

  // displacement
  Eigen::VectorXd U(dof);
  U.setZero();
  U.segment(0, n_Free) = U_m;
  U = Perm_T * U.eval();

  // start raw computattion results conversion
  // element internal reaction
  element_reaction = Eigen::MatrixXd::Zero(exist_element_ids.size(), 1 + e_react_dof_id_.size());
  int cnt = 0;
  for (const int e_id : exist_element_ids)
  {
    Eigen::VectorXd Ue(2 * node_dof_);
    for (int k = 0; k < 2 * node_dof_; k++)
    {
      Ue[k] = U[id_map_(e_id, k)];
    }

    element_reaction(cnt, 0) = e_id;
    element_reaction.block(cnt, 1, 1, e_react_dof_id_.size()) =
      (rot_m_list_[e_id] * element_K_list_[e_id] * Ue).transpose();
    cnt++;
  }
  stored_element_reaction_ = element_reaction;

  // fixities reaction
  fixities_reaction = Eigen::MatrixXd::Zero(n_SubFixedNode, 1 + node_dof_);
  cnt = 0;
  for (int i = 0; i < Fixities_.rows(); i++)
  {
    int fix_node_id = Fixities_(i, 0);
    if (sub_nodes_set.end() != sub_nodes_set.find(fix_node_id))
    {
      fixities_reaction(cnt, 0) = fix_node_id;
      fixities_reaction.block(cnt, 1, 1, node_dof_) = (R.segment(fix_node_id * node_dof_, node_dof_)).transpose();
      cnt++;
    }
  }
  stored_fixities_reaction_ = fixities_reaction;

  // node displacement
  node_displ = Eigen::MatrixXd::Zero(sub_nodes_set.size(), 1 + node_dof_);
  cnt = 0;
  for (auto it = sub_nodes_set.begin(); it != sub_nodes_set.end(); ++it)
  {
    int v_id = *it;
    node_displ(cnt, 0) = v_id;
    node_displ.block(cnt, 1, 1, node_dof_) = (U.segment(v_id * node_dof_, node_dof_)).transpose();
    cnt++;
  }
  stored_nodal_deformation_ = node_displ;

  if (verbose_)
  {
    printOutTimer();
  }

  if (write_result_)
  {
    write_output_json(Vertices_, Elements_, node_displ, fixities_reaction, element_reaction,
      output_json_file_path_ + PathSeparator + output_json_file_name_);
  }

  has_stored_deformation_ = true;
  stored_existing_ids_ = exist_element_ids;

  // void *testWhetherMemoryLeakDetectionWorks = malloc(1);

  // stiffness criteria check
  return checkStiffnessCriteria(node_displ, fixities_reaction, element_reaction);
} // end core solve function

// overloaded solve
bool Stiffness::solve(const std::vector<int> &exist_element_ids,
                      const bool &cond_num)
{
  Eigen::MatrixXd U, R, F;
  return solve(exist_element_ids, U, R, F, cond_num);
}

// overloaded solve
bool Stiffness::solve(Eigen::MatrixXd& node_displ,
                      Eigen::MatrixXd& fixities_reaction,
                      Eigen::MatrixXd& element_reaction,
                      const bool& cond_num)
{
  int nElement = nE();
  std::vector<int> all_e_ids;
  for (int i = 0; i < nElement; i++)
  {
    all_e_ids.push_back(i);
  }
  return solve(
    all_e_ids, node_displ, fixities_reaction, element_reaction, cond_num);
}

// overloaded solve
bool Stiffness::solve(const bool &cond_num)
{
  Eigen::MatrixXd U, R, F;
  return solve(U, R, F, cond_num);
}

bool Stiffness::getElementStiffnessMatrices(std::vector<Eigen::MatrixXd> &element_stiffness_mats)
{
    if (!is_init_)
    {
      throw std::runtime_error("stiffness checker not inited yet.\n");
    }
    element_stiffness_mats = element_K_list_;
    return true;
}

bool Stiffness::getElementLocal2GlobalRotationMatrices(std::vector<Eigen::MatrixXd> &e_L2G_rot_mats)
{
  if (!is_init_)
  {
    throw std::runtime_error("stiffness checker not inited yet.\n");
  }
  e_L2G_rot_mats = rot_m_list_;
  return true;
}

bool Stiffness::getSelfWeightNodalLoad(const std::vector<int>& exist_e_ids, Eigen::VectorXd& self_weight_load)
{
  createSelfWeightLumpedLoad(exist_e_ids, self_weight_load);
  return true;
}

bool Stiffness::getUniformlyDistributedLumpedLoad(const std::vector<int>& exist_e_ids, Eigen::VectorXd& lumped_load)
{
  createUniformlyDistributedLumpedLoad(exist_e_ids, lumped_load);
  return true;
}

int Stiffness::nE() const
{
  return int(this->Elements_.rows());
}

int Stiffness::nV() const
{
  return int(this->Vertices_.rows());
}

int Stiffness::nFixV() const
{
  return int(this->Fixities_.rows());
}

int Stiffness::dim() const
{
  return this->dim_;
}

bool Stiffness::getSolvedResults(Eigen::MatrixXd &node_displ,
                      Eigen::MatrixXd &fixities_reaction,
                      Eigen::MatrixXd &element_reaction,
                      bool &pass_criteria)
{
  if(!hasStoredResults()) {
    throw std::runtime_error("no stored result found.\n");
  }
  node_displ = stored_nodal_deformation_;
  fixities_reaction = stored_fixities_reaction_;
  element_reaction = stored_element_reaction_;
  pass_criteria = checkStiffnessCriteria(node_displ, fixities_reaction, element_reaction);
  return true;
}

bool Stiffness::getSolvedCompliance(double &compliance)
{
  if (!hasStoredResults())
  {
    throw std::runtime_error("no stored result found.\n");
  }
  compliance = stored_compliance_;
  return true;
}

bool Stiffness::getMaxNodalDeformation(double &max_trans, double &max_rot,
    int &max_trans_vid, int &max_rot_vid)
{
  if (!hasStoredResults())
  {
    throw std::runtime_error("no stored result found.\n");
  }
  const int nNode = int(stored_nodal_deformation_.rows());
  int mt_i, mr_i, mr_j;

  Eigen::VectorXd trans_norm(nNode);
  trans_norm.setZero();
  for(int i = 0; i < nNode; i++)
  {
    for(int j = 1; j <= 3; j++)
    {
      trans_norm[i] += std::pow(stored_nodal_deformation_(i, j), 2);
    }
    trans_norm[i] = sqrt(trans_norm[i]);
  }
  max_trans = trans_norm.maxCoeff(&mt_i);
  max_trans_vid = int(stored_nodal_deformation_(mt_i, 0));
  // max_trans = stored_nodal_deformation_.block(0, 1, nNode, 3).cwiseAbs().maxCoeff(&mt_i, &mt_j);
  // max_trans_vid = stored_nodal_deformation_(mt_i, 0);

  max_rot = stored_nodal_deformation_.block(0, 4, nNode, 3).cwiseAbs().maxCoeff(&mr_i, &mr_j);
  max_rot_vid = int(stored_nodal_deformation_(mr_j, 0));
  return true;
}

bool Stiffness::checkStiffnessCriteria(const Eigen::MatrixXd &node_displ,
                                       const Eigen::MatrixXd &fixities_reaction,
                                       const Eigen::MatrixXd &element_reation)
{
  // stiffness check
  // nodal displacement check
  const int nNode = int(node_displ.rows());

  Eigen::VectorXd trans_norm(nNode);
  trans_norm.setZero();
  for(int i = 0; i < nNode; i++)
  {
    for(int j = 1; j <= 3; j++)
    {
      trans_norm[i] += std::pow(node_displ(i, j), 2);
    }
    trans_norm[i] = sqrt(trans_norm[i]);
  }
  double max_trans = trans_norm.maxCoeff();
  if (max_trans > transl_tol_)
  {
    return false;
  }

  // stability check
  // element reaction check
  // grounded element shouldn't be in tension
  // double sec_mod = M_PI * std::pow(material_parm_.radius_, 3) / 4;
  // double area = M_PI * std::pow(material_parm_.radius_, 2);
  //
  // for (int i = 0; i < fixities_reaction.rows(); i++)
  // {
  //   int v_id = (int) fixities_reaction(i, 0);
  //   const auto vertF = frame_.getVert(v_id);
  //   const auto& nghdE = vertF->getNghdElement();
  //
  //   double eF_lx;
  //   for(const auto& e : nghdE) {
  //     if(e->isFixed()) {
  //       // find element reaction
  //       int e_result_id = -1;
  //
  //       for(int j = 0; j < element_reation.size(); j++) {
  //         if (e->id() == element_reation(j, 0)) {
  //           e_result_id = j;
  //           break;
  //         }
  //       }
  //
  //       if(e->endVertU()->id() == v_id) {
  //         eF_lx = element_reation(e_result_id, 1);
  //         // only want tensile
  //         if(eF_lx <= 0) {
  //           eF_lx = 0;
  //         }
  //       } else {
  //         eF_lx = element_reation(e_result_id, 7);
  //         // only want tensile
  //         if(eF_lx >= 0) {
  //           eF_lx = 0;
  //         }
  //       } // end pts in local axis
  //     } // e is fixed
  //   }
  //   std::cout << "result eM: " << eF_lx << std::endl;
  //
  //   // moment reaction at fixities
  //   Eigen::VectorXd fixM = fixities_reaction.block<1,2>(i, 4);
  //
  //   // tension/compression bending stress
  //   Eigen::VectorXd sigma_b = fixM / sec_mod;
  //   std::cout << "bending stess: " << sigma_b << std::endl;
  //
  //   // axial tensile stress
  //   Eigen::VectorXd sigma_a = (eF_lx / area) * Eigen::VectorXd::Ones(2);
  //   std::cout << "axial stess: " << sigma_a << std::endl;
  //
  //   // total tensile stress = bending tensile stress + axial tensile stress
  //   Eigen::VectorXd total_tensile = sigma_b + sigma_a;
  //   std::cout << "total stess: " << total_tensile << "\n/" << 0.1 * material_parm_.tensile_yeild_stress_ << std::endl;
  //
  //   // if  > 0.1 (tbd) * yield stress
  //   if(std::abs(total_tensile.maxCoeff()) > 0.1 * material_parm_.tensile_yeild_stress_) {
  //     return false;
  //   }
  // } // fixities_reaction

 // for (int i = 0; i < element_reation.rows(); i++)
 // {
 //   int e_id = (int) element_reation(i, 0);
 //   const auto e = frame_.getElement(e_id);
 //   if (e->endVertU()->isFixed() || e->endVertV()->isFixed())
 //   {
 //     // check tension
 //     if (element_reation(i, 1) < 0)
 //     {
 //       assert(element_reation(i, 7) > 0 && "element axial reaction sign not consistent!");
 //
 //       if (verbose_)
 //       {
 //         std::cout << "grounded element #" << e_id
 //                   << " is in tension, the structure is not stable." << std::endl;
 //       }
 //
 //       return false;
 //     }
 //   }
 // }

  return true;
}

void Stiffness::printOutTimer()
{
  if (verbose_)
  {
    printf("***Stiffness timer result:\n");
    stiff_solver_.solve_timer_.Print("SolveK:");
    create_k_.Print("CreateGlobalK:");
    // check_ill_.Print("CheckIllCond:");
  }
}

} // namespace stiffness_checker
} // namespace conmech


#ifndef COMMON_SEGMENTS_HPP
#define COMMON_SEGMENTS_HPP

#include <iostream>
#include <vector>
#include <memory>
#include <Eigen/Dense>

/*

セグメントクラスの設計

このクラスは自立分散システム？（ADS）における自立個を表現する

まず、自立個として必要なことは、
・自分の状態を保持する自己状態ベクトルr
を保持し、
・自立分散システム全体の状態を表現するシステム状態ベクトルR
・系全体の状態Rから得られるベクトル？z
を受け取ることで、次の自己状態ベクトルをえることである。

 ながれとしては

 各セグメントの
 システムの記述、パラメータの設定
 ・パラメータは同じシステムでも異なってくるのでシステム定義とは異なり、複数存在しなければならない
 ・複数のセグメントの状態、パラメータをそれぞれのセグメントにどのように渡すのか？

 まず、

 セグメントクラス
 含有として
 ・状態
 ・

 */

namespace ADS {

    // ADS内のセグメント挙動を記述するインターフェイスクラス？ これを継承して具体的なセグメントのシステムを記述する
    // 各個セグメントにおけるパラメータの設定が問題になる

    /*
     * バランス
     * 負荷配分
     *
     */

    //状態量ツリー
    template<typename T>
    class TreeState {
    public:

        TreeState();
        TreeState(const std::vector<std::vector<T> >&);
        TreeState(const TreeState&);

        //vector<vector <T>> の本体を返す
        std::vector<std::vector<T> >& GetTree() { return R; };
        //本体を更新
        //void SetTree(std::vector<std::vector<T> >& in_) {R = in_;};

        //RVOSによる最適化が働くのでそのまま帰すことにする
        //MOVEセマンティクスは考えない
        const TreeState<T> operator+(const TreeState<T>&) const;

        const TreeState<T> operator-(const TreeState<T>&) const;

        const TreeState<T> operator*(double) const;
        //const TreeState<T> operator/(double, const TreeState<T> &);

    private:

        //コア
        //状態量ツリー
        std::vector<std::vector<T> > R;

    };

    /*
     * セグメントクラスがやること
     *
     * １セグメントのシステムを書く
     * システムは
     * r_dot = f(t, P, z, R, idx)
     * (t:時間、P:系のパラメータ、z:系全体から定まるベクトル、R:系全体を示す状態ベクトル、idx:セグメントのINDEX)
     * として書かれる
     *
     * また、系全体から定まるベクトルzを計算する関数を書く
     * これは
     * z = g(t, P, R)
     * (t:時間、P:系のパラメータ、R:系全体を示す状態ベクトル)
     * として書かれる
     *
     * Rについて これはAdsManager内で管理される
     * Rは系全体の状態を示す、実質行列となる
     * セグメントiにおける状態を示すベクトルをr_iとすれば、Rは
     * R＝{r_1 r_2 ... r_n} (n個のセグメント)
     * となる
     * これはvectorで示すことにする
     *
     * Pについて これはAdsManager内で管理される
     * Pはこのセグメント群（おなじシステムで記述されるセグメントの集まり）すべてのパラメータを示す、多くの実装では行列になると思われる
     * セグメントiにおけるパラメータを示すベクトルをp_iとすれば、Pは
     * P={p_1 p_2 ... p_n} (n個のセグメント)
     * となる
     * なのでPはvectorで示すことになる
     *
     * */
    template<typename T>
    class Segment {

    public:

        Segment();

        Segment(unsigned int no_);

        //セグメントのシステム
//        * １セグメントのシステムを書く
//                * システムは
//        * r_dot = f(t, P, z, R, idx)
//                  * (t:時間、P:系のパラメータ、z:系全体から定まるベクトル、R:系全体を示す状態ベクトル、idx:セグメントのINDEX)
//        * として書かれる
        virtual T f(double t_
                , const std::vector<std::vector<std::vector<double> > >& P_
                , const std::vector<double>& z_
                , const std::vector<std::vector<T> >& R_
                , unsigned long idx_) = 0;

        virtual T f(double t_
                , const std::vector<std::vector<std::vector<double> > >& P_
                , const std::vector<double>& z_
                , const TreeState& R_
                , unsigned long idx_) = 0;

        // 入力として、系の状態ベクトルを受け取り、系全体から定まるベクトルを返す
//        * z = g(t, P, R)
//              * (t:時間、P:系のパラメータ、R:系全体を示す状態ベクトル)
//        * として書かれる
        virtual std::vector<double> g(double t_, const std::vector<std::vector<std::vector<double> > >& P_,
                const std::vector<std::vector<T> >& R_) = 0;

        virtual std::vector<double> g(double t_
                , const std::vector<std::vector<std::vector<double> > >& P_
                , const TreeState& R_) = 0;

        //このシステムで記述されるセグメント数の設定
        void SetNo(const unsigned int no_) { no_segments = no_; };

        //セグメント数の取得
        const unsigned int GetNo() { return no_segments; };

        // 組み込みセグメントIDの設定
        void SetID(const unsigned int id_) { id = id_; };

        //セグメントIDの取得
        const unsigned int GetID() { return id; };

        //セグメントの有効化、無効化
        void Enable() { is_enable = true; };

        void Disable() { is_enable = false; };

    private:

        unsigned int no_segments; //セグメント数

        unsigned int id;        // 組み込みセグメントID

        bool is_enable;         //有効化

    };

    //ここでは各々のセグメントの時間発展とそれらの組み込み、初期状態設定を行う
    /*
     * AdsManagerがホストする状態量としては
     *
     * 経過時間:t
     * 系の状態ベクトル:R
     * 系のパラメータ:P
     *
     * セグメントの実装から決まったこと
     * 系の状態ベクトルRはツリー構造をもつ
     * なので実装はR:vector<vector<T>>となる
     *
     * また、それぞれのセグメントにおけるパラメータ設定も、ツリー構造とした
     * なのでP:vector<vector<vector<double>>>となる
     *TODO: パラメータベクトルは使いやすそうなクラスかなんかにする
     *
     *
     * */

    template<typename T>
    class AdsManager {
    public:

        AdsManager();

        //セグメントの登録を行う、セグメントIDを返す
        /*
         * セグメントの登録を行う
         * 処理の流れとしては
         *
         * セグメントストレージにプッシュバック
         * 個数分の状態量、パラメータベクトルの確保を行う
         *
         * セグメントインデックスを返す
         *
         * */
        unsigned long AddSegment(std::shared_ptr<Segment<T> >);

        //セグメント数の設定も行う
        unsigned long AddSegment(std::shared_ptr<Segment<T> >, unsigned int no_segments_);

        //パラメータ設定、初期値設定インターフェース

        //各セグメントの状態を更新する
        /*
         * 全体の積分計算を行う
         * */
        void Update();

    private:

        //セグメント
        std::vector<std::shared_ptr<Segment<T> > > segments;
        //状態量ツリー
        //std::vector<std::vector<T> > R;
        TreeState R;
        //パラメータツリー
        std::vector<std::vector<std::vector<double> > > P;

        //経過時間
        double t;

        //タイムピッチ
        double dt;

    };

};

#endif	// COMMON_SEGMENTS_HPP


//---------------------------------------------------------------------------//
//!
//! \file   Utility_BasicBivariateDistribution.hpp
//! \author Alex Robinson
//! \brief  The basic bivariate distribution class declaration
//!
//---------------------------------------------------------------------------//

#ifndef UTILITY_BASIC_BIVARIATE_DISTRIBUTION_HPP
#define UTILITY_BASIC_BIVARIATE_DISTRIBUTION_HPP

// Std Lib Includes
#include <stdexcept>

// Boost Includes
#include <boost/serialization/split_member.hpp>

// FRENSIE Includes
#include "Utility_OStreamableObject.hpp"
#include "Utility_UnitTraits.hpp"
#include "Utility_QuantityTraits.hpp"
#include "Utility_DistributionTraits.hpp"
#include "Utility_DistributionSerializationHelpers.hpp"

/*! \defgroup bivariate_distributions Bivariate Distributions
 */

namespace Utility{

/*! The unit-aware basic bivariate distribution
 * \ingroup bivariate_distributions
 */
template<typename PrimaryIndependentUnit,
         typename SecondaryIndependentUnit,
         typename DependentUnit>
class UnitAwareBasicBivariateDistribution : public OStreamableObject
{

  // Typedef for this type
  typedef UnitAwareBasicBivariateDistribution<PrimaryIndependentUnit,SecondaryIndependentUnit,DependentUnit> ThisType;

protected:

  //! The primary independent unit traits typedef
  typedef UnitTraits<PrimaryIndependentUnit> PrimaryIndepUnitTraits;

  //! The secondary independent unit traits typedef
  typedef UnitTraits<SecondaryIndependentUnit> SecondaryIndepUnitTraits;

  //! The inverse primary independent unit traits typedef
  typedef UnitTraits<typename UnitTraits<PrimaryIndependentUnit>::InverseUnit> InversePrimaryIndepUnitTraits;

  //! The inverse secondary independent unit traits typedef
  typedef UnitTraits<typename UnitTraits<SecondaryIndependentUnit>::InverseUnit> InverseSecondaryIndepUnitTraits;

  //! The inverse independent unit traits typedef
  typedef UnitTraits<typename UnitTraits<typename UnitTraits<PrimaryIndependentUnit>::InverseUnit>::template GetMultipliedUnitType<typename UnitTraits<SecondaryIndependentUnit>::InverseUnit>::type> InverseIndepUnitTraits;

  //! The dependent unit traits typedef
  typedef UnitTraits<DependentUnit> DepUnitTraits;

public:

  //! The primary independent unit type
  typedef PrimaryIndependentUnit PrimaryIndepUnit;

  //! The secondary independent unit type
  typedef SecondaryIndependentUnit SecondaryIndepUnit;

  //! The dependent unit type
  typedef DependentUnit DepUnit;

  //! The primary independent quantity type
  typedef typename PrimaryIndepUnitTraits::template GetQuantityType<double>::type PrimaryIndepQuantity;

  //! The secondary independent quantity type
  typedef typename SecondaryIndepUnitTraits::template GetQuantityType<double>::type SecondaryIndepQuantity;

  //! The inverse secondary independent quantity type
  typedef typename InverseSecondaryIndepUnitTraits::template GetQuantityType<double>::type InverseSecondaryIndepQuantity;

  //! The dependent quantity type
  typedef typename DepUnitTraits::template GetQuantityType<double>::type DepQuantity;

  //! Constructor
  UnitAwareBasicBivariateDistribution()
  { /* ... */ }

  //! Destructor
  virtual ~UnitAwareBasicBivariateDistribution()
  { /* ... */ }

  //! Evaluate the distribution
  virtual DepQuantity evaluate(
            const PrimaryIndepQuantity primary_indep_var_value,
            const SecondaryIndepQuantity secondary_indep_var_value ) const = 0;

  //! Evaluate the secondary conditional PDF
  virtual InverseSecondaryIndepQuantity evaluateSecondaryConditionalPDF(
            const PrimaryIndepQuantity primary_indep_var_value,
            const SecondaryIndepQuantity secondary_indep_var_value ) const = 0;

  //! Return a random sample from the secondary conditional PDF
  virtual SecondaryIndepQuantity sampleSecondaryConditional(
                const PrimaryIndepQuantity primary_indep_var_value ) const = 0;

  //! Return a random sample and record the number of trials
  virtual SecondaryIndepQuantity sampleSecondaryConditionalAndRecordTrials(
                            const PrimaryIndepQuantity primary_indep_var_value,
                            DistributionTraits::Counter& trials ) const = 0;

  //! Return the upper bound of the distribution primary independent variable
  virtual PrimaryIndepQuantity getUpperBoundOfPrimaryIndepVar() const = 0;

  //! Return the lower bound of the distribution primary independent variable
  virtual PrimaryIndepQuantity getLowerBoundOfPrimaryIndepVar() const = 0;

  //! Return the upper bound of the secondary conditional distribution
  virtual SecondaryIndepQuantity getUpperBoundOfSecondaryConditionalIndepVar(
                const PrimaryIndepQuantity primary_indep_var_value ) const = 0;

  //! Return the lower bound of the secondary conditional distribution
  virtual SecondaryIndepQuantity getLowerBoundOfSecondaryConditionalIndepVar(
                const PrimaryIndepQuantity primary_indep_var_value ) const = 0;

  //! Test if the distribution is tabular in the primary dimension
  virtual bool isPrimaryDimensionTabular() const = 0;

  //! Test if the distribution is continuous in the primary dimension
  virtual bool isPrimaryDimensionContinuous() const = 0;

  //! Test if the distribution has the same primary bounds
  bool hasSamePrimaryBounds( const UnitAwareBasicBivariateDistribution& distribution ) const;

protected:

  //! Add distribution data to the stream
  template<typename... Types>
  void toStreamDistImpl( std::ostream& os,
                         const std::string& name,
                         const Types&... data ) const;

private:

  // Save the distribution to an archive
  template<typename Archive>
  void save( Archive& ar, const unsigned version ) const
  { /* ... */ }

  // Load the distribution from an archive
  template<typename Archive>
  void load( Archive& ar, const unsigned version )
  { /* ... */ }

  BOOST_SERIALIZATION_SPLIT_MEMBER();

  // Declare the boost serialization access object as a friend
  friend class boost::serialization::access;
};

/*! The basic bivariate distribution (unit-agnostic)
 * \ingroup bivariate_distributions
 */
typedef UnitAwareBasicBivariateDistribution<void,void,void> BasicBivariateDistribution;

/*! \brief Exception thrown by BivariateDistribution objects when an invalid
 * parameter is encountered.
 * \ingroup bivariate_distributions
 */
class BadBivariateDistributionParameter : public std::logic_error
{
public:
  BadBivariateDistributionParameter( const std::string& msg )
    : std::logic_error( msg )
  { /* ... */ }

  ~BadBivariateDistributionParameter() throw()
  { /* ... */ }
};

} // end Utility namespace

BOOST_SERIALIZATION_ASSUME_ABSTRACT_DISTRIBUTION3( UnitAwareBasicBivariateDistribution );
BOOST_SERIALIZATION_DISTRIBUTION3_VERSION( UnitAwareBasicBivariateDistribution, 0 );

//---------------------------------------------------------------------------//
// Template Includes
//---------------------------------------------------------------------------//

#include "Utility_BasicBivariateDistribution_def.hpp"

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

#endif // end UTILITY_BASIC_BIVARIATE_DISTRIBUTION_HPP

//---------------------------------------------------------------------------//
// end Utility_BivariateDistribution.hpp
//---------------------------------------------------------------------------//


#define OFDIS_INTERNAL
#include <Eigen/Core>
#include <Eigen/LU>
#include <Eigen/Dense>
#include "litiv/3rdparty/ofdis/fdf/image.h"
#include "litiv/3rdparty/ofdis/patch.hpp"
#include "litiv/3rdparty/ofdis/patchgrid.hpp"
#include "litiv/utils/defines.hpp" // only used here for compiler flags

template<ofdis::FlowInputType eInput, ofdis::FlowOutputType eOutput>
ofdis::PatGridClass<eInput,eOutput>::PatGridClass(
        const camparam* cpt_in,
        const camparam* cpo_in,
        const optparam* op_in) :
        cpt(cpt_in),
        cpo(cpo_in),
        op(op_in) {
    // Generate grid on current scale
    steps = op->steps;
    nopw = ceil((float)cpt->width/(float)steps);
    noph = ceil((float)cpt->height/(float)steps);
    const int offsetw = floor((cpt->width-(nopw-1)*steps)/2);
    const int offseth = floor((cpt->height-(noph-1)*steps)/2);
    nopatches = nopw*noph;
    pt_ref.resize(nopatches);
    p_init.resize(nopatches);
    pat.reserve(nopatches);
    im_ao_eg = new Eigen::Map<const Eigen::MatrixXf>(nullptr,cpt->height,cpt->width);
    im_ao_dx_eg = new Eigen::Map<const Eigen::MatrixXf>(nullptr,cpt->height,cpt->width);
    im_ao_dy_eg = new Eigen::Map<const Eigen::MatrixXf>(nullptr,cpt->height,cpt->width);
    im_bo_eg = new Eigen::Map<const Eigen::MatrixXf>(nullptr,cpt->height,cpt->width);
    im_bo_dx_eg = new Eigen::Map<const Eigen::MatrixXf>(nullptr,cpt->height,cpt->width);
    im_bo_dy_eg = new Eigen::Map<const Eigen::MatrixXf>(nullptr,cpt->height,cpt->width);
    int patchid = 0;
    for(int x=0; x<nopw; ++x) {
        for(int y=0; y<noph; ++y) {
            int i = x*noph+y;
            pt_ref[i][0] = x*steps+offsetw;
            pt_ref[i][1] = y*steps+offseth;
            p_init[i].setZero();
            pat.push_back(new PatClass<eInput,eOutput>(cpt,cpo,op,patchid));
            patchid++;
        }
    }
}

template<ofdis::FlowInputType eInput, ofdis::FlowOutputType eOutput>
ofdis::PatGridClass<eInput,eOutput>::~PatGridClass() {
    delete im_ao_eg;
    delete im_ao_dx_eg;
    delete im_ao_dy_eg;
    delete im_bo_eg;
    delete im_bo_dx_eg;
    delete im_bo_dy_eg;
    for(int i=0; i< nopatches; ++i)
        delete pat[i];
}

template<ofdis::FlowInputType eInput, ofdis::FlowOutputType eOutput>
void ofdis::PatGridClass<eInput,eOutput>::SetComplGrid(PatGridClass *cg_in) {
    cg = cg_in;
}

template<ofdis::FlowInputType eInput, ofdis::FlowOutputType eOutput>
void ofdis::PatGridClass<eInput,eOutput>::InitializeGrid(const float* im_ao_in, const float* im_ao_dx_in, const float* im_ao_dy_in) {
    im_ao = im_ao_in;
    im_ao_dx = im_ao_dx_in;
    im_ao_dy = im_ao_dy_in;
    new(im_ao_eg) Eigen::Map<const Eigen::MatrixXf>(im_ao,cpt->height,cpt->width); // new placement operator
    new(im_ao_dx_eg) Eigen::Map<const Eigen::MatrixXf>(im_ao_dx,cpt->height,cpt->width);
    new(im_ao_dy_eg) Eigen::Map<const Eigen::MatrixXf>(im_ao_dy,cpt->height,cpt->width);
#if USING_OPENMP
    #pragma omp parallel for schedule(static)
#endif //USING_OPENMP
    for (int i = 0; i < nopatches; ++i) {
        pat[i]->InitializePatch(im_ao_eg, im_ao_dx_eg, im_ao_dy_eg, pt_ref[i]);
        p_init[i].setZero();
    }
}

template<ofdis::FlowInputType eInput, ofdis::FlowOutputType eOutput>
void ofdis::PatGridClass<eInput,eOutput>::SetTargetImage(const float* im_bo_in, const float* im_bo_dx_in, const float* im_bo_dy_in) {
    im_bo = im_bo_in;
    im_bo_dx = im_bo_dx_in;
    im_bo_dy = im_bo_dy_in;
    new(im_bo_eg) Eigen::Map<const Eigen::MatrixXf>(im_bo,cpt->height,cpt->width); // new placement operator
    new(im_bo_dx_eg) Eigen::Map<const Eigen::MatrixXf>(im_bo_dx,cpt->height,cpt->width); // new placement operator
    new(im_bo_dy_eg) Eigen::Map<const Eigen::MatrixXf>(im_bo_dy,cpt->height,cpt->width); // new placement operator
#if USING_OPENMP
    #pragma omp parallel for schedule(static)
#endif //USING_OPENMP
    for(int i = 0; i < nopatches; ++i)
        pat[i]->SetTargetImage(im_bo_eg, im_bo_dx_eg, im_bo_dy_eg);
}

template<ofdis::FlowInputType eInput, ofdis::FlowOutputType eOutput>
void ofdis::PatGridClass<eInput,eOutput>::Optimize() {
#if USING_OPENMP
    #pragma omp parallel for schedule(dynamic,10)
#endif //USING_OPENMP
    for(int i = 0; i < nopatches; ++i)
        pat[i]->OptimizeIter(p_init[i], true); // optimize until convergence
}

template<ofdis::FlowInputType eInput, ofdis::FlowOutputType eOutput>
void ofdis::PatGridClass<eInput,eOutput>::InitializeFromCoarserOF(const float* flow_prev) {
#if USING_OPENMP
    #pragma omp parallel for schedule(dynamic,10)
#endif //USING_OPENMP
    for (int ip = 0; ip < nopatches; ++ip) {
        int x = floor(pt_ref[ip][0] / 2); // better, but slower: use bil. interpolation here
        int y = floor(pt_ref[ip][1] / 2);
        int i = y*(cpt->width/2) + x;
        if(eOutput==ofdis::FlowOutput_OpticalFlow) {
            p_init[ip](0) = flow_prev[2*i]*2;
            p_init[ip](1) = flow_prev[2*i+1]*2;
        }
        else
            p_init[ip](0) = flow_prev[i]*2;
    }
}

template<ofdis::FlowInputType eInput, ofdis::FlowOutputType eOutput>
void ofdis::PatGridClass<eInput,eOutput>::AggregateFlowDense(float *flowout) const {
    float* we = new float[cpt->width * cpt->height];
    memset(flowout, 0, sizeof(float) * (op->nop * cpt->width * cpt->height) );
    memset(we,      0, sizeof(float) * (          cpt->width * cpt->height) );
#ifdef USE_PARALLEL_ON_FLOWAGGR // Using this enables OpenMP on flow aggregation. This can lead to race conditions. Experimentally we found that the result degrades only marginally. However, for our experiments we did not enable this.
    #pragma omp parallel for schedule(static)
#endif
    for(int ip = 0; ip < nopatches; ++ip) {
        if(pat[ip]->IsValid()) {
            const typename p_init_type::value_type* fl = pat[ip]->GetParam(); // flow/horiz displacement of this patch
            typename p_init_type::value_type flnew;
            const float* pweight = pat[ip]->GetpWeightPtr(); // use image error as weight
            int lb = -op->p_samp_s/2;
            int ub = op->p_samp_s/2-1;
            for(int y = lb; y <= ub; ++y) {
                for(int x = lb; x <= ub; ++x, ++pweight) {
                    int yt = (y + pt_ref[ip][1]);
                    int xt = (x + pt_ref[ip][0]);
                    if (xt >= 0 && yt >= 0 && xt < cpt->width && yt < cpt->height) {
                        int i = yt*cpt->width + xt;
                        float absw;
                        if(eInput==ofdis::FlowInput_RGB) {
                            absw = (float)(std::max(op->minerrval,*pweight)); ++pweight;
                            absw+= (float)(std::max(op->minerrval,*pweight)); ++pweight;
                            absw+= (float)(std::max(op->minerrval,*pweight));
                            absw = 1.0f/absw;
                        }
                        else
                            absw = 1.0f/(float)(std::max(op->minerrval,*pweight));
                        flnew = (*fl) * absw;
                        we[i] += absw;
                        if(eOutput==ofdis::FlowOutput_OpticalFlow) {
                            flowout[2*i]   += flnew[0];
                            flowout[2*i+1] += flnew[1];
                        }
                        else
                            flowout[i] += flnew[0];
                    }
                }
            }
        }
    }

    // if complementary (forward-backward merging) is given, integrate negative backward flow as well
    if(cg) {
        Eigen::Vector4f wbil; // bilinear weight vector
        Eigen::Vector4i pos;
#if USING_OPENMP
    #ifdef USE_PARALLEL_ON_FLOWAGGR
        #pragma omp parallel for schedule(static)
    #endif
#endif //USING_OPENMP
        for(int ip = 0; ip < cg->nopatches; ++ip) {
            if (cg->pat[ip]->IsValid()) {
                const typename p_init_type::value_type* fl = (cg->pat[ip]->GetParam()); // flow/horiz displacement of this patch
                typename p_init_type::value_type flnew;
                const Eigen::Vector2f rppos = cg->pat[ip]->GetPointPos(); // get patch position after optimization
                const float* pweight = cg->pat[ip]->GetpWeightPtr(); // use image error as weight
                Eigen::Vector2f resid;
                // compute bilinear weight vector
                pos[0] = ceil(rppos[0] +.00001); // make sure they are rounded up to natural number
                pos[1] = ceil(rppos[1] +.00001); // make sure they are rounded up to natural number
                pos[2] = floor(rppos[0]);
                pos[3] = floor(rppos[1]);
                resid[0] = rppos[0] - pos[2];
                resid[1] = rppos[1] - pos[3];
                wbil[0] = resid[0]*resid[1];
                wbil[1] = (1-resid[0])*resid[1];
                wbil[2] = resid[0]*(1-resid[1]);
                wbil[3] = (1-resid[0])*(1-resid[1]);
                int lb = -op->p_samp_s/2;
                int ub = op->p_samp_s/2-1;
                for(int y = lb; y <= ub; ++y) {
                    for(int x = lb; x <= ub; ++x, ++pweight) {
                        int yt = y + pos[1];
                        int xt = x + pos[0];
                        if(xt >= 1 && yt >= 1 && xt < (cpt->width-1) && yt < (cpt->height-1)) {
                            float absw;
                            if(eInput==ofdis::FlowInput_RGB) {
                                absw = (float)(std::max(op->minerrval,*pweight)); ++pweight;
                                absw+= (float)(std::max(op->minerrval,*pweight)); ++pweight;
                                absw+= (float)(std::max(op->minerrval,*pweight));
                                absw = 1.0f/absw;
                            }
                            else
                                absw = 1.0f/(float)(std::max(op->minerrval,*pweight));
                            flnew = (*fl) * absw;
                            int idxcc =  xt    +  yt   *cpt->width;
                            int idxfc = (xt-1) +  yt   *cpt->width;
                            int idxcf =  xt    + (yt-1)*cpt->width;
                            int idxff = (xt-1) + (yt-1)*cpt->width;
                            we[idxcc] += wbil[0] * absw;
                            we[idxfc] += wbil[1] * absw;
                            we[idxcf] += wbil[2] * absw;
                            we[idxff] += wbil[3] * absw;
                            if(eOutput==ofdis::FlowOutput_OpticalFlow) {
                                flowout[2*idxcc  ] -= wbil[0] * flnew[0];   // use reversed flow
                                flowout[2*idxcc+1] -= wbil[0] * flnew[1];
                                flowout[2*idxfc  ] -= wbil[1] * flnew[0];
                                flowout[2*idxfc+1] -= wbil[1] * flnew[1];
                                flowout[2*idxcf  ] -= wbil[2] * flnew[0];
                                flowout[2*idxcf+1] -= wbil[2] * flnew[1];
                                flowout[2*idxff  ] -= wbil[3] * flnew[0];
                                flowout[2*idxff+1] -= wbil[3] * flnew[1];
                            }
                            else {
                                flowout[idxcc] -= wbil[0] * flnew[0]; // simple averaging of inverse horizontal displacement
                                flowout[idxfc] -= wbil[1] * flnew[0];
                                flowout[idxcf] -= wbil[2] * flnew[0];
                                flowout[idxff] -= wbil[3] * flnew[0];
                            }
                        }
                    }
                }
            }
        }
    }
#if USING_OPENMP
    #pragma omp parallel for schedule(static, 100)
#endif //USING_OPENMP
    // normalize each pixel by dividing displacement by aggregated weights from all patches
    for (int yi = 0; yi < cpt->height; ++yi) {
        for (int xi = 0; xi < cpt->width; ++xi) {
            int i = yi*cpt->width + xi;
            if (we[i]>0) {
                if(eOutput==ofdis::FlowOutput_OpticalFlow) {
                    flowout[2*i  ] /= we[i];
                    flowout[2*i+1] /= we[i];
                }
                else
                    flowout[i] /= we[i];
            }
        }
    }
    delete[] we;
}

template class ofdis::PatGridClass<ofdis::FlowInput_Grayscale,ofdis::FlowOutput_OpticalFlow>;
template class ofdis::PatGridClass<ofdis::FlowInput_Gradient,ofdis::FlowOutput_OpticalFlow>;
template class ofdis::PatGridClass<ofdis::FlowInput_RGB,ofdis::FlowOutput_OpticalFlow>;
template class ofdis::PatGridClass<ofdis::FlowInput_Grayscale,ofdis::FlowOutput_StereoDepth>;
template class ofdis::PatGridClass<ofdis::FlowInput_Gradient,ofdis::FlowOutput_StereoDepth>;
template class ofdis::PatGridClass<ofdis::FlowInput_RGB,ofdis::FlowOutput_StereoDepth>;


//| Copyright Inria May 2015
//| This project has received funding from the European Research Council (ERC) under
//| the European Union's Horizon 2020 research and innovation programme (grant
//| agreement No 637972) - see http://www.resibots.eu
//|
//| Contributor(s):
//|   - Jean-Baptiste Mouret (jean-baptiste.mouret@inria.fr)
//|   - Antoine Cully (antoinecully@gmail.com)
//|   - Konstantinos Chatzilygeroudis (konstantinos.chatzilygeroudis@inria.fr)
//|   - Federico Allocati (fede.allocati@gmail.com)
//|   - Vaios Papaspyros (b.papaspyros@gmail.com)
//|   - Roberto Rama (bertoski@gmail.com)
//|
//| This software is a computer library whose purpose is to optimize continuous,
//| black-box functions. It mainly implements Gaussian processes and Bayesian
//| optimization.
//| Main repository: http://github.com/resibots/limbo
//| Documentation: http://www.resibots.eu/limbo
//|
//| This software is governed by the CeCILL-C license under French law and
//| abiding by the rules of distribution of free software.  You can  use,
//| modify and/ or redistribute the software under the terms of the CeCILL-C
//| license as circulated by CEA, CNRS and INRIA at the following URL
//| "http://www.cecill.info".
//|
//| As a counterpart to the access to the source code and  rights to copy,
//| modify and redistribute granted by the license, users are provided only
//| with a limited warranty  and the software's author,  the holder of the
//| economic rights,  and the successive licensors  have only  limited
//| liability.
//|
//| In this respect, the user's attention is drawn to the risks associated
//| with loading,  using,  modifying and/or developing or reproducing the
//| software by the user in light of its specific status of free software,
//| that may mean  that it is complicated to manipulate,  and  that  also
//| therefore means  that it is reserved for developers  and  experienced
//| professionals having in-depth computer knowledge. Users are therefore
//| encouraged to load and test the software's suitability as regards their
//| requirements in conditions enabling the security of their systems and/or
//| data to be ensured and,  more generally, to use and operate it in the
//| same conditions as regards security.
//|
//| The fact that you are presently reading this means that you have had
//| knowledge of the CeCILL-C license and that you accept its terms.
//|
#ifndef LIMBO_INIT_GRID_SAMPLING_HPP
#define LIMBO_INIT_GRID_SAMPLING_HPP

#include <Eigen/Core>

#include <limbo/tools/macros.hpp>

namespace limbo {
    namespace defaults {
        struct init_gridsampling {
            ///@ingroup init_defaults
            BO_PARAM(int, bins, 5);
        };
    }
    namespace init {
        /** @ingroup init
          \rst
          Grid sampling.

          Parameter:
            - ``int bins`` (number of bins)
          \endrst
        */
        template <typename Params>
        struct GridSampling {
            template <typename StateFunction, typename AggregatorFunction, typename Opt>
            void operator()(const StateFunction& seval, const AggregatorFunction&, Opt& opt) const
            {
                _explore(0, seval, Eigen::VectorXd::Constant(StateFunction::dim_in(), 0), opt);
            }

        private:
            // recursively explore all the dimensions
            template <typename StateFunction, typename Opt>
            void _explore(int dim_in, const StateFunction& seval, const Eigen::VectorXd& current,
                Opt& opt) const
            {
                for (double x = 0; x <= 1.0f; x += 1.0f / (double)Params::init_gridsampling::bins()) {
                    Eigen::VectorXd point = current;
                    point[dim_in] = x;
                    if (dim_in == current.size() - 1) {
                        opt.eval_and_add(seval, point);
                    }
                    else {
                        _explore(dim_in + 1, seval, point, opt);
                    }
                }
            }
        };
    }
}
#endif


//---------------------------------------------------------------------------//
// Copyright (c) 2020 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2020 Nikita Kaskov <nbering@nil.foundation>
//
// MIT License
//
// 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.
//---------------------------------------------------------------------------//

#ifndef CRYPTO3_ALGEBRA_FFT_MAKE_EVALUATION_DOMAIN_HPP
#define CRYPTO3_ALGEBRA_FFT_MAKE_EVALUATION_DOMAIN_HPP

#include <vector>

#include <boost/math/tools/polynomial.hpp>

#include <nil/crypto3/fft/domains/evaluation_domain.hpp>
#include <nil/crypto3/fft/domains/arithmetic_sequence_domain.hpp>
#include <nil/crypto3/fft/domains/basic_radix2_domain.hpp>
#include <nil/crypto3/fft/domains/extended_radix2_domain.hpp>
#include <nil/crypto3/fft/domains/geometric_sequence_domain.hpp>
#include <nil/crypto3/fft/domains/step_radix2_domain.hpp>

#include <nil/crypto3/fft/detail/field_utils.hpp>

namespace nil {
    namespace crypto3 {
        namespace fft {

            /*!
            @brief
             A convenience method for choosing an evaluation domain
             Returns an evaluation domain object in which the domain S has size
             |S| >= MinSize.
             The function get_evaluation_domain is chosen from different supported domains,
             depending on MinSize.
            */

            namespace detail {
                using namespace nil::crypto3::algebra;

                template<typename FieldType>
                bool is_basic_radix2_domain(std::size_t m) {
                    const std::size_t log_m = static_cast<std::size_t>(std::ceil(std::log2(m)));

                    return (m > 1) && (log_m <= fields::arithmetic_params<FieldType>::s) && (m == (1ul << log_m));
                }

                template<typename FieldType>
                bool is_extended_radix2_domain(std::size_t m) {
                    const std::size_t log_m = static_cast<std::size_t>(std::ceil(std::log2(m)));
                    const std::size_t small_m = m / 2;
                    const std::size_t log_small_m = static_cast<std::size_t>(std::ceil(std::log2(small_m)));

                    return (m > 1) && (log_m == fields::arithmetic_params<FieldType>::s + 1) &&
                           (small_m == (1ul << log_small_m)) &&
                           (log_small_m <= fields::arithmetic_params<FieldType>::s);
                }

                template<typename FieldType>
                bool is_step_radix2_domain(std::size_t m) {
                    const std::size_t log_m = static_cast<std::size_t>(std::ceil(std::log2(m)));
                    const std::size_t shift_log_m = (1ul << log_m);
                    const std::size_t log_shift_log_m = static_cast<std::size_t>(std::ceil(std::log2(shift_log_m)));
                    const std::size_t small_m = m - (1ul << (static_cast<std::size_t>(std::ceil(std::log2(m))) - 1));
                    const std::size_t log_small_m = static_cast<std::size_t>(std::ceil(std::log2(small_m)));

                    return (m > 1) && (small_m == (1ul << log_small_m)) && (shift_log_m == (1ul << log_shift_log_m)) &&
                           (log_shift_log_m <= fields::arithmetic_params<FieldType>::s);
                }

                template<typename FieldType>
                bool is_geometric_sequence_domain(std::size_t m) {
                    return (m > 1) &&
                           (typename FieldType::value_type(fields::arithmetic_params<FieldType>::geometric_generator) !=
                            FieldType::value_type::zero());
                }

                template<typename FieldType>
                bool is_arithmetic_sequence_domain(std::size_t m) {
                    return (m > 1) && (typename FieldType::value_type(
                                           fields::arithmetic_params<FieldType>::arithmetic_generator) !=
                                       FieldType::value_type::zero());
                }

            }    // namespace detail

            template<typename FieldType>
            std::shared_ptr<evaluation_domain<FieldType>> make_evaluation_domain(std::size_t m) {
                typedef std::shared_ptr<evaluation_domain<FieldType>> ret_type;

                const std::size_t big = 1ul << (std::size_t(std::ceil(std::log2(m))) - 1);
                const std::size_t rounded_small = (1ul << std::size_t(std::ceil(std::log2(m - big))));

                if (detail::is_basic_radix2_domain<FieldType>(m)) {
                    ret_type result;
                    result.reset(new basic_radix2_domain<FieldType>(m));
                    return result;
                }

                if (detail::is_extended_radix2_domain<FieldType>(m)) {
                    ret_type result;
                    result.reset(new extended_radix2_domain<FieldType>(m));
                    return result;
                }

                if (detail::is_step_radix2_domain<FieldType>(m)) {
                    ret_type result;
                    result.reset(new step_radix2_domain<FieldType>(m));
                    return result;
                }

                if (detail::is_basic_radix2_domain<FieldType>(big + rounded_small)) {
                    ret_type result;
                    result.reset(new basic_radix2_domain<FieldType>(big + rounded_small));
                    return result;
                }

                if (detail::is_extended_radix2_domain<FieldType>(big + rounded_small)) {
                    ret_type result;
                    result.reset(new extended_radix2_domain<FieldType>(big + rounded_small));
                    return result;
                }

                if (detail::is_step_radix2_domain<FieldType>(big + rounded_small)) {
                    ret_type result;
                    result.reset(new step_radix2_domain<FieldType>(big + rounded_small));
                    return result;
                }

                if (detail::is_geometric_sequence_domain<FieldType>(m)) {
                    ret_type result;
                    result.reset(new geometric_sequence_domain<FieldType>(m));
                    return result;
                }

                if (detail::is_arithmetic_sequence_domain<FieldType>(m)) {
                    ret_type result;
                    result.reset(new arithmetic_sequence_domain<FieldType>(m));
                    return result;
                }

                return ret_type();
            }
        }    // namespace fft
    }        // namespace crypto3
}    // namespace nil

#endif    // CRYPTO3_ALGEBRA_FFT_MAKE_EVALUATION_DOMAIN_HPP


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2003 Neil Firth
 Copyright (C) 2003 Ferdinando Ametrano
 Copyright (C) 2007 StatPro Italia srl

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#include <ql/instruments/barrieroption.hpp>
#include <ql/instruments/impliedvolatility.hpp>
#include <ql/pricingengines/barrier/analyticbarrierengine.hpp>
#include <ql/exercise.hpp>
#include <boost/scoped_ptr.hpp>

namespace QuantLib {

    BarrierOption::BarrierOption(
        Barrier::Type barrierType,
        Real barrier,
        Real rebate,
        const ext::shared_ptr<StrikedTypePayoff>& payoff,
        const ext::shared_ptr<Exercise>& exercise)
    : OneAssetOption(payoff, exercise),
      barrierType_(barrierType), barrier_(barrier), rebate_(rebate) {}

    void BarrierOption::setupArguments(PricingEngine::arguments* args) const {

        OneAssetOption::setupArguments(args);

        auto* moreArgs = dynamic_cast<BarrierOption::arguments*>(args);
        QL_REQUIRE(moreArgs != nullptr, "wrong argument type");
        moreArgs->barrierType = barrierType_;
        moreArgs->barrier = barrier_;
        moreArgs->rebate = rebate_;
    }

    Volatility BarrierOption::impliedVolatility(
             Real targetValue,
             const ext::shared_ptr<GeneralizedBlackScholesProcess>& process,
             Real accuracy,
             Size maxEvaluations,
             Volatility minVol,
             Volatility maxVol) const {

        QL_REQUIRE(!isExpired(), "option expired");

        ext::shared_ptr<SimpleQuote> volQuote(new SimpleQuote);

        ext::shared_ptr<GeneralizedBlackScholesProcess> newProcess =
            detail::ImpliedVolatilityHelper::clone(process, volQuote);

        // engines are built-in for the time being
        boost::scoped_ptr<PricingEngine> engine;
        switch (exercise_->type()) {
          case Exercise::European:
            engine.reset(new AnalyticBarrierEngine(newProcess));
            break;
          case Exercise::American:
          case Exercise::Bermudan:
            QL_FAIL("engine not available for non-European barrier option");
            break;
          default:
            QL_FAIL("unknown exercise type");
        }

        return detail::ImpliedVolatilityHelper::calculate(*this,
                                                          *engine,
                                                          *volQuote,
                                                          targetValue,
                                                          accuracy,
                                                          maxEvaluations,
                                                          minVol, maxVol);
    }

    BarrierOption::arguments::arguments()
    : barrierType(Barrier::Type(-1)), barrier(Null<Real>()),
      rebate(Null<Real>()) {}

    void BarrierOption::arguments::validate() const {
        OneAssetOption::arguments::validate();

        switch (barrierType) {
          case Barrier::DownIn:
          case Barrier::UpIn:
          case Barrier::DownOut:
          case Barrier::UpOut:
            break;
          default:
            QL_FAIL("unknown type");
        }

        QL_REQUIRE(barrier != Null<Real>(), "no barrier given");
        QL_REQUIRE(rebate != Null<Real>(), "no rebate given");
    }

    bool BarrierOption::engine::triggered(Real underlying) const {
        switch (arguments_.barrierType) {
          case Barrier::DownIn:
          case Barrier::DownOut:
            return underlying < arguments_.barrier;
          case Barrier::UpIn:
          case Barrier::UpOut:
            return underlying > arguments_.barrier;
          default:
            QL_FAIL("unknown type");
        }
    }

}