From 68cae20ba41fb73b53a172af971baac8d809da9d Mon Sep 17 00:00:00 2001
From: Lukas Wikander <lukas.wikander@astazero.com>
Date: Thu, 9 Sep 2021 10:31:26 +0200
Subject: [PATCH 1/2] Added a test checking if a reversed reversed trajectory
 is the same as the original

---
 common/CMakeLists.txt                   |  6 ++++
 common/tests/test_reversetrajectory.cpp | 41 +++++++++++++++++++++++++
 2 files changed, 47 insertions(+)
 create mode 100644 common/tests/test_reversetrajectory.cpp

diff --git a/common/CMakeLists.txt b/common/CMakeLists.txt
index c85132e1a..9718cc74c 100644
--- a/common/CMakeLists.txt
+++ b/common/CMakeLists.txt
@@ -105,6 +105,12 @@ add_test(relative_trajectory_point_test
 target_link_libraries(test_relativetrajectory
 	${MAESTRO_COMMON_TARGET}
 )
+add_executable(test_reversetrajectory tests/test_reversetrajectory.cpp)
+add_test(reverse_trajectory_test
+	${CMAKE_RUNTIME_OUTPUT_DIRECTORY}/test_reversetrajectory)
+target_link_libraries(test_reversetrajectory
+	${MAESTRO_COMMON_TARGET}
+)
 # Installation rules
 install(CODE "MESSAGE(STATUS \"Installing target ${MAESTRO_UTIL_TARGET}\")")
 install(TARGETS ${MAESTRO_UTIL_TARGET}
diff --git a/common/tests/test_reversetrajectory.cpp b/common/tests/test_reversetrajectory.cpp
new file mode 100644
index 000000000..ec85b96a5
--- /dev/null
+++ b/common/tests/test_reversetrajectory.cpp
@@ -0,0 +1,41 @@
+#include "../trajectory.hpp"
+#include <cstdlib>
+
+static void test_double_reverse();
+
+int main(int argc, char** argv) {
+	try {
+		test_double_reverse();
+		exit(EXIT_SUCCESS);
+	} catch (std::runtime_error& e) {
+		std::cerr << "Test " << __FILE__ << " failed: " << std::endl
+				  << e.what() << std::endl;
+		exit(EXIT_FAILURE);
+	}
+}
+
+void test_double_reverse() {
+
+	Trajectory t;
+	srand(1234);
+	for (int i = 0; i < 250; ++i) {
+		Trajectory::TrajectoryPoint p;
+		p.setTime(i*0.1);
+		p.setXCoord(rand() % 100 - 50);
+		p.setYCoord(rand() % 100 - 50);
+		p.setZCoord(rand() % 100 - 50);
+		p.setHeading(rand() % 100 - 5*M_PI);
+		p.setCurvature(rand() % 100 - 50);
+		p.setLongitudinalVelocity(rand() % 100 - 50);
+		p.setLongitudinalAcceleration(rand() % 100 - 50);
+		p.setLateralVelocity(rand() % 100 - 50);
+		p.setLateralAcceleration(rand() % 100 - 50);
+
+		t.points.push_back(p);
+	}
+
+	if (t == t.reversed().reversed()) {
+		return;
+	}
+	throw std::runtime_error("Reversing twice does not produce the original trajectory");
+}

From d74f7bd3949069b21d5146809feff94e37aa9bed Mon Sep 17 00:00:00 2001
From: Lukas Wikander <lukas.wikander@astazero.com>
Date: Thu, 9 Sep 2021 10:31:47 +0200
Subject: [PATCH 2/2] Added == operators for Trajectory and TrajectoryPoint

---
 common/trajectory.cpp | 17 +++++++++++++++++
 common/trajectory.hpp |  4 ++++
 2 files changed, 21 insertions(+)

diff --git a/common/trajectory.cpp b/common/trajectory.cpp
index 37b83bcc5..2f204344a 100644
--- a/common/trajectory.cpp
+++ b/common/trajectory.cpp
@@ -213,6 +213,23 @@ Trajectory::TrajectoryPoint Trajectory::TrajectoryPoint::relativeTo(
 	return relative;
 }
 
+
+bool Trajectory::TrajectoryPoint::operator==(
+		const TrajectoryPoint &other) const {
+	return getMode() == other.getMode()
+			&& abs(getTime() - other.getTime()) < 1e-6
+			&& (getPosition() - other.getPosition()).norm() < 1e-6
+			&& (getVelocity() - other.getVelocity()).norm() < 1e-4
+			&& (getAcceleration() - other.getAcceleration()).norm() < 1e-2
+			&& abs(getHeading() - other.getHeading()) < 1e-4
+			&& abs(getCurvature() - other.getCurvature()) < 1e-4;
+}
+
+bool Trajectory::operator==(
+		const Trajectory &other) const {
+	return points == other.points && id == other.id;
+}
+
 Trajectory Trajectory::relativeTo(
 		const Trajectory &other) const {
 	using namespace Eigen;
diff --git a/common/trajectory.hpp b/common/trajectory.hpp
index 42bbbc748..72c43e614 100644
--- a/common/trajectory.hpp
+++ b/common/trajectory.hpp
@@ -94,6 +94,8 @@ class Trajectory {
 
 		std::string toString() const;
 		std::string getFormatString() const;
+
+		bool operator==(const TrajectoryPoint& other) const;
     private:
         double time = 0;
 		Eigen::Vector3d position; //! x, y, z [m]
@@ -134,6 +136,8 @@ class Trajectory {
 	Trajectory reversed() const;
 	Trajectory rescaledToVelocity(const double vel_m_s) const;
 
+	bool operator==(const Trajectory& other) const;
+
 private:
 	static const std::regex fileHeaderPattern;
 	static const std::regex fileLinePattern;