Enable saving and restoring subsimulator state

include/cosim/algorithm/simulator.hpp

@@ -133,6 +133,55 @@ class simulator : public manipulable
     virtual step_result do_step(
         time_point currentT,
         duration deltaT) = 0;
+
+    /// A type used for references to saved states (see `save_state()`).
+    using state_index = int;
+
+    /**
+     *  Saves the current state.
+     *
+     *  This will create and store a copy of the simulator's current internal
+     *  state, so that it can be restored at a later time.  The copy is stored
+     *  internally in the simulator, and must be referred to by the returned
+     *  `state_index`. The index is only valid for this particular simulator.
+     *
+     *  The function may be called at any point after `setup()` has been called.
+     */
+    virtual state_index save_state() = 0;
+
+    /**
+     *  Saves the current state, overwriting a previously-saved state.
+     *
+     *  This function does the same as `save_state()`, except that it
+     *  overwrites a state which has previously been stored by that function.
+     *  The old index thereafter refers to the newly-saved state.
+     */
+    virtual void save_state(state_index stateIndex) = 0;
+
+    /**
+     *  Restores a previously-saved state.
+     *
+     *  This restores the simulator to a state which has previously been saved
+     *  using `save_state()`.
+     *
+     *  Note that the saved state is supposed to be the *complete and exact*
+     *  state of the simulator at the moment `save_state()` was called. For example,
+     *  if the state was saved while the simulator was in initialisation mode
+     *  (between `setup()` and `start_simulation()`), then it will be restored
+     *  in that mode, and `start_simulation()` must be called before the
+     *  simulation can start.  Similarly, if it is saved at logical time `t`,
+     *  then the first `do_step()` call after restoration must start at `t`.
+     */
+    virtual void restore_state(state_index stateIndex) = 0;
+
+    /**
+     *  Frees all resources (e.g. memory) associated with a saved state.
+     *
+     *  After this, the state may no longer be restored with `restore_state()`,
+     *  nor may it be overwritten with `save_state(state_index)`.  The
+     *  implementation is free to reuse the same `state_index` at a later point.
+     */
+    virtual void release_state(state_index stateIndex) = 0;
 };
 
 } // namespace cosim

include/cosim/fmi/v1/fmu.hpp

@@ -165,6 +165,11 @@ class slave_instance : public fmi::slave_instance
         gsl::span<const value_reference> variables,
         gsl::span<const std::string> values) override;
 
+    state_index save_state() override;
+    void save_state(state_index overwriteState) override;
+    void restore_state(state_index state) override;
+    void release_state(state_index state) override;
+
     // fmi::slave_instance methods
     std::shared_ptr<fmi::fmu> fmu() const override
     {

include/cosim/fmi/v2/fmu.hpp

@@ -21,9 +21,11 @@
 #include <string>
 #include <string_view>
 #include <vector>
+#include <queue>
 
 
 struct fmi2_import_t;
+using fmi2_FMU_state_t = void*;
 
 
 namespace cosim
@@ -165,6 +167,11 @@ class slave_instance : public fmi::slave_instance
         gsl::span<const value_reference> variables,
         gsl::span<const std::string> values) override;
 
+    state_index save_state() override;
+    void save_state(state_index stateIndex) override;
+    void restore_state(state_index stateIndex) override;
+    void release_state(state_index stateIndex) override;
+
     // fmi::slave_instance methods
     std::shared_ptr<fmi::fmu> fmu() const override
     {
@@ -178,13 +185,24 @@ class slave_instance : public fmi::slave_instance
     fmi2_import_t* fmilib_handle() const;
 
 private:
+    struct saved_state
+    {
+        fmi2_FMU_state_t fmuState = nullptr;
+        bool setupComplete = false;
+        bool simStarted = false;
+    };
+    void copy_current_state(saved_state& state);
+
     std::shared_ptr<v2::fmu> fmu_;
     fmi2_import_t* handle_;
 
     bool setupComplete_ = false;
     bool simStarted_ = false;
 
     std::string instanceName_;
+
+    std::vector<saved_state> savedStates_;
+    std::queue<state_index> savedStatesFreelist_;
 };
 
 

include/cosim/slave.hpp

@@ -23,6 +23,7 @@
 namespace cosim
 {
 
+
 /**
  *  An interface for classes that represent co-simulation slaves.
  *
@@ -276,6 +277,59 @@ class slave
         set_boolean_variables(boolean_variables, boolean_values);
         set_string_variables(string_variables, string_values);
     }
+
+    /// A type used for references to saved states (see `save_state()`).
+    using state_index = int;
+
+    /**
+     *  Saves the current state.
+     *
+     *  This will create and store a copy of the slave's current internal
+     *  state, so that it can be restored at a later time.  The copy is stored
+     *  internally in the slave, and must be referred to by the returned
+     *  `state_index`. The index is only valid for this particular slave.
+     *
+     *  The function may be called at any point after `setup()` has been called.
+     *
+     *  \pre `this->model_description().can_save_state`
+     */
+    virtual state_index save_state() = 0;
+
+    /**
+     *  Saves the current state, overwriting a previously-saved state.
+     *
+     *  This function does the same as `save_state()`, except that it
+     *  overwrites a state which has previously been stored by that function.
+     *  The old index thereafter refers to the newly-saved state.
+     *
+     *  \pre `this->model_description().can_save_state`
+     */
+    virtual void save_state(state_index stateIndex) = 0;
+
+    /**
+     *  Restores a previously-saved state.
+     *
+     *  This restores the slave to a state which has previously been saved
+     *  using `save_state()`.
+     *
+     *  Note that the saved state is supposed to be the *complete and exact*
+     *  state of the slave at the moment `save_state()` was called. For example,
+     *  if the state was saved while the slave was in initialisation mode
+     *  (between `setup()` and `start_simulation()`), then it will be restored
+     *  in that mode, and `start_simulation()` must be called before the
+     *  simulation can start.  Similarly, if it is saved at logical time `t`,
+     *  then the first `do_step()` call after restoration must start at `t`.
+     */
+    virtual void restore_state(state_index stateIndex) = 0;
+
+    /**
+     *  Frees all resources (e.g. memory) associated with a saved state.
+     *
+     *  After this, the state may no longer be restored with `restore_state()`,
+     *  nor may it be overwritten with `save_state(state_index)`.  The
+     *  implementation is free to reuse the same `state_index` at a later point.
+     */
+    virtual void release_state(state_index stateIndex) = 0;
 };
 
 

src/cosim/fmi/v1/fmu.cpp

@@ -549,6 +549,38 @@ void slave_instance::set_string_variables(
 }
 
 
+slave::state_index slave_instance::save_state()
+{
+    throw error(
+        make_error_code(errc::unsupported_feature),
+        "Getting and setting state not supported in FMI 1.0");
+}
+
+
+void slave_instance::save_state(state_index)
+{
+    throw error(
+        make_error_code(errc::unsupported_feature),
+        "Getting and setting state not supported in FMI 1.0");
+}
+
+
+void slave_instance::restore_state(state_index)
+{
+    throw error(
+        make_error_code(errc::unsupported_feature),
+        "Getting and setting state not supported in FMI 1.0");
+}
+
+
+void slave_instance::release_state(state_index)
+{
+    throw error(
+        make_error_code(errc::unsupported_feature),
+        "Getting and setting state not supported in FMI 1.0");
+}
+
+
 std::shared_ptr<v1::fmu> slave_instance::v1_fmu() const
 {
     return fmu_;

src/cosim/fmi/v2/fmu.cpp

@@ -569,6 +569,55 @@ void slave_instance::set_string_variables(
 }
 
 
+slave::state_index slave_instance::save_state()
+{
+    saved_state currentState;
+    copy_current_state(currentState);
+    if (savedStatesFreelist_.empty()) {
+        savedStates_.push_back(currentState);
+        return static_cast<state_index>(savedStates_.size()-1);
+    } else {
+        const auto stateIndex = savedStatesFreelist_.front();
+        savedStatesFreelist_.pop();
+        savedStates_.at(stateIndex) = currentState;
+        return stateIndex;
+    }
+}
+
+
+void slave_instance::save_state(state_index stateIndex)
+{
+    copy_current_state(savedStates_.at(stateIndex));
+}
+
+
+void slave_instance::restore_state(state_index stateIndex)
+{
+    const auto& state  = savedStates_.at(stateIndex);
+    const auto status = fmi2_import_set_fmu_state(handle_, state.fmuState);
+    if (status != fmi2_status_ok && status != fmi2_status_warning) {
+        throw error(
+            make_error_code(errc::model_error),
+            last_log_record(instanceName_).message);
+    }
+    setupComplete_ = state.setupComplete;
+    simStarted_ = state.simStarted;
+}
+
+
+void slave_instance::release_state(state_index state)
+{
+    auto fmuState = savedStates_.at(state).fmuState;
+    savedStatesFreelist_.push(state);
+    const auto status = fmi2_import_free_fmu_state(handle_, &fmuState);
+    if (status != fmi2_status_ok && status != fmi2_status_warning) {
+        throw error(
+            make_error_code(errc::model_error),
+            last_log_record(instanceName_).message);
+    }
+}
+
+
 std::shared_ptr<v2::fmu> slave_instance::v2_fmu() const
 {
     return fmu_;
@@ -581,6 +630,24 @@ fmi2_import_t* slave_instance::fmilib_handle() const
 }
 
 
+void slave_instance::copy_current_state(saved_state& state)
+{
+    if (!fmi2_import_get_capability(handle_, fmi2_cs_canGetAndSetFMUstate)) {
+        throw error(
+            make_error_code(errc::unsupported_feature),
+            instanceName_ + ": FMU does not support state saving");
+    }
+    const auto status = fmi2_import_get_fmu_state(handle_, &state.fmuState);
+    if (status != fmi2_status_ok && status != fmi2_status_warning) {
+        throw error(
+            make_error_code(errc::model_error),
+            last_log_record(instanceName_).message);
+    }
+    state.setupComplete = setupComplete_;
+    state.simStarted = simStarted_;
+}
+
+
 } // namespace v2
 } // namespace fmi
 } // namespace cosim

src/cosim/proxy/remote_slave.cpp

@@ -202,6 +202,34 @@ void cosim::proxy::remote_slave::set_string_variables(gsl::span<const cosim::val
     }
 }
 
+cosim::slave::state_index cosim::proxy::remote_slave::save_state()
+{
+    throw error(
+        make_error_code(errc::unsupported_feature),
+        "Proxyfmu does not support state saving");
+}
+
+void cosim::proxy::remote_slave::save_state(state_index)
+{
+    throw error(
+        make_error_code(errc::unsupported_feature),
+        "Proxyfmu does not support state saving");
+}
+
+void cosim::proxy::remote_slave::restore_state(state_index)
+{
+    throw error(
+        make_error_code(errc::unsupported_feature),
+        "Proxyfmu does not support state saving");
+}
+
+void cosim::proxy::remote_slave::release_state(state_index)
+{
+    throw error(
+        make_error_code(errc::unsupported_feature),
+        "Proxyfmu does not support state saving");
+}
+
 cosim::proxy::remote_slave::~remote_slave()
 {
     remote_slave::end_simulation();

src/cosim/proxy/remote_slave.hpp

@@ -59,6 +59,14 @@ class remote_slave : public slave
     void set_string_variables(gsl::span<const value_reference> variables,
         gsl::span<const std::string> values) override;
 
+    state_index save_state() override;
+
+    void save_state(state_index stateIndex) override;
+
+    void restore_state(state_index stateIndex) override;
+
+    void release_state(state_index stateIndex) override;
+
     ~remote_slave() override;
 
 private: