#ifndef DART_NEURAL_SNAPSHOT_HPP_
#define DART_NEURAL_SNAPSHOT_HPP_

#include <unordered_map>
#include <vector>

#include <Eigen/Dense>

#include "dart/neural/DifferentiableContactConstraint.hpp"
#include "dart/neural/NeuralConstants.hpp"
#include "dart/neural/NeuralUtils.hpp"
#include "dart/neural/WithRespectTo.hpp"
#include "dart/performance/PerformanceLog.hpp"
#include "dart/simulation/World.hpp"

namespace dart {

using namespace performance;

namespace neural {

class BackpropSnapshot
{
  friend class MappedBackpropSnapshot;

public:
  /// This saves a snapshot from a forward pass, with all the info we need in
  /// order to efficiently compute a backwards pass. Crucially, the positions
  /// must all be snapshots from before the timestep, yet this constructor must
  /// be called after the timestep.
  BackpropSnapshot(
      simulation::WorldPtr world,
      Eigen::VectorXs preStepPosition,
      Eigen::VectorXs preStepVelocity,
      Eigen::VectorXs preStepTorques,
      Eigen::VectorXs preConstraintVelocities,
      Eigen::VectorXs preStepLCPCache);

  /// This computes the implicit backprop without forming intermediate
  /// Jacobians. It takes a LossGradient with the position and velocity vectors
  /// filled it, though the loss with respect to torque is ignored and can be
  /// null. It returns a LossGradient with all three values filled in, position,
  /// velocity, and torque.
  void backprop(
      simulation::WorldPtr world,
      LossGradient& thisTimestepLoss,
      const LossGradient& nextTimestepLoss,
      PerformanceLog* perfLog = nullptr,
      bool exploreAlternateStrategies = false);

  /// This computes backprop in the high-level RL API's space, use `state` and
  /// `action` as the primitives we're taking gradients wrt to.
  LossGradientHighLevelAPI backpropState(
      simulation::WorldPtr world,
      const Eigen::VectorXs& nextTimestepStateLossGrad,
      PerformanceLog* perfLog = nullptr,
      bool exploreAlternateStrategies = false);

  /// This zeros out any components of the gradient that would want to push us
  /// out of the box-bounds encoded in the world for pos, vel, or force.
  void clipLossGradientsToBounds(
      simulation::WorldPtr world,
      Eigen::VectorXs& lossWrtPos,
      Eigen::VectorXs& lossWrtVel,
      Eigen::VectorXs& lossWrtForce);

  /// This computes and returns the whole vel-vel jacobian. For backprop, you
  /// don't actually need this matrix, you can compute backprop directly. This
  /// is here if you want access to the full Jacobian for some reason.
  const Eigen::MatrixXs& getVelVelJacobian(
      simulation::WorldPtr world, PerformanceLog* perfLog = nullptr);

  /// This computes and returns the whole pos-vel jacobian. For backprop, you
  /// don't actually need this matrix, you can compute backprop directly. This
  /// is here if you want access to the full Jacobian for some reason.
  const Eigen::MatrixXs& getPosVelJacobian(
      simulation::WorldPtr world, PerformanceLog* perfLog = nullptr);

  /// This computes and returns the whole force-vel jacobian. For backprop, you
  /// don't actually need this matrix, you can compute backprop directly. This
  /// is here if you want access to the full Jacobian for some reason.
  const Eigen::MatrixXs& getControlForceVelJacobian(
      simulation::WorldPtr world, PerformanceLog* perfLog = nullptr);

  /// This computes and returns the whole mass-vel jacobian. For backprop, you
  /// don't actually need this matrix, you can compute backprop directly. This
  /// is here if you want access to the full Jacobian for some reason.
  const Eigen::MatrixXs& getMassVelJacobian(
      simulation::WorldPtr world, PerformanceLog* perfLog = nullptr);

  /// This computes and returns the whole pos-pos jacobian. For backprop, you
  /// don't actually need this matrix, you can compute backprop directly. This
  /// is here if you want access to the full Jacobian for some reason.
  const Eigen::MatrixXs& getPosPosJacobian(
      simulation::WorldPtr world, PerformanceLog* perfLog = nullptr);

  /// This computes and returns the whole vel-pos jacobian. For backprop, you
  /// don't actually need this matrix, you can compute backprop directly. This
  /// is here if you want access to the full Jacobian for some reason.
  const Eigen::MatrixXs& getVelPosJacobian(
      simulation::WorldPtr world, PerformanceLog* perfLog = nullptr);

  /// This computes and returns the component of the pos-pos and pos-vel
  /// jacobians due to bounce approximation. For backprop, you don't actually
  /// need this matrix, you can compute backprop directly. This is here if you
  /// want access to the full Jacobian for some reason.
  const Eigen::MatrixXs& getBounceApproximationJacobian(
      simulation::WorldPtr world, PerformanceLog* perfLog = nullptr);

  /// This returns the Jacobian for state_t -> state_{t+1}.
  Eigen::MatrixXs getStateJacobian(simulation::WorldPtr world);

  /// This returns the Jacobian for action_t -> state_{t+1}.
  Eigen::MatrixXs getActionJacobian(simulation::WorldPtr world);

  /// Returns a concatenated vector of all the Skeletons' position()'s in the
  /// World, in order in which the Skeletons appear in the World's
  /// getSkeleton(i) returns them, BEFORE the timestep.
  Eigen::VectorXs getPreStepPosition();

  /// Returns a concatenated vector of all the Skeletons' velocity()'s in the
  /// World, in order in which the Skeletons appear in the World's
  /// getSkeleton(i) returns them, BEFORE the timestep.
  Eigen::VectorXs getPreStepVelocity();

  /// Returns a concatenated vector of all the joint torques that were applied
  /// during the forward pass, BEFORE the timestep.
  Eigen::VectorXs getPreStepTorques();

  /// Returns a concatenated vector of all the Skeletons' velocity()'s in the
  /// World, in order in which the Skeletons appear in the World's
  /// getSkeleton(i) returns them, AFTER integrating forward dynamics but BEFORE
  /// running the LCP.
  Eigen::VectorXs getPreConstraintVelocity();

  /// Returns a concatenated vector of all the Skeletons' position()'s in the
  /// World, in order in which the Skeletons appear in the World's
  /// getSkeleton(i) returns them, AFTER the timestep.
  Eigen::VectorXs getPostStepPosition();

  /// Returns a concatenated vector of all the Skeletons' velocity()'s in the
  /// World, in order in which the Skeletons appear in the World's
  /// getSkeleton(i) returns them, AFTER the timestep.
  Eigen::VectorXs getPostStepVelocity();

  /// Returns a concatenated vector of all the joint torques that were applied
  /// during the forward pass, AFTER the timestep.
  Eigen::VectorXs getPostStepTorques();

  /// Returns the LCP's cached solution from before the step
  const Eigen::VectorXs& getPreStepLCPCache();

  /////////////////////////////////////////////////////////////////////////////
  /// Just public for testing
  /////////////////////////////////////////////////////////////////////////////

  /// This returns the A_c matrix. You shouldn't ever need this matrix, it's
  /// just here to enable testing.
  Eigen::MatrixXs getClampingConstraintMatrix(simulation::WorldPtr world);

  /// This returns the V_c matrix. You shouldn't ever need this matrix, it's
  /// just here to enable testing.
  Eigen::MatrixXs getMassedClampingConstraintMatrix(simulation::WorldPtr world);

  /// This returns the A_ub matrix. You shouldn't ever need this matrix, it's
  /// just here to enable testing.
  Eigen::MatrixXs getUpperBoundConstraintMatrix(simulation::WorldPtr world);

  /// This returns the V_c matrix. You shouldn't ever need this matrix, it's
  /// just here to enable testing.
  Eigen::MatrixXs getMassedUpperBoundConstraintMatrix(
      simulation::WorldPtr world);

  /// This returns the E matrix. You shouldn't ever need this matrix, it's
  /// just here to enable testing.
  Eigen::MatrixXs getUpperBoundMappingMatrix();

  /// This returns the B matrix. You shouldn't ever need this matrix, it's
  /// just here to enable testing.
  Eigen::MatrixXs getBouncingConstraintMatrix(simulation::WorldPtr world);

  /// This returns the mass matrix for the whole world, a block diagonal
  /// concatenation of the skeleton mass matrices.
  Eigen::MatrixXs getMassMatrix(
      simulation::WorldPtr world, bool forFiniteDifferencing = false);

  /// This returns the inverse mass matrix for the whole world, a block diagonal
  /// concatenation of the skeleton inverse mass matrices.
  Eigen::MatrixXs getInvMassMatrix(
      simulation::WorldPtr world, bool forFiniteDifferencing = false);

  /// This is the subset of the A matrix from the original LCP that corresponds
  /// to clamping indices.
  Eigen::MatrixXs getClampingAMatrix();

  /// This returns the pos-C(pos,vel) Jacobian for the whole world, a block
  /// diagonal concatenation of the skeleton pos-C(pos,vel) Jacobians.
  Eigen::MatrixXs getPosCJacobian(simulation::WorldPtr world);

  /// This returns the vel-C(pos,vel) Jacobian for the whole world, a block
  /// diagonal concatenation of the skeleton vel-C(pos,vel) Jacobians.
  Eigen::MatrixXs getVelCJacobian(simulation::WorldPtr world);

  /// This computes and returns the whole vel-vel jacobian by finite
  /// differences. This is SUPER SLOW, and is only here for testing.
  Eigen::MatrixXs finiteDifferenceVelVelJacobian(
      simulation::WorldPtr world, bool useRidders = true);

  /// This computes and returns the whole vel-vel jacobian by Ridders
  /// extrapolated finite differences. This is SUPER DUPER SLOW,
  /// and is only here for testing.
  Eigen::MatrixXs finiteDifferenceRiddersVelVelJacobian(
      simulation::WorldPtr world);

  /// This computes and returns the whole pos-C(pos,vel) jacobian by finite
  /// differences. This is SUPER SUPER SLOW, and is only here for testing.
  Eigen::MatrixXs finiteDifferencePosVelJacobian(
      simulation::WorldPtr world, bool useRidders = true);

  /// This computes and returns the whole pos-C(pos,vel) jacobian by Ridders
  /// extrapolated finite differences. This is SUPER DUPER SLOW,
  /// and is only here for testing.
  Eigen::MatrixXs finiteDifferenceRiddersPosVelJacobian(
      simulation::WorldPtr world);

  /// This computes and returns the whole force-vel jacobian by finite
  /// differences. This is SUPER SLOW, and is only here for testing.
  Eigen::MatrixXs finiteDifferenceForceVelJacobian(
      simulation::WorldPtr world, bool useRidders = true);

  /// This computes and returns the whole force-vel jacobian by Ridders
  /// extrapolated finite differences. This is SUPER DUPER SLOW,
  /// and is only here for testing.
  Eigen::MatrixXs finiteDifferenceRiddersForceVelJacobian(
      simulation::WorldPtr world);

  /// This computes and returns the whole mass-vel jacobian by finite
  /// differences. This is SUPER SLOW, and is only here for testing.
  Eigen::MatrixXs finiteDifferenceMassVelJacobian(
      simulation::WorldPtr world, bool useRidders = true);

  /// This computes and returns the whole mass-vel jacobian by Ridders
  /// extrapolated finite differences. This is SUPER DUPER SLOW,
  /// and is only here for testing.
  Eigen::MatrixXs finiteDifferenceRiddersMassVelJacobian(
      simulation::WorldPtr world);

  /// This computes and returns the whole pos-pos jacobian by finite
  /// differences. This is SUPER SUPER SLOW, and is only here for testing.
  Eigen::MatrixXs finiteDifferencePosPosJacobian(
      simulation::WorldPtr world,
      std::size_t subdivisions = 20,
      bool useRidders = true);

  /// This computes and returns the whole pos-pos jacobian by Ridders
  /// extrapolated finite differences. This is SUPER DUPER SUPER SLOW,
  /// and is only here for testing.
  Eigen::MatrixXs finiteDifferenceRiddersPosPosJacobian(
      simulation::WorldPtr world, std::size_t subdivisions = 20);

  /// This computes and returns the whole vel-pos jacobian by finite
  /// differences. This is SUPER SUPER SLOW, and is only here for testing.
  Eigen::MatrixXs finiteDifferenceVelPosJacobian(
      simulation::WorldPtr world,
      std::size_t subdivisions = 20,
      bool useRidders = true);

  /// This computes and returns the whole vel-pos jacobian by Ridders
  /// extrapolated finite differences. This is SUPER DUPER SUPER SLOW,
  /// and is only here for testing.
  Eigen::MatrixXs finiteDifferenceRiddersVelPosJacobian(
      simulation::WorldPtr world, std::size_t subdivisions = 20);

  /// This computes and returns the whole wrt-vel jacobian by finite
  /// differences. This is SUPER SUPER SLOW, and is only here for testing.
  Eigen::MatrixXs finiteDifferenceVelJacobianWrt(
      simulation::WorldPtr world, WithRespectTo* wrt, bool useRidders = true);

  /// This computes and returns the whole wrt-vel jacobian by Ridders
  /// extrapolated finite differences. This is SUPER SUPER SLOW,
  /// and is only here for testing.
  Eigen::MatrixXs finiteDifferenceRiddersVelJacobianWrt(
      simulation::WorldPtr world, WithRespectTo* wrt);

  /// This computes and returns the whole wrt-pos jacobian by finite
  /// differences. This is SUPER SUPER SLOW, and is only here for testing.
  Eigen::MatrixXs finiteDifferencePosJacobianWrt(
      simulation::WorldPtr world, WithRespectTo* wrt, bool useRidders = true);

  /// This computes and returns the whole wrt-pos jacobian by Ridders
  /// extrapolated finite differences. This is SUPER SUPER SLOW,
  /// and is only here for testing.
  Eigen::MatrixXs finiteDifferenceRiddersPosJacobianWrt(
      simulation::WorldPtr world, WithRespectTo* wrt);

  /// This returns the P_c matrix. You shouldn't ever need this matrix, it's
  /// just here to enable testing.
  Eigen::MatrixXs getProjectionIntoClampsMatrix(
      simulation::WorldPtr world, bool forFiniteDifferencing = false);

  /// This replaces x with the result of M*x in place, without explicitly
  /// forming M
  Eigen::VectorXs implicitMultiplyByMassMatrix(
      simulation::WorldPtr world, const Eigen::VectorXs& x);

  /// This replaces x with the result of Minv*x in place, without explicitly
  /// forming Minv
  Eigen::VectorXs implicitMultiplyByInvMassMatrix(
      simulation::WorldPtr world, const Eigen::VectorXs& x);

  /// TODO(keenon): Remove me
  Eigen::MatrixXs getScratchAnalytical(
      simulation::WorldPtr world, WithRespectTo* wrt);

  /// TODO(keenon): Remove me
  Eigen::MatrixXs getScratchFiniteDifference(
      simulation::WorldPtr world, WithRespectTo* wrt, bool useRidders = true);

  /// TODO(keenon): Remove me
  Eigen::MatrixXs getScratchFiniteDifferenceRidders(
      simulation::WorldPtr world, WithRespectTo* wrt);

  /// This predicts what the next velocity will be using our linear algebra
  /// formula. This is only here for testing, to compare it against the actual
  /// result of a timestep.
  ///
  /// The `morePreciseButSlower` flag tells this function to do brute force
  /// steps to get constraint matrices A_c and A_ub, rather than use 1st order
  /// approximations. This is important because when we're doing
  /// finite-differencing over tiny EPS (1e-9) then tiny errors in the 1st order
  /// approximations blow up to become huge errors in gradients.
  Eigen::VectorXs getAnalyticalNextV(
      simulation::WorldPtr world, bool morePreciseButSlower = false);

  /// This computes and returns the whole wrt-vel jacobian. For backprop, you
  /// don't actually need this matrix, you can compute backprop directly. This
  /// is here if you want access to the full Jacobian for some reason.
  Eigen::MatrixXs getVelJacobianWrt(
      simulation::WorldPtr world, WithRespectTo* wrt);

  /// This computes and returns the whole wrt-pos jacobian. For backprop, you
  /// don't actually need this matrix, you can compute backprop directly. This
  /// is here if you want access to the full Jacobian for some reason.
  Eigen::MatrixXs getPosJacobianWrt(
      simulation::WorldPtr world, WithRespectTo* wrt);

  /// This returns the jacobian of constraint force, holding everyhing constant
  /// except the value of WithRespectTo
  Eigen::MatrixXs getJacobianOfConstraintForce(
      simulation::WorldPtr world, WithRespectTo* wrt);

  /// This returns the analytical expression for the Jacobian of Q*b, holding b
  /// constant, if there are some upper-bound indices
  Eigen::MatrixXs dQ_WithUB(
      simulation::WorldPtr world,
      Eigen::MatrixXs& Minv,
      Eigen::MatrixXs& A_c,
      Eigen::MatrixXs& E,
      Eigen::MatrixXs& A_c_ub_E,
      Eigen::VectorXs rhs,
      WithRespectTo* wrt);
  /// This returns the analytical expression for the Jacobian of Q^T*b, holding
  /// b constant, if there are some upper-bound indices
  Eigen::MatrixXs dQT_WithUB(
      simulation::WorldPtr world,
      Eigen::MatrixXs& Minv,
      Eigen::MatrixXs& A_c,
      Eigen::MatrixXs& E,
      Eigen::MatrixXs& A_ub,
      Eigen::VectorXs rhs,
      WithRespectTo* wrt);
  /// This returns the analytical expression for the Jacobian of Q*b, holding b
  /// constant, if there are no upper-bound indices
  Eigen::MatrixXs dQ_WithoutUB(
      simulation::WorldPtr world,
      Eigen::MatrixXs& Minv,
      Eigen::MatrixXs& A_c,
      Eigen::VectorXs rhs,
      WithRespectTo* wrt);

  /// This returns the jacobian of Qb, holding b constant, with respect to
  /// wrt, by finite differencing
  Eigen::MatrixXs finiteDifferenceJacobianOfQb(
      simulation::WorldPtr world,
      Eigen::VectorXs b,
      WithRespectTo* wrt,
      bool useRidders = true);

  /// This returns the jacobian of Qb, holding b constant, with respect to
  /// wrt, by finite differencing
  Eigen::MatrixXs finiteDifferenceRiddersJacobianOfQb(
      simulation::WorldPtr world, Eigen::VectorXs b, WithRespectTo* wrt);

  /// This returns the vector of constants that get added to the diagonal of Q
  /// to guarantee that Q is full-rank
  Eigen::VectorXs getConstraintForceMixingDiagonal();

  /// This returns the jacobian of Q^{-1}b, holding b constant, with respect to
  /// wrt
  Eigen::MatrixXs getJacobianOfLCPConstraintMatrixClampingSubset(
      simulation::WorldPtr world, Eigen::VectorXs b, WithRespectTo* wrt);

  /// This returns the jacobian of Q^{-1}b, holding b constant, with respect to
  /// wrt, by finite differencing
  Eigen::MatrixXs finiteDifferenceJacobianOfLCPConstraintMatrixClampingSubset(
      simulation::WorldPtr world,
      Eigen::VectorXs b,
      WithRespectTo* wrt,
      bool useRidders = true);

  /// This returns the jacobian of Q^{-1}b, holding b constant, with respect to
  /// wrt, by Ridders extrapolated finite differencing
  Eigen::MatrixXs
  finiteDifferenceRiddersJacobianOfLCPConstraintMatrixClampingSubset(
      simulation::WorldPtr world, Eigen::VectorXs b, WithRespectTo* wrt);

  /// This returns the jacobian of b (from Q^{-1}b) with respect to wrt
  Eigen::MatrixXs getJacobianOfLCPOffsetClampingSubset(
      simulation::WorldPtr world, WithRespectTo* wrt);

  /// This returns the jacobian of b (from Q^{-1}b) with respect to wrt, by
  /// finite differencing
  Eigen::MatrixXs finiteDifferenceJacobianOfLCPOffsetClampingSubset(
      simulation::WorldPtr world, WithRespectTo* wrt, bool useRidders = true);

  /// This returns the jacobian of b (from Q^{-1}b) with respect to wrt, by
  /// Ridders extrapolated finite differencing
  Eigen::MatrixXs finiteDifferenceRiddersJacobianOfLCPOffsetClampingSubset(
      simulation::WorldPtr world, WithRespectTo* wrt);

  /// This returns the jacobian of b (from Q^{-1}b) with respect to wrt, by
  /// finite differencing
  Eigen::MatrixXs finiteDifferenceJacobianOfLCPEstimatedOffsetClampingSubset(
      simulation::WorldPtr world, WithRespectTo* wrt, bool useRidders = true);

  /// This returns the jacobian of b (from Q^{-1}b) with respect to wrt, by
  /// Ridders extrapolated finite differencing
  Eigen::MatrixXs
  finiteDifferenceRiddersJacobianOfLCPEstimatedOffsetClampingSubset(
      simulation::WorldPtr world, WithRespectTo* wrt);

  /// This returns the subset of the A matrix used by the original LCP for just
  /// the clamping constraints. It relates constraint force to constraint
  /// acceleration. It's a mass matrix, just in a weird frame.
  void computeLCPConstraintMatrixClampingSubset(
      simulation::WorldPtr world,
      Eigen::MatrixXs& Q,
      const Eigen::MatrixXs& A_c,
      const Eigen::MatrixXs& A_ub,
      const Eigen::MatrixXs& E);

  /// This returns the subset of the b vector used by the original LCP for just
  /// the clamping constraints. It's just the relative velocity at the clamping
  /// contact points.
  void computeLCPOffsetClampingSubset(
      simulation::WorldPtr world,
      Eigen::VectorXs& b,
      const Eigen::MatrixXs& A_c);

  /// This computes and returns an estimate of the constraint impulses for the
  /// clamping constraints. This is based on a linear approximation of the
  /// constraint impulses.
  Eigen::VectorXs estimateClampingConstraintImpulses(
      simulation::WorldPtr world,
      const Eigen::MatrixXs& A_c,
      const Eigen::MatrixXs& A_ub,
      const Eigen::MatrixXs& E);

  /// This returns the jacobian of P_c * v, holding everyhing constant except
  /// the value of WithRespectTo
  Eigen::MatrixXs getJacobianOfProjectionIntoClampsMatrix(
      simulation::WorldPtr world, Eigen::VectorXs v, WithRespectTo* wrt);

  /// This computes and returns the jacobian of P_c * v by finite
  /// differences. This is SUPER SLOW, and is only here for testing.
  Eigen::MatrixXs finiteDifferenceJacobianOfProjectionIntoClampsMatrix(
      simulation::WorldPtr world,
      Eigen::VectorXs v,
      WithRespectTo* wrt,
      bool useRidders = true);

  /// This computes and returns the jacobian of P_c * v by Ridders extrapolated
  /// finite differences. This is SUPER SLOW, and is only here for testing.
  Eigen::MatrixXs finiteDifferenceRiddersJacobianOfProjectionIntoClampsMatrix(
      simulation::WorldPtr world, Eigen::VectorXs v, WithRespectTo* wrt);

  /// This returns the jacobian of M^{-1}(pos, inertia) * tau, holding
  /// everything constant except the value of WithRespectTo
  Eigen::MatrixXs getJacobianOfMinv(
      simulation::WorldPtr world, Eigen::VectorXs tau, WithRespectTo* wrt);

  /// This computes and returns the jacobian of M^{-1}(pos, inertia) * tau by
  /// finite differences. This is SUPER SLOW, and is only here for testing.
  Eigen::MatrixXs finiteDifferenceJacobianOfMinv(
      simulation::WorldPtr world,
      Eigen::VectorXs tau,
      WithRespectTo* wrt,
      bool useRidders = true);

  /// This computes and returns the jacobian of M^{-1}(pos, inertia) * tau by
  /// Ridders extrapolated finite differences. This is SUPER SLOW, and is
  /// only here for testing.
  Eigen::MatrixXs finiteDifferenceRiddersJacobianOfMinv(
      simulation::WorldPtr world, Eigen::VectorXs tau, WithRespectTo* wrt);

  /// This returns the jacobian of M(pos, inertia) * v, holding
  /// everything constant except the value of WithRespectTo
  Eigen::MatrixXs getJacobianOfM(
      simulation::WorldPtr world, Eigen::VectorXs v, WithRespectTo* wrt);

  /// This computes and returns the jacobian of M(pos, inertia) * v by
  /// finite differences. This is SUPER SLOW, and is only here for testing.
  Eigen::MatrixXs finiteDifferenceJacobianOfM(
      simulation::WorldPtr world,
      Eigen::VectorXs v,
      WithRespectTo* wrt,
      bool useRidders = true);

  /// This computes and returns the jacobian of M(pos, inertia) * v by
  /// Ridders extrapolated finite differences. This is SUPER SLOW, and is
  /// only here for testing.
  Eigen::MatrixXs finiteDifferenceRiddersJacobianOfM(
      simulation::WorldPtr world, Eigen::VectorXs v, WithRespectTo* wrt);

  /// This returns the jacobian of C(pos, inertia, vel), holding everything
  /// constant except the value of WithRespectTo
  Eigen::MatrixXs getJacobianOfC(
      simulation::WorldPtr world, WithRespectTo* wrt);

  /// This returns the jacobian of C(pos, inertia, vel), holding everything
  /// constant except the value of WithRespectTo
  Eigen::MatrixXs computeJacobianOfC(
      simulation::WorldPtr world, WithRespectTo* wrt);

  /// This computes and returns the jacobian of C(pos, inertia, vel) by finite
  /// differences. This is SUPER SLOW, and is only here for testing.
  Eigen::MatrixXs finiteDifferenceJacobianOfC(
      simulation::WorldPtr world, WithRespectTo* wrt, bool useRidders = true);

  /// This computes and returns the jacobian of C(pos, inertia, vel) by Ridders
  /// extrapolated finite differences. This is SUPER SLOW, and is only here
  /// for testing.
  Eigen::MatrixXs finiteDifferenceRiddersJacobianOfC(
      simulation::WorldPtr world, WithRespectTo* wrt);

  /// This returns the jacobian of M^{-1}(pos, inertia) * (C(pos, inertia, vel)
  /// + mPreStepTorques), holding everything constant except the value of
  /// WithRespectTo
  Eigen::MatrixXs getJacobianOfMinvC(
      simulation::WorldPtr world, WithRespectTo* wrt);

  /// This computes and returns the jacobian of M^{-1}(pos, inertia) * C(pos,
  /// inertia, vel) by finite differences. This is SUPER SLOW, and is only here
  /// for testing.
  Eigen::MatrixXs finiteDifferenceJacobianOfMinvC(
      simulation::WorldPtr world, WithRespectTo* wrt, bool useRidders = false);

  /// This computes and returns the jacobian of M^{-1}(pos, inertia) * C(pos,
  /// inertia, vel) by Ridders extrapolated finite differences. This is SUPER
  /// SLOW, and is only here for testing.
  Eigen::MatrixXs finiteDifferenceRiddersJacobianOfMinvC(
      simulation::WorldPtr world, WithRespectTo* wrt);

  /// This returns a fast approximation to A_c in the neighborhood of the
  /// original
  Eigen::MatrixXs estimateClampingConstraintMatrixAt(
      simulation::WorldPtr world, Eigen::VectorXs pos);

  /// This returns a fast approximation to A_ub in the neighborhood of the
  /// original
  Eigen::MatrixXs estimateUpperBoundConstraintMatrixAt(
      simulation::WorldPtr world, Eigen::VectorXs pos);

  /// Only for testing: VERY SLOW. This returns the actual value of A_c at the
  /// desired position.
  Eigen::MatrixXs getClampingConstraintMatrixAt(
      simulation::WorldPtr world, Eigen::VectorXs pos);

  /// Only for testing: VERY SLOW. This returns the actual value of A_ub at the
  /// desired position.
  Eigen::MatrixXs getUpperBoundConstraintMatrixAt(
      simulation::WorldPtr world, Eigen::VectorXs pos);

  /// Only for testing: VERY SLOW. This returns the actual value of E at the
  /// desired position.
  Eigen::MatrixXs getUpperBoundMappingMatrixAt(
      simulation::WorldPtr world, Eigen::VectorXs pos);

  /// Only for testing: VERY SLOW. This returns the actual value of the bounce
  /// diagonals at the desired position.
  Eigen::VectorXs getBounceDiagonalsAt(
      simulation::WorldPtr world, Eigen::VectorXs pos);

  /// This computes the Jacobian of A_c*f0 with respect to position using
  /// impulse tests.
  Eigen::MatrixXs getJacobianOfClampingConstraints(
      simulation::WorldPtr world, Eigen::VectorXs f0);

  /// This computes the finite difference Jacobian of A_c*f0 with respect to
  /// position. This is AS SLOW AS FINITE DIFFERENCING THE WHOLE ENGINE, which
  /// is way too slow to use in practice.
  Eigen::MatrixXs finiteDifferenceJacobianOfClampingConstraints(
      simulation::WorldPtr world, Eigen::VectorXs f0, bool useRidders = true);

  /// This computes the finite difference Jacobian of A_c*f0 with respect to
  /// position. This is AS SLOW AS FINITE DIFFERENCING THE WHOLE ENGINE, which
  /// is way too slow to use in practice. Using Ridders, so even slower.
  Eigen::MatrixXs finiteDifferenceRiddersJacobianOfClampingConstraints(
      simulation::WorldPtr world, Eigen::VectorXs f0);

  /// This computes the Jacobian of A_c^T*v0 with respect to position using
  /// impulse tests.
  Eigen::MatrixXs getJacobianOfClampingConstraintsTranspose(
      simulation::WorldPtr world, Eigen::VectorXs v0);

  /// This computes the finite difference Jacobian of A_c^T*v0 with respect to
  /// position. This is AS SLOW AS FINITE DIFFERENCING THE WHOLE ENGINE, which
  /// is way too slow to use in practice.
  Eigen::MatrixXs finiteDifferenceJacobianOfClampingConstraintsTranspose(
      simulation::WorldPtr world, Eigen::VectorXs v0, bool useRidders = true);

  /// This computes the finite difference Jacobian of A_c^T*v0 with respect to
  /// position. This is AS SLOW AS FINITE DIFFERENCING THE WHOLE ENGINE, which
  /// is way too slow to use in practice. Using Ridders, so even slower.
  Eigen::MatrixXs finiteDifferenceRiddersJacobianOfClampingConstraintsTranspose(
      simulation::WorldPtr world, Eigen::VectorXs v0);

  /// This computes the Jacobian of A_ub*(E*f0) with respect to position using
  /// impulse tests.
  Eigen::MatrixXs getJacobianOfUpperBoundConstraints(
      simulation::WorldPtr world, Eigen::VectorXs E_f0);

  /// This computes the finite difference Jacobian of A_ub*E*f0 with respect to
  /// position. This is AS SLOW AS FINITE DIFFERENCING THE WHOLE ENGINE, which
  /// is way too slow to use in practice.
  Eigen::MatrixXs finiteDifferenceJacobianOfUpperBoundConstraints(
      simulation::WorldPtr world, Eigen::VectorXs f0, bool useRidders = true);

  /// This computes the finite difference Jacobian of A_ub*E*f0 with respect to
  /// position. This is AS SLOW AS FINITE DIFFERENCING THE WHOLE ENGINE, which
  /// is way too slow to use in practice. Uses Ridders method.
  Eigen::MatrixXs finiteDifferenceRiddersJacobianOfUpperBoundConstraints(
      simulation::WorldPtr world, Eigen::VectorXs f0);

  /// This computes the Jacobian of A_ub^T*v0 with respect to position using
  /// impulse tests.
  Eigen::MatrixXs getJacobianOfUpperBoundConstraintsTranspose(
      simulation::WorldPtr world, Eigen::VectorXs v0);

  /// This returns the jacobian of constraint force, holding everything constant
  /// except the value of WithRespectTo
  Eigen::MatrixXs finiteDifferenceJacobianOfConstraintForce(
      simulation::WorldPtr world, WithRespectTo* wrt, bool useRidders = true);

  /// This returns the jacobian of constraint force, holding everything constant
  /// except the value of WithRespectTo. Uses Ridders method.
  Eigen::MatrixXs finiteDifferenceRiddersJacobianOfConstraintForce(
      simulation::WorldPtr world, WithRespectTo* wrt);

  /// This returns the jacobian of estimated constraint force, without actually
  /// running forward passes, holding everyhing constant except the value of
  /// WithRespectTo
  Eigen::MatrixXs finiteDifferenceJacobianOfEstimatedConstraintForce(
      simulation::WorldPtr world, WithRespectTo* wrt, bool useRidders = true);

  /// This returns the jacobian of estimated constraint force, without actually
  /// running forward passes, holding everyhing constant except the value of
  /// WithRespectTo. Uses Ridders method.
  Eigen::MatrixXs finiteDifferenceRiddersJacobianOfEstimatedConstraintForce(
      simulation::WorldPtr world, WithRespectTo* wrt);

  /// These was the mX() vector used to construct this. Pretty much only here
  /// for testing.
  Eigen::VectorXs getContactConstraintImpulses();

  /// These was the fIndex() vector used to construct this. Pretty much only
  /// here for testing.
  Eigen::VectorXi getContactConstraintMappings();

  /// Returns the vector of the coefficients on the diagonal of the bounce
  /// matrix. These are 1+restitutionCoeff[i].
  Eigen::VectorXs getBounceDiagonals();

  /// Returns the vector of the restitution coeffs, sized for the number of
  /// bouncing collisions.
  Eigen::VectorXs getRestitutionDiagonals();

  /// Returns the penetration correction hack "bounce" (or 0 if the contact is
  /// not inter-penetrating or is actively bouncing) at each contact point.
  Eigen::VectorXs getPenetrationCorrectionVelocities();

  /// Returns the constraint impulses along the clamping constraints
  Eigen::VectorXs getClampingConstraintImpulses();

  /// Returns the relative velocities along the clamping constraints
  Eigen::VectorXs getClampingConstraintRelativeVels();

  /// Returns the velocity change caused by illegal impulses in the LCP this
  /// timestep
  Eigen::VectorXs getVelocityDueToIllegalImpulses();

  /// Returns the velocity pre-LCP
  Eigen::VectorXs getPreLCPVelocity();

  /// Returns true if there were any bounces in this snapshot.
  bool hasBounces();

  /// Returns the number of contacts (regardless of state) in this snapshot.
  std::size_t getNumContacts();

  /// Returns the number of clamping contacts in this snapshot.
  std::size_t getNumClamping();

  /// Returns the number of upper bound contacts in this snapshot.
  std::size_t getNumUpperBound();

  /// These are the gradient constraint matrices from the LCP solver
  std::vector<std::shared_ptr<ConstrainedGroupGradientMatrices>>
      mGradientMatrices;

  /// This is the clamping constraints from all the constrained
  /// groups, concatenated together
  std::vector<std::shared_ptr<DifferentiableContactConstraint>>
  getDifferentiableConstraints();

  /// This is the clamping constraints from all the constrained
  /// groups, concatenated together
  std::vector<std::shared_ptr<DifferentiableContactConstraint>>
  getClampingConstraints();

  /// This is the upper bound constraints from all the constrained
  /// groups, concatenated together
  std::vector<std::shared_ptr<DifferentiableContactConstraint>>
  getUpperBoundConstraints();

  /// This verifies that the two matrices are equal to some tolerance, and if
  /// they're not it prints the information needed to replicated this scenario
  /// and it exits the program.
  void equalsOrCrash(
      std::shared_ptr<simulation::World> world,
      Eigen::MatrixXs analytical,
      Eigen::MatrixXs bruteForce,
      std::string name);

  /// This compares our analytical sub-Jacobians (like dMinv), to attempt to
  /// diagnose where there are differences creeping in between our finite
  /// differencing and our analytical results.
  void diagnoseSubJacobianErrors(
      std::shared_ptr<simulation::World> world, WithRespectTo* wrt);

  /// This prints code to the console to replicate a scenario for testing.
  void printReplicationInstructions(std::shared_ptr<simulation::World> world);

  /// Returns true if we were able to standardize our LCP results, false if we
  /// weren't
  bool areResultsStandardized() const;

  /// If this is true, we use finite-differencing to compute all of the
  /// requested Jacobians. This override can be useful to verify if there's a
  /// bug in the analytical Jacobians that's causing learning to not converge.
  void setUseFDOverride(bool override);

  /// If this is true, we check all Jacobians against their finite-differencing
  /// counterparts at runtime. If they aren't sufficiently close, we immediately
  /// crash the program and print what went wrong and some simple replication
  /// instructions.
  void setSlowDebugResultsAgainstFD(bool slowDebug);

  /// This does a battery of tests comparing the speeds to compute all the
  /// different Jacobians, both with finite differencing and analytically, and
  /// prints the results to std out.
  void benchmarkJacobians(
      std::shared_ptr<simulation::World> world, int numSamples);

protected:
  /// If this is true, we use finite-differencing to compute all of the
  /// requested Jacobians. This override can be useful to verify if there's a
  /// bug in the analytical Jacobians that's causing learning to not converge.
  bool mUseFDOverride;

  /// If this is true, we check all Jacobians against their finite-differencing
  /// counterparts at runtime. If they aren't sufficiently close, we immediately
  /// crash the program and print what went wrong and some simple replication
  /// instructions.
  bool mSlowDebugResultsAgainstFD;

  /// This is the global timestep length. This is included here because it shows
  /// up as a constant in some of the matrices.
  s_t mTimeStep;

  /// This is the total DOFs for this World
  std::size_t mNumDOFs;

  /// This is the number of total dimensions on all the constraints active in
  /// the world
  std::size_t mNumConstraintDim;

  /// This is the number of total constraint dimensions that are clamping
  std::size_t mNumClamping;

  /// This is the number of total constraint dimensions that are upper bounded
  std::size_t mNumUpperBound;

  /// This is the number of total constraint dimensions that are upper bounded
  std::size_t mNumBouncing;

  /// These are the offsets into the total degrees of freedom for each skeleton
  std::unordered_map<std::string, std::size_t> mSkeletonOffset;

  /// These are the number of degrees of freedom for each skeleton
  std::unordered_map<std::string, std::size_t> mSkeletonDofs;

  /// The position of all the DOFs of the world BEFORE the timestep
  Eigen::VectorXs mPreStepPosition;

  /// The velocities of all the DOFs of the world BEFORE the timestep
  Eigen::VectorXs mPreStepVelocity;

  /// The torques on all the DOFs of the world BEFORE the timestep
  Eigen::VectorXs mPreStepTorques;

  /// The LCP's initial cached value BEFORE the timestep
  Eigen::VectorXs mPreStepLCPCache;

  /// The velocities of all the DOFs of the world AFTER an unconstrained forward
  /// step, but BEFORE the LCP runs
  Eigen::VectorXs mPreConstraintVelocities;

  /// The position of all the DOFs of the world AFTER the timestep
  Eigen::VectorXs mPostStepPosition;

  /// The velocities of all the DOFs of the world AFTER the timestep
  /// created
  Eigen::VectorXs mPostStepVelocity;

  /// The torques on all the DOFs of the world AFTER the timestep
  Eigen::VectorXs mPostStepTorques;

private:
  /// These are mCached versions of the various Jacobians
  bool mCachedPosPosDirty;
  Eigen::MatrixXs mCachedPosPos;
  bool mCachedPosVelDirty;
  Eigen::MatrixXs mCachedPosVel;
  bool mCachedBounceApproximationDirty;
  Eigen::MatrixXs mCachedBounceApproximation;
  bool mCachedVelPosDirty;
  Eigen::MatrixXs mCachedVelPos;
  bool mCachedVelVelDirty;
  Eigen::MatrixXs mCachedVelVel;
  bool mCachedForcePosDirty;
  Eigen::MatrixXs mCachedForcePos;
  bool mCachedForceVelDirty;
  Eigen::MatrixXs mCachedForceVel;
  bool mCachedMassVelDirty;
  Eigen::MatrixXs mCachedMassVel;
  bool mCachedPosCDirty;
  Eigen::MatrixXs mCachedPosC;
  bool mCachedVelCDirty;
  Eigen::MatrixXs mCachedVelC;

  Eigen::VectorXs scratch(simulation::WorldPtr world);

  enum MatrixToAssemble
  {
    CLAMPING,
    MASSED_CLAMPING,
    UPPER_BOUND,
    MASSED_UPPER_BOUND,
    BOUNCING
  };

  Eigen::MatrixXs assembleMatrix(
      simulation::WorldPtr world, MatrixToAssemble whichMatrix);

  enum BlockDiagonalMatrixToAssemble
  {
    MASS,
    INV_MASS,
    POS_C,
    VEL_C
  };

  Eigen::MatrixXs assembleBlockDiagonalMatrix(
      simulation::WorldPtr world,
      BlockDiagonalMatrixToAssemble whichMatrix,
      bool forFiniteDifferencing = false);

  enum VectorToAssemble
  {
    CONTACT_CONSTRAINT_IMPULSES,
    CONTACT_CONSTRAINT_MAPPINGS,
    BOUNCE_DIAGONALS,
    RESTITUTION_DIAGONALS,
    PENETRATION_VELOCITY_HACK,
    CLAMPING_CONSTRAINT_IMPULSES,
    CLAMPING_CONSTRAINT_RELATIVE_VELS,
    VEL_DUE_TO_ILLEGAL,
    PRE_STEP_VEL,
    PRE_STEP_TAU,
    PRE_LCP_VEL,
    CFM_CONSTANTS
  };
  template <typename Vec>
  Vec assembleVector(VectorToAssemble whichVector);

  template <typename Vec>
  const Vec& getVectorToAssemble(
      std::shared_ptr<ConstrainedGroupGradientMatrices> matrices,
      VectorToAssemble whichVector);
};

using BackpropSnapshotPtr = std::shared_ptr<BackpropSnapshot>;

} // namespace neural
} // namespace dart

#endif


/**
 * \copyright
 * Copyright (c) 2012-2020, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/project/license
 */

#include "FEFLOWMeshInterface.h"

#include <cctype>
#include <memory>

#include <boost/algorithm/string/trim.hpp>

#include "BaseLib/Logging.h"

#include "BaseLib/FileTools.h"
#include "BaseLib/StringTools.h"

#include "Applications/FileIO/FEFLOW/FEFLOWGeoInterface.h"
#include "GeoLib/Point.h"
#include "GeoLib/Polygon.h"

#include "MeshLib/Elements/Elements.h"
#include "MeshLib/Mesh.h"
#include "MeshLib/Node.h"

namespace FileIO
{
MeshLib::Mesh* FEFLOWMeshInterface::readFEFLOWFile(const std::string& filename)
{
    std::ifstream in(filename.c_str());
    if (!in)
    {
        ERR("FEFLOWMeshInterface::readFEFLOWFile(): Could not open file {:s}.",
            filename);
        return nullptr;
    }

    FEM_CLASS fem_class;
    FEM_DIM fem_dim;
    std::vector<GeoLib::Point*>* points = nullptr;
    std::vector<GeoLib::Polyline*>* lines = nullptr;

    bool isXZplane = false;

    std::vector<MeshLib::Node*> vec_nodes;
    std::vector<MeshLib::Element*> vec_elements;

    std::vector<std::vector<std::size_t>> vec_elementsets;

    std::string line_string;
    std::stringstream line_stream;
    while (!in.eof())
    {
        std::getline(in, line_string);
        boost::trim_right(line_string);
        //....................................................................
        // CLASS: the version number follows afterward, e.g. CLASS (v.5.313)
        if (line_string.find("CLASS") != std::string::npos)
        {
            std::getline(in, line_string);
            boost::trim_right(line_string);
            line_stream.str(line_string);
            // problem class, time mode, problem orientation, dimension, nr.
            // layers for 3D, saturation switch, precision of results, precision
            // of coordinates
            line_stream >> fem_class.problem_class >> fem_class.time_mode >>
                fem_class.orientation >> fem_class.dimension >>
                fem_class.n_layers3d;
            line_stream.clear();
        }
        //....................................................................
        // DIMENS
        else if (line_string == "DIMENS")
        {
            // DIMENS
            std::getline(in, line_string);
            line_stream.str(line_string);
            line_stream >> fem_dim.n_nodes >> fem_dim.n_elements >>
                fem_dim.n_nodes_of_element >> std::ws;
            // create node pointers with dummy coordinates to create element
            // objects.
            // True coordinates are set later in COOR and ELEV_I.
            vec_nodes.resize(fem_dim.n_nodes);
            std::size_t count = 0;
            double dummy_coords[3] = {};
            std::generate(vec_nodes.begin(), vec_nodes.end(), [&]()
                          {
                              return new MeshLib::Node(dummy_coords, count++);
                          });
            line_stream.clear();
        }
        //....................................................................
        // NODE (node index for elements)
        else if (line_string == "NODE")
        {
            assert(!vec_nodes.empty());

            MeshLib::MeshElemType eleType = MeshLib::MeshElemType::INVALID;
            if (fem_dim.n_nodes_of_element == 2)
            {
                eleType = MeshLib::MeshElemType::LINE;
            }
            else if (fem_dim.n_nodes_of_element == 3)
            {
                eleType = MeshLib::MeshElemType::TRIANGLE;
            }
            else if (fem_dim.n_nodes_of_element == 4 &&
                     fem_class.dimension == 2)
            {
                eleType = MeshLib::MeshElemType::QUAD;
            }
            else if (fem_dim.n_nodes_of_element == 4 &&
                     fem_class.dimension == 3)
            {
                eleType = MeshLib::MeshElemType::TETRAHEDRON;
            }
            else if (fem_dim.n_nodes_of_element == 6 &&
                     fem_class.dimension == 3)
            {
                eleType = MeshLib::MeshElemType::PRISM;
            }
            else if (fem_dim.n_nodes_of_element == 8 &&
                     fem_class.dimension == 3)
            {
                eleType = MeshLib::MeshElemType::HEXAHEDRON;
            }

            if (eleType == MeshLib::MeshElemType::INVALID)
            {
                ERR("FEFLOWInterface::readFEFLOWFile(): Unsupported element "
                    "type with the number of node = {:d} and dim = {:d}",
                    fem_dim.n_nodes_of_element, fem_class.dimension);
                std::for_each(vec_nodes.begin(), vec_nodes.end(),
                              [](MeshLib::Node* nod)
                              {
                                  delete nod;
                              });
                vec_nodes.clear();
                return nullptr;
            }

            vec_elements.reserve(fem_dim.n_elements);
            for (std::size_t i = 0; i < fem_dim.n_elements; i++)
            {
                std::getline(in, line_string);
                vec_elements.push_back(
                    readElement(fem_dim, eleType, line_string, vec_nodes));
            }
        }
        else if (line_string == "VARNODE")
        {
            assert(!vec_nodes.empty());

            vec_elements.reserve(fem_dim.n_elements);
            if (fem_dim.n_nodes_of_element == 0)
            {  // mixed element case
                if (!std::getline(in, line_string))
                {
                    ERR("FEFLOWInterface::readFEFLOWFile(): read element "
                        "error");
                    std::for_each(vec_nodes.begin(), vec_nodes.end(),
                                  [](MeshLib::Node* nod) { delete nod; });
                    vec_nodes.clear();
                    return nullptr;
                }
            }

            for (std::size_t i = 0; i < fem_dim.n_elements; i++)
            {
                std::getline(in, line_string);
                vec_elements.push_back(
                    readElement(line_string, vec_nodes));
            }
        }
        //....................................................................
        // COOR
        else if (line_string == "COOR")
        {
            readNodeCoordinates(in, fem_class, fem_dim, vec_nodes);
        }
        else if (line_string == "XYZCOOR")
        {
            readNodeCoordinates(in, vec_nodes);
        }
        // ELEV_I
        else if (line_string == "ELEV_I")
        {
            if (fem_class.dimension == 2)
            {
                continue;
            }
            readElevation(in, fem_class, fem_dim, vec_nodes);
        }
        //....................................................................
        // GRAVITY
        else if (line_string == "GRAVITY")
        {
            getline(in, line_string);
            line_stream.str(line_string);
            double vec[3] = {};
            line_stream >> vec[0] >> vec[1] >> vec[2];
            if (vec[0] == 0.0 && vec[1] == -1.0 && vec[2] == 0.0)
            {
                // x-z plane
                isXZplane = true;
            }
            line_stream.clear();
        }
        //....................................................................
        // ELEMENTALSETS
        else if (line_string == "ELEMENTALSETS")
        {
            readELEMENTALSETS(in, vec_elementsets);
        }
        //....................................................................
        // SUPERMESH
        else if (line_string == "SUPERMESH")
        {
            FileIO::FEFLOWGeoInterface::readSuperMesh(
                in, fem_class.dimension, points, lines);
        }
        //....................................................................
    }
    in.close();

    INFO("Create mesh");
    std::string project_name(
        BaseLib::extractBaseNameWithoutExtension(filename));
    auto mesh(
        std::make_unique<MeshLib::Mesh>(project_name, vec_nodes, vec_elements));
    INFO("Set values for material property.");
    auto opt_material_ids(mesh->getProperties().createNewPropertyVector<int>(
        "MaterialIDs", MeshLib::MeshItemType::Cell, 1));
    if (!opt_material_ids)
    {
        WARN("Could not create PropertyVector for MaterialIDs in Mesh.");
    }
    else
    {
        opt_material_ids->resize(mesh->getNumberOfElements());
        setMaterialIDs(fem_class, fem_dim, lines, vec_elementsets, vec_elements,
                       *opt_material_ids);
    }

    if (isXZplane)
    {
        for (auto* nod : vec_nodes)
        {
            (*nod)[2] = (*nod)[1];
            (*nod)[1] = 0.0;
        }
        if (points)
        {
            for (auto* pt : *points)
            {
                (*pt)[2] = (*pt)[1];
                (*pt)[1] = .0;
            }
        }
    }

    return mesh.release();
}

void FEFLOWMeshInterface::readNodeCoordinates(
    std::ifstream& in, std::vector<MeshLib::Node*>& vec_nodes)
{
    std::string line_string;
    char dummy_char;  // for comma(,)

    for (unsigned k = 0; k < vec_nodes.size(); ++k)
    {
        // read the line containing the coordinates as string
        if (!std::getline(in, line_string))
        {
            ERR("Could not read the node '{:d}'.", k);
            for (auto* n : vec_nodes)
            {
                delete n;
            }
            return;
        }
        std::stringstream line_stream;
        line_stream.str(line_string);
        // parse the particular coordinates from the string read above
        for (std::size_t i(0); i < 3; ++i)
        {
            if (!(line_stream >> (*vec_nodes[k])[i]))
            {
                ERR("Could not parse coordinate {:d} of node '{:d}'.", i, k);
                for (auto* n : vec_nodes)
                {
                    delete n;
                }
                return;
            }
            if (!(line_stream >> dummy_char) && i < 2)  // read comma
            {
                ERR("Could not parse node '{:d}'.", k);
                for (auto* n : vec_nodes)
                {
                    delete n;
                }
                return;
            }
        }
    }
}

void FEFLOWMeshInterface::readNodeCoordinates(
    std::ifstream& in, const FEM_CLASS& fem_class, const FEM_DIM& fem_dim,
    std::vector<MeshLib::Node*> const& vec_nodes)
{
    const std::size_t no_nodes_per_layer =
        (fem_class.dimension == 2)
            ? fem_dim.n_nodes
            : fem_dim.n_nodes / (fem_class.n_layers3d + 1);
    assert(no_nodes_per_layer > 0);
    const std::size_t n_lines = (no_nodes_per_layer - 1) / 12 + 1;
    const std::size_t n_layers =
        (fem_class.dimension == 3) ? fem_class.n_layers3d + 1 : 1;
    std::string line_string;
    std::stringstream line_stream;
    double x;
    char dummy_char;  // for comma(,)
    // x, y
    for (unsigned k = 0; k < 2; k++)
    {
        // each line
        for (std::size_t i = 0; i < n_lines; i++)
        {
            getline(in, line_string);
            line_stream.str(line_string);
            for (unsigned j = 0; j < 12; j++)
            {
                if (i * 12 + j >= no_nodes_per_layer)
                {
                    break;
                }
                line_stream >> x >> dummy_char;
                for (std::size_t l = 0; l < n_layers; l++)
                {
                    const std::size_t n = i * 12 + l * no_nodes_per_layer + j;
                    MeshLib::Node* m_nod = vec_nodes[n];
                    if (k == 0)
                    {
                        (*m_nod)[0] = x;
                    }
                    else
                    {
                        (*m_nod)[1] = x;
                    }
                }
            }
            line_stream.clear();
        }
    }
}

std::vector<std::size_t> FEFLOWMeshInterface::getIndexList(
    const std::string& str_ranges)
{
    std::vector<std::size_t> vec_node_IDs;

    // insert space before and after minus for splitting
    std::string str_ranges2(BaseLib::replaceString("-", " # ", str_ranges));
    BaseLib::trim(str_ranges2);
    auto splitted_str = BaseLib::splitString(str_ranges2, ' ');
    bool is_range = false;
    for (auto str : splitted_str)
    {
        if (str.empty())
        {
            continue;
        }
        if (str[0] == '#')
        {
            is_range = true;
        }
        else if (is_range)
        {
            const std::size_t start = vec_node_IDs.back();
            const auto end = BaseLib::str2number<std::size_t>(str);
            for (std::size_t i = start + 1; i < end + 1; i++)
            {
                vec_node_IDs.push_back(i);
            }
            is_range = false;
        }
        else
        {
            BaseLib::trim(str);
            vec_node_IDs.push_back(BaseLib::str2number<std::size_t>(str));
        }
    }

    return vec_node_IDs;
}

void FEFLOWMeshInterface::readElevation(std::ifstream& in,
                                        const FEM_CLASS& fem_class,
                                        const FEM_DIM& fem_dim,
                                        std::vector<MeshLib::Node*>& vec_nodes)
{
    const std::size_t no_nodes_per_layer =
        fem_dim.n_nodes / (fem_class.n_layers3d + 1);
    double z = .0;
    std::string str_nodeList;
    std::string line_string;
    std::stringstream line_stream;
    std::size_t l = 0;
    unsigned mode = 0;  // 0: exit, 1: slice no, 2: elevation value, 3:
                        // continued line of mode 2
    std::streamoff pos_prev_line = 0;
    while (true)
    {
        pos_prev_line = in.tellg();
        std::getline(in, line_string);
        boost::trim_right(line_string);

        // check mode
        auto columns = BaseLib::splitString(line_string, ' ');
        if (!in || std::isalpha(line_string[0]))
        {
            mode = 0;
        }
        else if (line_string.empty())
        {
            continue;
        }
        else if (line_string[0] == '\t')
        {
            mode = 3;
        }
        else if (columns.size() == 1)
        {
            mode = 1;
        }
        else
        {  // columns.size()>1
            mode = 2;
        }

        // process stocked data
        if (mode != 3 && !str_nodeList.empty())
        {
            // process previous lines
            auto vec_nodeIDs = getIndexList(str_nodeList);
            for (auto n0 : vec_nodeIDs)
            {
                const std::size_t n = n0 - 1 + l * no_nodes_per_layer;
                (*vec_nodes[n])[2] = z;
            }
            str_nodeList.clear();
        }

        if (mode == 0)
        {
            break;
        }
        if (mode == 1)
        {
            // slice number
            l++;
            assert(l + 1 == BaseLib::str2number<std::size_t>(columns.front()));
        }
        else if (mode == 2)
        {
            // parse current line
            line_stream.str(line_string);
            line_stream >> z;
            std::getline(line_stream, str_nodeList);
            boost::trim(str_nodeList);
            line_stream.clear();
        }
        else if (mode == 3)
        {
            // continue reading node range
            BaseLib::trim(line_string, '\t');
            str_nodeList += " " + line_string;
        }
    }

    // move stream position to previous line
    if (std::isalpha(line_string[0]))
    {
        in.seekg(pos_prev_line);
    }
}

MeshLib::Element* FEFLOWMeshInterface::readElement(
    std::string const& line, std::vector<MeshLib::Node*> const& nodes)
{
    std::stringstream ss(line);

    int ele_type;
    ss >> ele_type;

    MeshLib::MeshElemType elem_type;
    std::size_t n_nodes_of_element;
    switch (ele_type)
    {
        case 6:
            elem_type = MeshLib::MeshElemType::TETRAHEDRON;
            n_nodes_of_element = 4;
            break;
        case 7:
            elem_type = MeshLib::MeshElemType::PRISM;
            n_nodes_of_element = 6;
            break;
        case 8:
            elem_type = MeshLib::MeshElemType::HEXAHEDRON;
            n_nodes_of_element = 8;
            break;
        default:
            WARN("Could not parse element type.");
            return nullptr;
    }

    unsigned idx[8];
    for (std::size_t i = 0; i < n_nodes_of_element; ++i)
    {
        ss >> idx[i];
    }
    auto** ele_nodes = new MeshLib::Node*[n_nodes_of_element];

    switch (elem_type)
    {
        default:
            for (std::size_t k(0); k < n_nodes_of_element; ++k)
            {
                ele_nodes[k] = nodes[idx[k] - 1];
            }
            break;
        case MeshLib::MeshElemType::HEXAHEDRON:
        case MeshLib::MeshElemType::PRISM:
            const unsigned n_half_nodes = n_nodes_of_element / 2;
            for (unsigned k(0); k < n_half_nodes; ++k)
            {
                ele_nodes[k] = nodes[idx[k + n_half_nodes] - 1];
                ele_nodes[k + n_half_nodes] = nodes[idx[k] - 1];
            }
            break;
    }

    switch (elem_type)
    {
        case MeshLib::MeshElemType::TETRAHEDRON:
            return new MeshLib::Tet(ele_nodes);
        case MeshLib::MeshElemType::HEXAHEDRON:
            return new MeshLib::Hex(ele_nodes);
        case MeshLib::MeshElemType::PRISM:
            return new MeshLib::Prism(ele_nodes);
        default:
            return nullptr;
    }
}

MeshLib::Element* FEFLOWMeshInterface::readElement(
    const FEM_DIM& fem_dim, const MeshLib::MeshElemType elem_type,
    const std::string& line, const std::vector<MeshLib::Node*>& nodes)
{
    std::stringstream ss(line);

    unsigned idx[8];
    for (std::size_t i = 0; i < fem_dim.n_nodes_of_element; ++i)
    {
        ss >> idx[i];
    }
    auto** ele_nodes = new MeshLib::Node*[fem_dim.n_nodes_of_element];

    switch (elem_type)
    {
        default:
            for (unsigned k(0); k < fem_dim.n_nodes_of_element; ++k)
            {
                ele_nodes[k] = nodes[idx[k] - 1];
            }
            break;
        case MeshLib::MeshElemType::HEXAHEDRON:
        case MeshLib::MeshElemType::PRISM:
            const unsigned n_half_nodes = fem_dim.n_nodes_of_element / 2;
            for (unsigned k(0); k < n_half_nodes; ++k)
            {
                ele_nodes[k] = nodes[idx[k + n_half_nodes] - 1];
                ele_nodes[k + n_half_nodes] = nodes[idx[k] - 1];
            }
            break;
    }

    switch (elem_type)
    {
        case MeshLib::MeshElemType::LINE:
            return new MeshLib::Line(ele_nodes);
        case MeshLib::MeshElemType::TRIANGLE:
            return new MeshLib::Tri(ele_nodes);
        case MeshLib::MeshElemType::QUAD:
            return new MeshLib::Quad(ele_nodes);
        case MeshLib::MeshElemType::TETRAHEDRON:
            return new MeshLib::Tet(ele_nodes);
        case MeshLib::MeshElemType::HEXAHEDRON:
            return new MeshLib::Hex(ele_nodes);
        case MeshLib::MeshElemType::PRISM:
            return new MeshLib::Prism(ele_nodes);
        default:
            assert(false);
            return nullptr;
    }
}

void FEFLOWMeshInterface::readELEMENTALSETS(
    std::ifstream& in, std::vector<std::vector<std::size_t>>& vec_elementsets)
{
    auto compressSpaces = [](std::string const& str)
    {
        std::stringstream ss(str);
        std::string new_str;
        std::string word;
        while (ss)
        {
            ss >> word;
            new_str += " " + word;
        }
        return new_str;
    };

    std::string line_string;
    std::string str_idList;
    std::streampos pos_prev_line = 0;
    while (true)
    {
        pos_prev_line = in.tellg();
        getline(in, line_string);

        unsigned mode = 0;
        if (!in)
        {
            mode = 0;  // reached the end of the file
        }
        else if (line_string.empty())
        {
            continue;  // skip and see what comes next
        }
        else if (std::isalpha(line_string[0]))
        {
            mode = 0;  // reached the next section
        }
        else if (line_string[0] == ' ')
        {
            mode = 1;  // start of the element set definition
        }
        else if (line_string[0] == '\t')
        {
            mode = 2;  // continue the definition
        }
        else
        {
            ERR("Failed during parsing of an ELEMENTALSETS section in a FEFLOW "
                "file");
            break;
        }

        if (mode != 2 && !str_idList.empty())
        {
            vec_elementsets.push_back(getIndexList(str_idList));
            str_idList.clear();
        }

        if (mode == 0)
        {
            break;
        }
        if (mode == 1)
        {
            // starting a new set
            std::string set_name;
            std::string ids;
            BaseLib::trim(line_string, ' ');
            if (line_string[0] == '"')
            {  // multiple words
                auto pos = line_string.find_last_of('"');
                set_name = line_string.substr(1, pos - 1);  // without quotation
                ids = line_string.substr(pos + 1);
            }
            else
            {  // single word
                auto pos = line_string.find_first_of(' ');
                set_name = line_string.substr(0, pos);
                ids = line_string.substr(pos + 1);
            }
            INFO("Found an element group - {:s}", set_name.data());
            str_idList += compressSpaces(ids);
        }
        else
        {
            // continue reading a element ids
            BaseLib::trim(line_string, '\t');
            str_idList += compressSpaces(line_string);
        }
    }
    // move stream position to previous line
    if (std::isalpha(line_string[0]))
    {
        in.seekg(pos_prev_line);
    }
}

void FEFLOWMeshInterface::setMaterialIDs(
    FEM_CLASS const& fem_class,
    FEM_DIM const& fem_dim,
    std::vector<GeoLib::Polyline*>* const& lines,
    std::vector<std::vector<std::size_t>> const& vec_elementsets,
    std::vector<MeshLib::Element*> const& vec_elements,
    std::vector<int>& material_ids)
{
    assert(material_ids.size()==vec_elements.size());
    if (!vec_elementsets.empty())
    {
        for (std::size_t matid = 0; matid < vec_elementsets.size(); ++matid)
        {
            auto& eids = vec_elementsets[matid];
            for (auto eid : eids)
            {
                material_ids[eid - 1] =
                    matid;  // Element IDs given by FEFLOW starts from one!
            }
        }
    }
    else if (lines && !lines->empty())
    {
        for (std::size_t i = 0; i < vec_elements.size(); ++i)
        {
            MeshLib::Node const gpt =
                MeshLib::getCenterOfGravity(*vec_elements[i]);
            std::size_t matId = 0;
            for (std::size_t j = 0; j < lines->size(); j++)
            {
                GeoLib::Polyline* poly = (*lines)[j];
                if (!poly->isClosed())
                {
                    continue;
                }

                GeoLib::Polygon polygon(*poly, true);
                if (polygon.isPntInPolygon(gpt[0], gpt[1], gpt[2]))
                {
                    matId = j;
                    break;
                }
            }
            material_ids[i] = matId;
        }
    }
    else if (fem_class.n_layers3d > 0)
    {
        const std::size_t no_nodes_per_layer =
            fem_dim.n_nodes / (fem_class.n_layers3d + 1);
        for (std::size_t i = 0; i < vec_elements.size(); i++)
        {
            MeshLib::Element* e = vec_elements[i];
            std::size_t e_min_nodeID = std::numeric_limits<std::size_t>::max();
            for (std::size_t j = 0; j < e->getNumberOfBaseNodes(); j++)
            {
                e_min_nodeID = std::min(e_min_nodeID, e->getNodeIndex(j));
            }
            std::size_t layer_id = e_min_nodeID / no_nodes_per_layer;
            material_ids[i] = layer_id;
        }
    }
}

}  // namespace FileIO