From 32d5435affa15f740b44508f12ed13e2382fa472 Mon Sep 17 00:00:00 2001
From: Chi Huu Huynh <73843190+Chi-EEE@users.noreply.github.com>
Date: Wed, 6 Dec 2023 17:59:13 +0000
Subject: [PATCH] Make initialize and start consistent

---
 raspberry_pi/src/car/display/CarConsole.hpp      |  2 ++
 raspberry_pi/src/car/system/CarSystem.cpp        |  2 +-
 .../src/car/system/lidar/LidarDevice.hpp         |  1 +
 raspberry_pi/src/car/system/lidar/LidarDummy.hpp |  2 ++
 .../src/car/system/lidar/LidarScanner.hpp        | 16 ++++++++--------
 .../src/car/system/messaging/MessagingSystem.hpp |  1 -
 6 files changed, 14 insertions(+), 10 deletions(-)

diff --git a/raspberry_pi/src/car/display/CarConsole.hpp b/raspberry_pi/src/car/display/CarConsole.hpp
index 9b12ce47..21639dfb 100644
--- a/raspberry_pi/src/car/display/CarConsole.hpp
+++ b/raspberry_pi/src/car/display/CarConsole.hpp
@@ -78,6 +78,8 @@ namespace car::display {
 
 			Loop loop(&screen, main_component);
 
+			this->car_system->initalize();
+			this->car_system->start();
 			// The main loop:
 			while (!loop.HasQuitted()) {
 				this->car_system->update();
diff --git a/raspberry_pi/src/car/system/CarSystem.cpp b/raspberry_pi/src/car/system/CarSystem.cpp
index ac895bc1..3afea40c 100644
--- a/raspberry_pi/src/car/system/CarSystem.cpp
+++ b/raspberry_pi/src/car/system/CarSystem.cpp
@@ -3,7 +3,6 @@
 namespace car::system {
 	CarSystem::CarSystem(const std::string& websocket_url, std::unique_ptr<LidarDevice> lidar_device, std::unique_ptr<MessagingSystem> messaging_system) : lidar_device(std::move(lidar_device)), messaging_system(std::move(messaging_system))
 	{
-		this->initalize();
 	}
 
 	CarSystem::~CarSystem()
@@ -14,6 +13,7 @@ namespace car::system {
 	void CarSystem::initalize()
 	{
 		this->messaging_system->initalize();
+		this->lidar_device->initialize();
 
 		this->messaging_system->move_command_signal.connect([this](MoveCommand move_command) {
 			this->move(move_command);
diff --git a/raspberry_pi/src/car/system/lidar/LidarDevice.hpp b/raspberry_pi/src/car/system/lidar/LidarDevice.hpp
index dd035787..e72f1612 100644
--- a/raspberry_pi/src/car/system/lidar/LidarDevice.hpp
+++ b/raspberry_pi/src/car/system/lidar/LidarDevice.hpp
@@ -13,6 +13,7 @@ namespace car::system::lidar {
 	class LidarDevice
 	{
 	public:
+		virtual void initialize() const = 0;
 		virtual void start() const = 0;
 		virtual std::vector<Measure> scan() const = 0;
 		virtual void terminate() const = 0;
diff --git a/raspberry_pi/src/car/system/lidar/LidarDummy.hpp b/raspberry_pi/src/car/system/lidar/LidarDummy.hpp
index ae7fe4b7..63c71078 100644
--- a/raspberry_pi/src/car/system/lidar/LidarDummy.hpp
+++ b/raspberry_pi/src/car/system/lidar/LidarDummy.hpp
@@ -15,6 +15,8 @@ namespace car::system::lidar {
 		{
 		};
 
+		void initialize() const override {};
+
 		void start() const override {};
 
 		std::vector<Measure> scan() const override
diff --git a/raspberry_pi/src/car/system/lidar/LidarScanner.hpp b/raspberry_pi/src/car/system/lidar/LidarScanner.hpp
index d7e393c7..6ad0d677 100644
--- a/raspberry_pi/src/car/system/lidar/LidarScanner.hpp
+++ b/raspberry_pi/src/car/system/lidar/LidarScanner.hpp
@@ -18,13 +18,20 @@ namespace car::system::lidar
 	public:
 		LidarScanner(const std::string &lidar_port) : lidar(RPLidar::create(lidar_port).value())
 		{
-			this->initialize();
 		};
+
 		~LidarScanner()
 		{
 			this->terminate();
 		};
 
+		void initialize() const
+		{
+			this->lidar->reset();
+			this->lidar->stop();
+			this->lidar->stop_motor();
+		};
+
 		void start() const
 		{
 			this->lidar->start_motor();
@@ -59,13 +66,6 @@ namespace car::system::lidar
 		}
 
 	private:
-		void initialize() const
-		{
-			this->lidar->reset();
-			this->lidar->stop();
-			this->lidar->stop_motor();
-		};
-
 		std::unique_ptr<RPLidar> lidar;
 	};
 }
diff --git a/raspberry_pi/src/car/system/messaging/MessagingSystem.hpp b/raspberry_pi/src/car/system/messaging/MessagingSystem.hpp
index febf6a66..afce6bd4 100644
--- a/raspberry_pi/src/car/system/messaging/MessagingSystem.hpp
+++ b/raspberry_pi/src/car/system/messaging/MessagingSystem.hpp
@@ -27,7 +27,6 @@ namespace car::system::messaging {
 	{
 	public:
 		MessagingSystem(const std::string& websocket_url) : websocket_url(websocket_url) {
-
 		};
 
 		void initalize()