From 0740c3bbb6dc5ee27420380134cffa0dd3956143 Mon Sep 17 00:00:00 2001
From: Bjarne von Horn <vh@igh.de>
Date: Thu, 4 Jul 2024 18:30:35 +0200
Subject: [PATCH] permutate domain ids with FAKE_DOMAIN_PERMUTATION env

---
 fake_lib/fakeethercat.cpp | 44 ++++++++++++++++++++++++++++++++++++---
 1 file changed, 41 insertions(+), 3 deletions(-)

diff --git a/fake_lib/fakeethercat.cpp b/fake_lib/fakeethercat.cpp
index 7e5f894c..6f00f148 100644
--- a/fake_lib/fakeethercat.cpp
+++ b/fake_lib/fakeethercat.cpp
@@ -23,6 +23,13 @@
 #include "fakeethercat.h"
 
 #include <cstring>
+#include <sstream>
+#include <string>
+#include <utility>
+#include <iterator>
+
+
+static std::vector<size_t> getPermutationVector(size_t count);
 
 size_t pdo::sizeInBytes() const
 {
@@ -149,10 +156,11 @@ int ecrt_domain_state(
 
 int ec_master::activate()
 {
+    const auto permutate = getPermutationVector(domains.size());
     int i = 0;
     for (auto &domain : domains)
     {
-        if (domain.activate(i))
+        if (domain.activate(permutate[i]))
             return -1;
         ++i;
     }
@@ -163,7 +171,12 @@ int ecrt_master_activate(
     ec_master_t *master /**< EtherCAT master. */
 )
 {
-    return master->activate();
+    try {
+        return master->activate();
+    } catch (const std::exception& e) {
+        std::cerr << "Could not activate: " << e.what() << '\n';
+        return -1;
+    }
 }
 
 int ecrt_master_application_time(
@@ -274,12 +287,37 @@ ec_master_t *ecrt_request_master(
 
 static const char *getName()
 {
-    if (const auto ans = getenv("FAKE_EC_NAME")) {
+    if (const auto ans = getenv("FAKE_EC_NAME"))
+    {
         return ans;
     }
     return "FakeTaxi";
 }
 
+static std::vector<size_t> getPermutationVector(size_t count)
+{
+    std::vector<size_t> ans;
+    for (size_t i = 0; i < count; ++i)
+    {
+        ans.push_back(i);
+    }
+    const auto spec = getenv("FAKE_DOMAIN_PERMUTATION");
+    if (!spec)
+        return ans;
+    std::istringstream is(spec);
+    std::istream_iterator<int> begin(is), end;
+    std::vector<int> values(begin, end);
+    if (values.size() % 2)
+    {
+        throw std::invalid_argument("Specify an even number of indices to permutate.\n");
+    }
+    for (size_t i = 0; i < values.size() / 2; ++i)
+    {
+        std::swap(ans.at(values[2 * i]), ans.at(values[2 * i + 1]));
+    }
+    return ans;
+}
+
 ec_master::ec_master() : rt_ipc(rtipc_create(getName(), "/tmp/FakeTaxi"))
 {
 }