diff --git a/.gitignore b/.gitignore
index 11e586d4..5585c1dc 100644
--- a/.gitignore
+++ b/.gitignore
@@ -199,4 +199,14 @@ TODO.md
 
 **/nul
 
+# Ignore Claude Code configuration
 .claude/
+
+
+# Ignore large SUMO output files
+fcd.xml
+
+
+# Ignore large assets managed via download script
+tests/Applications/SUMO_CARLA_EcoDriving/MLK_Carla_Scenario/CARLAFiles/MLK_noped1002_final_debug.fbx
+
diff --git a/CommonLib/ConfigHelper.cpp b/CommonLib/ConfigHelper.cpp
index f9a4bbb7..e26fd99c 100644
--- a/CommonLib/ConfigHelper.cpp
+++ b/CommonLib/ConfigHelper.cpp
@@ -1,41 +1,7 @@
 #include "ConfigHelper.h"
-#include <windows.h>
-#include <shlwapi.h>
-#pragma comment(lib, "shlwapi.lib")
 
 using namespace std;
 
-// Helper functions for path operations (C++14 compatible replacement for std::filesystem)
-namespace {
-	std::string getParentPath(const std::string& path) {
-		size_t pos = path.find_last_of("\\/");
-		return (pos != std::string::npos) ? path.substr(0, pos) : "";
-	}
-
-	std::string getCurrentDirectory() {
-		char buffer[MAX_PATH];
-		GetCurrentDirectoryA(MAX_PATH, buffer);
-		return std::string(buffer);
-	}
-
-	bool isRelativePath(const std::string& path) {
-		if (path.empty()) return true;
-		// Check for drive letter (e.g., "C:")
-		if (path.length() > 1 && path[1] == ':') return false;
-		// Check for UNC path or root path
-		if (path[0] == '\\' || path[0] == '/') return false;
-		return true;
-	}
-
-	std::string combinePaths(const std::string& base, const std::string& relative) {
-		char combined[MAX_PATH];
-		PathCombineA(combined, base.c_str(), relative.c_str());
-		char normalized[MAX_PATH];
-		PathCanonicalizeA(normalized, combined);
-		return std::string(normalized);
-	}
-}
-
 
 // Value-Defintions of the different String values
 enum TypeNames_enum {
@@ -123,12 +89,6 @@ int ConfigHelper::getConfig(string configName) {
 	// }
 #endif
 
-	// Store the config file's directory for resolving relative paths
-	std::string configDir = getParentPath(configName);
-	if (configDir.empty()) {
-		configDir = getCurrentDirectory();
-	}
-
 	YAML::Node node;
 
 	YAML::Node config = YAML::LoadFile(configName);
@@ -142,14 +102,14 @@ int ConfigHelper::getConfig(string configName) {
 	}
 	else {
 		SimulationSetup.EnableRealSim = true;
-		if (!SuppressDefaultMessages) printf("\nWill enable real sim as default!\n");
+		printf("\nWill enable real sim as default!\n");
 	}
 	if (node["EnableVerboseLog"]) {
 		SimulationSetup.EnableVerboseLog = parserFlag(node, "EnableVerboseLog");
 	}
 	else {
 		SimulationSetup.EnableVerboseLog = false;
-		if (!SuppressDefaultMessages) printf("\nWill disable verbose log as default!\n");
+		printf("\nWill disable verbose log as default!\n");
 	}
 	if (node["SimulationEndTime"]) {
 		SimulationSetup.SimulationEndTime = parserDouble(node, "SimulationEndTime");
@@ -170,42 +130,42 @@ int ConfigHelper::getConfig(string configName) {
 	}
 	else {
 		SimulationSetup.SelectedTrafficSimulator = "SUMO";
-		if (!SuppressDefaultMessages) printf("\nTraffic Simulator not specified! Will use SUMO as default!\n");
+		printf("\nTraffic Simulator not specified! Will use SUMO as default!\n");
 	}
 	if (node["TrafficSimulatorIP"]) {
 		SimulationSetup.TrafficSimulatorIP = parserString(node, "TrafficSimulatorIP");
 	}
 	else {
 		SimulationSetup.TrafficSimulatorIP = "127.0.0.1";
-		if (!SuppressDefaultMessages) printf("\nTraffic Simulator IP not specified! Will use localhost 127.0.0.1 as default!\n");
+		printf("\nTraffic Simulator IP not specified! Will use localhost 127.0.0.1 as default!\n");
 	}
 	if (node["TrafficSimulatorPort"]) {
 		SimulationSetup.TrafficSimulatorPort = parserInteger(node, "TrafficSimulatorPort");
 	}
 	else {
 		SimulationSetup.TrafficSimulatorPort = 1337;
-		if (!SuppressDefaultMessages) printf("\nTraffic Simulator Port not specified! Will use 1337 as default!\n");
+		printf("\nTraffic Simulator Port not specified! Will use 1337 as default!\n");
 	}
 	if (node["SimulationMode"]) {
 		SimulationSetup.SimulationMode = parserInteger(node, "SimulationMode");
 	}
 	else {
 		SimulationSetup.SimulationMode = 0;
-		if (!SuppressDefaultMessages) printf("\nSimulation mode not specified! Will use Mode 0 as default!\n");
-	}
+		printf("\nSimulation mode not specified! Will use Mode 0 as default!\n");
+}
 	if (node["SimulationModeParameter"]) {
 		SimulationSetup.SimulationModeParameter = parserDouble(node, "SimulationModeParameter");
 	}
 	else {
 		SimulationSetup.SimulationModeParameter = 0;
-		if (!SuppressDefaultMessages) printf("\nSimulation mode parameter not specified! Will use Parameter=0 as default!\n");
+		printf("\nSimulation mode parameter not specified! Will use Parameter=0 as default!\n");
 	}
 	if (node["VehicleMessageField"]) {
 		parserStringVector(node, "VehicleMessageField", SimulationSetup.VehicleMessageField);
 	}
 	else {
 		SimulationSetup.VehicleMessageField = { "id", "type", "speed", "positionX", "positionY", "positionZ", "heading", "color", "linkId", "laneId", "distanceTravel", "acceleration", "speedDesired", "acceleartionDesired", "hasPrecedingVehicle", "precedingVehicleId", "precedingVehicleDistance", "precedingVehicleSpeed", "signalLightDistance", "signalLightColor", "speedLimit", "speedLimitNext", "speedLimitChangeDistance", "linkIdNext", "grade" , "activeLaneChange", "signalLightId", "signalLightHeadId", "lightIndicators"};
-		if (!SuppressDefaultMessages) printf("\nWill use all available vehicle message field!\n");
+		printf("\nWill use all available vehicle message field!\n");
 	}
 
 
@@ -214,24 +174,17 @@ int ConfigHelper::getConfig(string configName) {
 	//  Store Vehicle Message Field to an unordered_set
 	// -------------------
 	std::unordered_set <std::string> VehicleMessageField_set;
-	if (!SuppressDefaultMessages) {
-		printf("Vehicle message selected:");
-		for (size_t i = 0; i < SimulationSetup.VehicleMessageField.size(); i++) {
-			VehicleMessageField_set.insert(SimulationSetup.VehicleMessageField[i]);
-			printf("\t%s", SimulationSetup.VehicleMessageField[i].c_str());
-		}
-		printf("\n\n");
-	}
-	else {
-		for (size_t i = 0; i < SimulationSetup.VehicleMessageField.size(); i++) {
-			VehicleMessageField_set.insert(SimulationSetup.VehicleMessageField[i]);
-		}
+	printf("Vehicle message selected:");
+	for (size_t i = 0; i < SimulationSetup.VehicleMessageField.size(); i++) {
+		VehicleMessageField_set.insert(SimulationSetup.VehicleMessageField[i]);
+		printf("\t%s", SimulationSetup.VehicleMessageField[i].c_str());
 	}
 
 	// -------------------
 	//  SANITY CHECK: Vehicle Message Field
 	// -------------------
 	// currently, mandate messages are id, speed, one of speedDesired or accelerationDesired
+	printf("\n\n");
 	if (VehicleMessageField_set.find("id") == VehicleMessageField_set.end()) {
 		printf("ERROR: Must select 'id' as part of VehicleMessageField\n");
 		exit(-1);
@@ -270,7 +223,7 @@ int ConfigHelper::getConfig(string configName) {
 		XilSetup.AsServer = parserFlag(node, "AsServer");
 	}else{
 		XilSetup.AsServer = false;
-		if (!SuppressDefaultMessages) printf("\nXil not specified as server or client! Will set Xil as client!\n");
+		printf("\nXil not specified as server or client! Will set Xil as client!\n");
 	}
 
 	parserSubscription(node, "VehicleSubscription", XilSetup.VehicleSubscription);
@@ -437,38 +390,58 @@ int ConfigHelper::getConfig(string configName) {
 	}
 	else {
 		SumoSetup.ExecutionOrder = 1;
-		if (!SuppressDefaultMessages) printf("\nSumo Execution Order not specified! Will use 1 as default!\n");
+		printf("\nSumo Execution Order not specified! Will use 1 as default!\n");
 	}
-	if (node["EnableAutoLaunch"]) {
-		SumoSetup.EnableAutoLaunch = parserFlag(node, "EnableAutoLaunch");
+	if (node["EgoID"]) {
+		SumoSetup.EgoID = node["EgoID"].as<std::string>();
 	}
 	else {
-		SumoSetup.EnableAutoLaunch = false;
+		SumoSetup.EgoID = "ego";
+		printf("\nSumo EgoID not specified! Will use 'ego' as default!\n");
+	}
+	if (node["EgoInsertTime"]) {
+		SumoSetup.EgoInsertTime = node["EgoInsertTime"].as<double>();
+	}
+	else {
+		SumoSetup.EgoInsertTime = 28985.0;
+		printf("\nSumo EgoInsertTime not specified! Will use 28985.0 as default!\n");
 	}
-	if (node["SumoConfigFile"]) {
-		SumoSetup.SumoConfigFile = parserString(node, "SumoConfigFile");
 
-		// Convert relative path to absolute path (relative to config file directory)
-		if (!SumoSetup.SumoConfigFile.empty() && isRelativePath(SumoSetup.SumoConfigFile)) {
-			SumoSetup.SumoConfigFile = combinePaths(configDir, SumoSetup.SumoConfigFile);
-		}
+	// ---- Ego depart parameters (TraCI vehicle.add) ----
+	if (node["EgoDepart"]) {
+		SumoSetup.EgoDepart = parserString(node, "EgoDepart");
 	}
 	else {
-		SumoSetup.SumoConfigFile = "";
+		SumoSetup.EgoDepart = "now";
+		printf("\nSumo EgoDepart not specified! Will use 'now' as default!\n");
 	}
-	if (node["NumClients"]) {
-		SumoSetup.NumClients = parserInteger(node, "NumClients");
+
+	if (node["EgoDepartLane"]) {
+		SumoSetup.EgoDepartLane = parserString(node, "EgoDepartLane");
 	}
 	else {
-		SumoSetup.NumClients = 1;
+		SumoSetup.EgoDepartLane = "0";
+		printf("\nSumo EgoDepartLane not specified! Will use '0' as default!\n");
 	}
-	if (node["RuntimeLibraryPath"]) {
-		SumoSetup.RuntimeLibraryPath = parserString(node, "RuntimeLibraryPath");
+
+	if (node["EgoDepartPos"]) {
+		SumoSetup.EgoDepartPos = parserString(node, "EgoDepartPos");
 	}
 	else {
-		SumoSetup.RuntimeLibraryPath = "";
+		SumoSetup.EgoDepartPos = "1";
+		printf("\nSumo EgoDepartPos not specified! Will use '1' as default!\n");
 	}
 
+	if (node["EgoDepartSpeed"]) {
+		SumoSetup.EgoDepartSpeed = parserString(node, "EgoDepartSpeed");
+	}
+	else {
+		SumoSetup.EgoDepartSpeed = "0.1";
+		printf("\nSumo EgoDepartSpeed not specified! Will use '0.1' as default!\n");
+	}
+
+
+
 
 	// ===========================================================================
 	// 			READ Carla Setup section
@@ -479,7 +452,7 @@ int ConfigHelper::getConfig(string configName) {
 	}
 	else {
 		CarlaSetup.EnableVerboseLog = false;
-		if (!SuppressDefaultMessages) printf("\nWill disable verbose log as default!\n");
+		printf("\nWill disable verbose log as default!\n");
 	}
 	if (node["EnableCosimulation"]) {
 		CarlaSetup.EnableCosimulation = parserFlag(node, "EnableCosimulation");
@@ -488,31 +461,11 @@ int ConfigHelper::getConfig(string configName) {
 		CarlaSetup.EnableCosimulation = false;
 	}
 
-	//if (node["EnableEgoSimulink"]) {
-	//	CarlaSetup.EnableEgoSimulink = parserFlag(node, "EnableEgoSimulink");
-	//}
-	//else {
-	//	CarlaSetup.EnableEgoSimulink = false;
-	//}
-	if (node["EnableExternalControl"]) {
-		CarlaSetup.EnableExternalControl = parserFlag(node, "EnableExternalControl");
-	}
-	else {
-		CarlaSetup.EnableExternalControl = false;
-	}
-	if (node["UseVehicleTypeAsBlueprint"]) {
-		CarlaSetup.UseVehicleTypeAsBlueprint = parserFlag(node, "UseVehicleTypeAsBlueprint");
-	}
-	else {
-		CarlaSetup.UseVehicleTypeAsBlueprint = false;
-	}
-
-	if (node["CenteredViewId"]) {
-		CarlaSetup.CenteredViewId = parserString(node, "CenteredViewId");
+	if (node["EnableEgoSimulink"]) {
+		CarlaSetup.EnableEgoSimulink = parserFlag(node, "EnableEgoSimulink");
 	}
 	else {
-		CarlaSetup.CenteredViewId = "ego";
-		if (!SuppressDefaultMessages) printf("\nCentered View Id not specified! Will use ego as default!\n");
+		CarlaSetup.EnableEgoSimulink = false;
 	}
 
 	if (node["CarlaServerIP"]) {
@@ -567,7 +520,7 @@ int ConfigHelper::getConfig(string configName) {
 	}
 	else {
 		CarlaSetup.CarlaMapName = "Town01";
-		if (!SuppressDefaultMessages) printf("\nCarla Map not specified! Will use Town01 as default!\n");
+		printf("\nCarla Map not specified! Will use Town01 as default!\n");
 	}
 	if (node["TrafficRefreshRate"]) {
 		CarlaSetup.TrafficRefreshRate = parserDouble(node, "TrafficRefreshRate");
@@ -582,7 +535,7 @@ int ConfigHelper::getConfig(string configName) {
 	}
 	else {
 		CarlaSetup.InterestedIds = {"ego"};
-		if (!SuppressDefaultMessages) printf("\nDefault to track actor with id ego\n");
+		printf("\nDefault to track actor with id ego\n");
 	}
 
 	return 0;
@@ -756,7 +709,7 @@ void ConfigHelper::parserSubscription(YAML::Node rootnode, std::string name, Sub
 				break;
 
 			case intersection:
-				if (att.compare("id") == 0 || att.compare("name") == 0) {
+				if (att.compare("id") == 0 || att.compare("name") == 0 || att.compare("all") == 0) {
 					extractSubscriptionAttributes(attnode, type, att, attMap);
 				}
 				else {
@@ -1009,6 +962,8 @@ void ConfigHelper::getVehSubscriptionList(Subscription_t VehSub, std::unordered_
 }
 
 void ConfigHelper::getSigSubscriptionList(Subscription_t SigSub) {
+	SubscriptionSignalList.signalId_v.clear();
+	SubscriptionSignalList.subAllSignalFlag = false;
 
 	// type, attribute, ip, port
 	// string, SubAttMap_t, vector <string>, vector <int>
@@ -1018,52 +973,72 @@ void ConfigHelper::getSigSubscriptionList(Subscription_t SigSub) {
 		vector <string> idlist;
 		if (type.compare("intersection") == 0) {
 			SubAttMap_t att = get<1>(SigSub[iSub]);
+
+			bool subAll = false;
+			if (att.find("all") != att.end() && !att["all"].empty()) {
+				string v = att["all"][0];
+				if (v == "true" || v == "True" || v == "TRUE" || v == "1") {
+					subAll = true;
+				}
+			}
+
+			if (subAll) {
+				SubscriptionSignalList.subAllSignalFlag = true;
+			}
+
+			vector<string> idlist;
+
 			if (att.find("name") != att.end()) {
 				idlist = att["name"];
-				for (size_t i = 0; i < idlist.size(); i++) {
-					if (SubscriptionSignalList.signalId_v.find(idlist[i]) == SubscriptionSignalList.signalId_v.end()) {
-						SubscriptionSignalList.signalId_v.insert(idlist[i]);
-					}
+			}
+			else if (att.find("id") != att.end()) {
+				idlist = att["id"];
+			}
+
+
+			for (size_t i = 0; i < idlist.size(); i++) {
+				if (SubscriptionSignalList.signalId_v.find(idlist[i]) == SubscriptionSignalList.signalId_v.end()) {
+					SubscriptionSignalList.signalId_v.insert(idlist[i]);
 				}
 			}
 
+
 			// get port map
 			vector <int> port_v;
 			port_v = get<3>(SigSub[iSub]);
 
-			for (auto it : port_v) {
-				// if already has this socket port, then no need to initialize
-				if (SocketPort2SubscriptionList_um.find(it) != SocketPort2SubscriptionList_um.end()) {
+			for (auto port : port_v) {
 
-				}
-				else {
+
+				if (SocketPort2SubscriptionList_um.find(port) == SocketPort2SubscriptionList_um.end()) {
 					SubscriptionAllList_t subAllList;
-					SocketPort2SubscriptionList_um[it] = subAllList;
+					SocketPort2SubscriptionList_um[port] = subAllList;
 				}
 
+
+				if (subAll) {
+					SocketPort2SubscriptionList_um[port].SignalList.subAllSignalFlag = true;
+				}
+
+
 				for (size_t i = 0; i < idlist.size(); i++) {
-					if (SocketPort2SubscriptionList_um[it].SignalList.signalId_v.find(idlist[i]) == SocketPort2SubscriptionList_um[it].SignalList.signalId_v.end()) {
-						SocketPort2SubscriptionList_um[it].SignalList.signalId_v.insert(idlist[i]);
+					if (SocketPort2SubscriptionList_um[port].SignalList.signalId_v.find(idlist[i]) ==
+						SocketPort2SubscriptionList_um[port].SignalList.signalId_v.end()) {
+						SocketPort2SubscriptionList_um[port].SignalList.signalId_v.insert(idlist[i]);
 					}
 				}
 			}
-
 		}
 		else {
 			// ERROR HANDLING
-
 		}
-
-		
 	}
 
-	////if (CarMakerSetup.EnableCosimulation && CarMakerSetup.SynchronizeTrafficSignal){
-	//if (CarMakerSetup.SynchronizeTrafficSignal) {
-	//	SubscriptionSignalList.subAllSignalFlag = true;
-	//}
-	//else {
-	//	SubscriptionSignalList.subAllSignalFlag = false;
-	//}
+	if (SubscriptionSignalList.subAllSignalFlag) {
+		for (auto& kv : SocketPort2SubscriptionList_um) {
+			kv.second.SignalList.subAllSignalFlag = true;
+		}
+	}
 }
 
 void ConfigHelper::getDetSubscriptionList(Subscription_t DetSub) {
diff --git a/CommonLib/ConfigHelper.h b/CommonLib/ConfigHelper.h
index 54e9cb87..556bbc04 100644
--- a/CommonLib/ConfigHelper.h
+++ b/CommonLib/ConfigHelper.h
@@ -114,9 +114,7 @@ struct CarlaSetup_t {
 
 	bool EnableCosimulation;
 
-	bool EnableExternalControl;
-
-	bool UseVehicleTypeAsBlueprint;
+	bool EnableEgoSimulink;
 
 	std::string CarlaServerIP;
 
@@ -128,8 +126,6 @@ struct CarlaSetup_t {
 
 	std::string CarlaMapName;
 
-	std::string CenteredViewId;
-
 	double TrafficRefreshRate;
 
 	std::vector<std::string> InterestedIds;
@@ -140,12 +136,13 @@ struct SumoSetup_t {
 
 	int SpeedMode;
 	int ExecutionOrder;
+	std::string EgoID;
+	double EgoInsertTime;
 
-	// Auto-launch SUMO configuration
-	bool EnableAutoLaunch;
-	std::string SumoConfigFile;
-	int NumClients;
-	std::string RuntimeLibraryPath;
+	std::string EgoDepart;
+	std::string EgoDepartLane;
+	std::string EgoDepartPos;
+	std::string EgoDepartSpeed;
 };
 
 typedef struct SubscriptionVehicleList_t {
@@ -188,9 +185,6 @@ class ConfigHelper
 
 	int getConfig(std::string configName);
 
-	// Flag to suppress "Will use X as default" messages during config parsing
-	bool SuppressDefaultMessages = true;
-
 	bool parserFlag(YAML::Node node, std::string name);
 	std::string parserString(YAML::Node node, std::string name);
 	double parserDouble(YAML::Node node, std::string name);
diff --git a/CommonLib/MsgHelper.py b/CommonLib/MsgHelper.py
index 66de1629..a0994501 100644
--- a/CommonLib/MsgHelper.py
+++ b/CommonLib/MsgHelper.py
@@ -1,6 +1,6 @@
 from re import M
 from typing import List
-
+from struct import unpack
 from numpy import byte
 from CommonLib.VehDataMsgDefs import VehData, TrafficLightData, DetectorData
 import struct
@@ -11,8 +11,7 @@ class MessageType:
     traffic_light_data = 2
     detector_data = 3
 
-#   VehicleMessageField: [id, type, vehicleClass, speed, acceleration, positionX, positionY, positionZ, heading,
-#     color, linkId, laneId, distanceTravel, speedDesired, grade, length, width, height]
+
 
 class MsgHelper:
     
@@ -219,6 +218,50 @@ def depack_veh_data(self, byte_data: bytes)-> VehData:
             veh_data.height, byte_index = MsgHelper.unpack_float(byte_data, byte_index)
             
         return  veh_data
+    
+
+    def calc_veh_msg_size_bytes(self, veh_data: VehData) -> int:
+        """
+        calculate the Msgsize of single vehicle information
+        """
+        msg_size = (
+            round(self.vehicle_msg_field_valid.get('id', 0) * (1 + len(veh_data.id))
+                + self.vehicle_msg_field_valid.get('type', 0) * (1 + len(veh_data.type))
+                + self.vehicle_msg_field_valid.get('vehicleClass', 0) * (1 + len(veh_data.vehicleClass))
+                + self.vehicle_msg_field_valid.get('speed', 0) * 4
+                + self.vehicle_msg_field_valid.get('acceleration', 0) * 4
+                + self.vehicle_msg_field_valid.get('positionX', 0) * 4
+                + self.vehicle_msg_field_valid.get('positionY', 0) * 4
+                + self.vehicle_msg_field_valid.get('positionZ', 0) * 4
+                + self.vehicle_msg_field_valid.get('heading', 0) * 4
+                + self.vehicle_msg_field_valid.get('color', 0) * 4
+                + self.vehicle_msg_field_valid.get('linkId', 0) * (1 + len(veh_data.linkId))
+                + self.vehicle_msg_field_valid.get('laneId', 0) * 4
+                + self.vehicle_msg_field_valid.get('distanceTravel', 0) * 4
+                + self.vehicle_msg_field_valid.get('speedDesired', 0) * 4
+                + self.vehicle_msg_field_valid.get('accelerationDesired', 0) * 4
+                + self.vehicle_msg_field_valid.get('hasPrecedingVehicle', 0) * 1
+                + self.vehicle_msg_field_valid.get('precedingVehicleId', 0) * (1 + len(veh_data.precedingVehicleId))
+                + self.vehicle_msg_field_valid.get('precedingVehicleDistance', 0) * 4
+                + self.vehicle_msg_field_valid.get('precedingVehicleSpeed', 0) * 4
+                + self.vehicle_msg_field_valid.get('signalLightId', 0) * (1 + len(veh_data.signalLightId))
+                + self.vehicle_msg_field_valid.get('signalLightHeadId', 0) * 4
+                + self.vehicle_msg_field_valid.get('signalLightDistance', 0) * 4
+                + self.vehicle_msg_field_valid.get('signalLightColor', 0) * 1
+                + self.vehicle_msg_field_valid.get('speedLimit', 0) * 4
+                + self.vehicle_msg_field_valid.get('speedLimitNext', 0) * 4
+                + self.vehicle_msg_field_valid.get('speedLimitChangeDistance', 0) * 4
+                + self.vehicle_msg_field_valid.get('linkIdNext', 0) * (1 + len(veh_data.linkIdNext))
+                + self.vehicle_msg_field_valid.get('grade', 0) * 4
+                + self.vehicle_msg_field_valid.get('activeLaneChange', 0) * 1
+                + self.vehicle_msg_field_valid.get('length', 0) * 4
+                + self.vehicle_msg_field_valid.get('width', 0) * 4
+                + self.vehicle_msg_field_valid.get('height', 0) * 4)
+        )
+        veh_msg_size = round(msg_size) + self.msg_each_header_size  # each-header=3 bytes
+        return int(veh_msg_size)
+
+
 
     def pack_veh_data(self, byte_data: bytearray, byte_index, veh_data: VehData):
         # Calculate nMsgSize based on vehicle_msg_field_valid flags and veh_data field lengths
@@ -393,19 +436,86 @@ def pack_veh_data(self, byte_data: bytearray, byte_index, veh_data: VehData):
         
         return byte_data, msg_size, byte_index
 
-    def depack_traffic_light_data(self, byte_data: bytes)-> TrafficLightData:
-        traffic_light_data = TrafficLightData()
-        byte_index = 0
-        if self.traffic_light_msg_field_valid.get('id'):
-            traffic_light_data.id, byte_index = MsgHelper.unpack_uint32(byte_data, byte_index)
-        if self.traffic_light_msg_field_valid.get('name'):
-            str_data, str_len, byte_index, uint8Arr = MsgHelper.depack_string(byte_data, byte_index)
-            traffic_light_data.name = str_data
-        if self.traffic_light_msg_field_valid.get('state'):
-            str_data, str_len, byte_index, uint8Arr = MsgHelper.depack_string(byte_data, byte_index)
-            traffic_light_data.state = str_data
+    # def depack_traffic_light_data(self, byte_data: bytes)-> TrafficLightData:
+    #     traffic_light_data = TrafficLightData()
+    #     byte_index = 0
+    #     if self.traffic_light_msg_field_valid.get('id'):
+    #         traffic_light_data.id, byte_index = MsgHelper.unpack_uint32(byte_data, byte_index)
+    #     if self.traffic_light_msg_field_valid.get('name'):
+    #         str_data, str_len, byte_index, uint8Arr = MsgHelper.depack_string(byte_data, byte_index)
+    #         traffic_light_data.name = str_data
+    #     if self.traffic_light_msg_field_valid.get('state'):
+    #         str_data, str_len, byte_index, uint8Arr = MsgHelper.depack_string(byte_data, byte_index)
+    #         traffic_light_data.state = str_data
+
+    #     return traffic_light_data
+
+
+
+
+    import struct
+
+    def depack_traffic_light_data(self, byte_data: bytes):
+        """
+        byte_data 是 payload（已经去掉了 msgSize(uint16)+identifier(uint8) 之后的部分）
+        C++ payload: [nameLen][name][id(uint16)][stateLen][state]
+        """
+        idx = 0
+
+        # nameLen
+        if len(byte_data) < 1:
+            return TrafficLightData(id="UNKNOWN", name="", state="")
+
+        name_len = byte_data[idx]
+        idx += 1
+
+        # name
+        if len(byte_data) < idx + name_len:
+            return TrafficLightData(id="UNKNOWN", name="", state="")
+
+        name = byte_data[idx:idx + name_len].decode("ascii", errors="ignore")
+        idx += name_len
+
+        # id (uint16 little-endian)
+        if len(byte_data) < idx + 2:
+            return TrafficLightData(id="UNKNOWN", name=name, state="")
+
+        (tls_id_u16,) = struct.unpack_from("<H", byte_data, idx)
+        idx += 2
+
+        # stateLen
+        if len(byte_data) < idx + 1:
+            return TrafficLightData(id=str(tls_id_u16), name=name, state="")
+
+        state_len = byte_data[idx]
+        idx += 1
+
+        # state
+        if len(byte_data) < idx + state_len:
+            state_bytes = byte_data[idx:]
+        else:
+            state_bytes = byte_data[idx:idx + state_len]
+
+        state = state_bytes.decode("ascii", errors="ignore")
+
+
+        tls = TrafficLightData(
+            id=str(tls_id_u16),   
+            name=name,
+            state=state
+        )
+
+        try:
+            tls.id_u16 = tls_id_u16
+            tls.state_len = state_len
+            tls.name_len = name_len
+        except Exception:
+            pass
+
+        return tls
+
+
 
-        return traffic_light_data
     
     def pack_traffic_light_data(self, byte_data: bytearray, byte_index, traffic_light_data: TrafficLightData):
         # Calculate nMsgSize based on traffic_light_msg_field_valid flags and traffic_light_data field lengths
@@ -520,12 +630,14 @@ def depack_string(byte_data: bytearray, byte_index: int):
     
         # Read the length of the string
         strLen = struct.unpack('B', byte_data[byte_index:byte_index+1])[0]
+        if strLen > 50:
+            raise ValueError("Max characters of id, linkId, type, precedingVehicleId, linkIdNext must be smaller than 50")
 
         byte_index += 1
 
         # Initialize a string of size 50 and a uint8 array of size 50
-        str_data = [''] * strLen
-        uint8Arr = [0] * strLen
+        str_data = [''] * 50
+        uint8Arr = [0] * 50
 
         # Read characters from byte_data according to strLen
         for i in range(strLen):
@@ -599,7 +711,6 @@ def test_pack_string():
     
     unpacked_str, str_len, byte_index, uint8Arr = msg_helper.depack_string(packed_data, 0)
     print("Unpacked string:", unpacked_str)
-    
 if __name__ == "__main__":
     test_pack_simple_message()
     test_pack_string()
\ No newline at end of file
diff --git a/CommonLib/RealSimPack.m b/CommonLib/RealSimPack.m
index fd0b66b2..ccb9c233 100644
--- a/CommonLib/RealSimPack.m
+++ b/CommonLib/RealSimPack.m
@@ -72,7 +72,7 @@ function setupImpl(obj)
 % %             try
 
                 % initialize ByteData
-                ByteData = zeros(1024, 1, 'uint8');
+                ByteData = zeros(2048, 1, 'uint8');
                 
                 % parser ByteData
                 [ByteData, nMsgSize] = obj.packVehData(simState, t, ByteData, VehDataBus);
@@ -98,7 +98,7 @@ function validateInputsImpl(obj, simState, t, VehData)
         function [sz1, sz2] = getOutputSizeImpl(obj)
             % Maximum length of linear indices and element vector is the
             % number of elements in the input
-            sz1 = [1024 1]; sz2 = [1 1]; 
+            sz1 = [2048 1]; sz2 = [1 1]; 
         end
         
         function [fz1, fz2] = isOutputFixedSizeImpl(~)
diff --git a/CommonLib/RealSimSocket.cpp b/CommonLib/RealSimSocket.cpp
index efba51ab..2b3bc241 100644
--- a/CommonLib/RealSimSocket.cpp
+++ b/CommonLib/RealSimSocket.cpp
@@ -38,8 +38,8 @@
 #pragma comment (lib, "Mswsock.lib")
 #pragma comment (lib, "AdvApi32.lib")
 
-#define RECVBUFFERSIZE 1024
-#define SENDBUFFERSIZE 1024
+#define RECVBUFFERSIZE 2048
+#define SENDBUFFERSIZE 2048
 
 
 // s-function parameters
diff --git a/CommonLib/SocketHelper.cpp b/CommonLib/SocketHelper.cpp
index ba94d000..15fd1bb2 100644
--- a/CommonLib/SocketHelper.cpp
+++ b/CommonLib/SocketHelper.cpp
@@ -253,7 +253,7 @@ void SocketHelper::socketShutdown() {
 #else
 		shutdown(selfServerSock[iS], SHUT_RDWR);
 		shutdown(clientSock[iS], SHUT_RDWR);
-	#ifdef RS_DSPACE
+	#ifdef DSRTLX
 		close(selfServerSock[iS]);
 		close(clientSock[iS]);
 	#endif
@@ -265,7 +265,7 @@ void SocketHelper::socketShutdown() {
 		closesocket(serverSock[iS]);
 #else
 		shutdown(serverSock[iS], SHUT_RDWR);
-	#ifdef RS_DSPACE
+	#ifdef DSRTLX
 		close(serverSock[iS]);
 	#endif
 #endif	
@@ -280,6 +280,36 @@ void SocketHelper::socketShutdown() {
 }
 
 
+static int sendAll(int sock, const char* buf, int len) {
+	int totalSent = 0;
+	while (totalSent < len) {
+		int n = send(sock, buf + totalSent, len - totalSent, 0);
+		if (n == SOCKET_ERROR) {
+#ifdef _WIN32
+			int err = WSAGetLastError();
+			if (err == WSAEWOULDBLOCK) {
+				Sleep(1);
+				continue;
+			}
+			fprintf(stderr, "sendAll failed, WSA=%d\n", err);
+#else
+			perror("sendAll failed");
+#endif
+			return -1;
+		}
+		if (n == 0) {
+#ifdef _WIN32
+			fprintf(stderr, "sendAll failed: peer closed (send returned 0)\n");
+#endif
+			return -1;
+		}
+		totalSent += n;
+	}
+	return 0;
+}
+
+
+
 void SocketHelper::enableWaitClientTrigger() {
 	// if enable, then need to receive ULONG_MAX signal to trigger start of service
 	ENABLE_WAIT_CLIENT_TRIGGER = true;
@@ -401,7 +431,7 @@ int SocketHelper::initConnection(std::string errorLogName) {
 				printSocketErrorMessage(WSAGetLastError());
 #else
 				printf("Unable to connect to server! error\n");
-			#ifdef RS_DSPACE
+			#ifdef DSRTLX
 				close(serverSock[iS]);
 			#endif
 #endif
@@ -439,7 +469,7 @@ int SocketHelper::initConnection(std::string errorLogName) {
 				WSACleanup();
 #else
 				fprintf(stderr, "%s: \n", "socket() failed");
-			#ifdef RS_DSPACE
+			#ifdef DSRTLX
 				close(selfServerSock[iS]);
 			#endif
 #endif
@@ -470,7 +500,7 @@ int SocketHelper::initConnection(std::string errorLogName) {
 					f << "bind() failed! error: " << endl;
 					f.close();
 			}
-			#ifdef RS_DSPACE
+			#ifdef DSRTLX
 				close(selfServerSock[iS]);
 			#endif
 #endif
@@ -495,7 +525,7 @@ int SocketHelper::initConnection(std::string errorLogName) {
 					f.close();
 				}
 				fprintf(stderr, "%s:\n", "listen() failed");
-			#ifdef RS_DSPACE
+			#ifdef DSRTLX
 				close(selfServerSock[iS]);
 			#endif
 #endif
@@ -581,7 +611,7 @@ int SocketHelper::initConnection(std::string errorLogName) {
 							f.close();
 					}
 						fprintf(stderr, "%s: \n", "accept() failed");
-					#ifdef RS_DSPACE
+					#ifdef DSRTLX
 						close(clientSock[iS]);
 					#endif
 #endif
@@ -684,7 +714,7 @@ int SocketHelper::initConnection(std::string errorLogName) {
 							}
 							fprintf(stderr, "%s: \n", "accept() failed");
 							//exit(1);
-						#ifdef RS_DSPACE
+						#ifdef DSRTLX
 							close(clientSock[iS]);
 						#endif
 #endif
@@ -747,7 +777,7 @@ int SocketHelper::initConnection(std::string errorLogName) {
 					}
 					printf("recv() failed with error code ");
 					//exit(EXIT_FAILURE);
-					#ifdef RS_DSPACE
+					#ifdef DSRTLX
 						close(clientSock[iS]);
 					#endif
 #endif
@@ -779,7 +809,7 @@ int SocketHelper::initConnection(std::string errorLogName) {
 			char sendClientBuffer[SENDCLIENTBUFSIZE];
 			memcpy(sendClientBuffer, (char*)&StartServiceMsg, sizeof(StartServiceMsg));
 			int sendMsgSize = sizeof(StartServiceMsg);
-			if (send(serverSock[iS], sendClientBuffer, sendMsgSize, 0) < 0)
+			if (sendAll(serverSock[iS], sendClientBuffer, sendMsgSize) < 0)  //if (send(serverSock[iS], sendClientBuffer, sendMsgSize, 0) < 0)
 			{
 #ifdef WIN32
 				if (!errorLogName.empty()) {
@@ -797,7 +827,7 @@ int SocketHelper::initConnection(std::string errorLogName) {
 					f.close();
 				}
 				printf("send failed with error:\n");
-				#ifdef RS_DSPACE
+				#ifdef DSRTLX
 					close(serverSock[iS]);
 				#endif
 #endif
@@ -814,6 +844,25 @@ int SocketHelper::initConnection(std::string errorLogName) {
 	return 0;
 }
 
+static int recvAll(int sock, char* buf, int len) {
+	int total = 0;
+	while (total < len) {
+		int n = recv(sock, buf + total, len - total, 0);
+		if (n == 0) return -1;            
+		if (n < 0) {
+#ifdef WIN32
+			int err = WSAGetLastError();
+			if (err == WSAEINTR) continue;     
+#endif
+			return -1;
+		}
+		total += n;
+	}
+	return total;
+}
+
+
+
 int SocketHelper::recvData(int sock, int* simState, float* simTime, MsgHelper& Msg_c) {
 	// this function will receive all possible data
 
@@ -823,6 +872,15 @@ int SocketHelper::recvData(int sock, int* simState, float* simTime, MsgHelper& M
 
 	int recvSize;
 
+	if (recvAll(sock, recvBuffer, Msg_c.msgHeaderSize) < 0) {
+#ifdef WIN32
+		printf("recvAll(msgHeader) failed, WSA=%d\n", WSAGetLastError());
+#endif
+		return -1;
+	}
+
+
+/*
 	if ((recvSize = recv(sock, recvBuffer, Msg_c.msgHeaderSize, 0)) == SOCKET_ERROR) {
 #ifdef WIN32
 		if (WSAGetLastError() == WSAEINTR) {
@@ -838,6 +896,8 @@ int SocketHelper::recvData(int sock, int* simState, float* simTime, MsgHelper& M
 #endif
 		return -1;
 	}
+*/
+
 	//+++++++++
 	// Parser received message
 	//+++++++++
@@ -861,6 +921,15 @@ int SocketHelper::recvData(int sock, int* simState, float* simTime, MsgHelper& M
 		uint8_t iMsgTypeRecv = 0;
 
 		char recvVehDataHeaderBuf[256];
+
+		if (recvAll(sock, recvVehDataHeaderBuf, Msg_c.msgEachHeaderSize) < 0) {
+#ifdef WIN32
+			printf("recvAll(eachHeader) failed, WSA=%d\n", WSAGetLastError());
+#endif
+			return -1;
+		}
+
+/*
 		if ((recvSize = recv(sock, recvVehDataHeaderBuf, Msg_c.msgEachHeaderSize, 0)) == SOCKET_ERROR) {
 #ifdef WIN32
 			if (WSAGetLastError() == WSAEFAULT) {
@@ -873,8 +942,19 @@ int SocketHelper::recvData(int sock, int* simState, float* simTime, MsgHelper& M
 #endif
 			return -1;
 		}
+*/
+
 		Msg_c.depackMsgType(recvVehDataHeaderBuf, &iMsgSizeRecv, &iMsgTypeRecv);
+		if (iMsgTypeRecv < 1 || iMsgTypeRecv > 3 || iMsgSizeRecv < Msg_c.msgEachHeaderSize || iMsgSizeRecv > 65535) {
+			printf("DESYNC? bad eachHeader: type=%u size=%u raw=%02X %02X %02X\n",
+				(unsigned)iMsgTypeRecv, (unsigned)iMsgSizeRecv,
+				(unsigned char)recvVehDataHeaderBuf[0],
+				(unsigned char)recvVehDataHeaderBuf[1],
+				(unsigned char)recvVehDataHeaderBuf[2]);
+			return -1;
+		}
 
+/*
 		char recvEachDataBuf[1024];
 		// this recvSize is purely the message body, without the Msg_c.msgEachHeaderSize
 		if ((recvSize = recv(sock, recvEachDataBuf, iMsgSizeRecv - Msg_c.msgEachHeaderSize, 0)) == SOCKET_ERROR) {
@@ -889,21 +969,40 @@ int SocketHelper::recvData(int sock, int* simState, float* simTime, MsgHelper& M
 #endif
 			return -1;
 		}
+*/
+
+
+		int bodySize = iMsgSizeRecv - Msg_c.msgEachHeaderSize;
+		if (bodySize <= 0) {
+			printf("ERROR: invalid bodySize=%d\n", bodySize);
+			return -1;
+		}
+		std::vector<char> recvEachDataBuf(bodySize);
+
+
+		if (recvAll(sock, recvEachDataBuf.data(), bodySize) < 0) {
+#ifdef WIN32
+			printf("recvAll(body) failed, WSA=%d\n", WSAGetLastError());
+#endif
+			return -1;
+		}
+
+		recvSize = bodySize;  
 
 
 		string id;
 		switch (iMsgTypeRecv) {
 		case 1:
-			Msg_c.depackVehData(recvEachDataBuf, iVehData);
+			Msg_c.depackVehData(recvEachDataBuf.data(), iVehData);
 			id = iVehData.id;
 			Msg_c.VehDataRecv_um[id] = iVehData;
 			break;
 		case 2:
-			Msg_c.depackTrafficLightData(recvEachDataBuf, &iTlsData);
+			Msg_c.depackTrafficLightData(recvEachDataBuf.data(), &iTlsData);
 			Msg_c.TlsDataRecv_um[iTlsData.name] = iTlsData;
 			break;
 		case 3:
-			Msg_c.depackDetectorData(recvEachDataBuf, recvSize, &iDetData);
+			Msg_c.depackDetectorData(recvEachDataBuf.data(), recvSize, &iDetData);
 			Msg_c.DetDataRecv_um[get<1>(iDetData)] = iDetData;
 			break;
 		}
@@ -915,7 +1014,93 @@ int SocketHelper::recvData(int sock, int* simState, float* simTime, MsgHelper& M
 }
 
 
+int SocketHelper::sendData(int sock, int iClient, float simTimeSend, uint8_t simStateSend, MsgHelper Msg_c)
+{
 
+	std::vector<char> tempVeh;
+	std::vector<char> tempTls;
+	std::vector<char> tempDet;
+
+	tempVeh.reserve(64 * 1024);   
+	tempTls.reserve(8 * 1024);
+	tempDet.reserve(8 * 1024);
+
+	int tempVehDataByte = 0;
+	int tempTlsDataByte = 0;
+	int tempDetDataByte = 0;
+
+	// Vehicle
+	if (Msg_c.VehDataSend_um.size() > 0) {
+		for (size_t iV = 0; iV < Msg_c.VehDataSend_um[sock].size(); iV++) {
+
+			const int kPerVehReserve = 1024;
+
+			if ((int)tempVeh.size() < tempVehDataByte + kPerVehReserve) {
+				tempVeh.resize(tempVehDataByte + kPerVehReserve);
+			}
+			Msg_c.packVehData(Msg_c.VehDataSend_um[sock][iV], tempVeh.data(), &tempVehDataByte);
+		}
+	}
+
+	// Tls
+	if (Msg_c.TlsDataSend_um.size() > 0) {
+		for (size_t iT = 0; iT < Msg_c.TlsDataSend_um[sock].size(); iT++) {
+			const int kPerTlsReserve = 256;
+			if ((int)tempTls.size() < tempTlsDataByte + kPerTlsReserve) {
+				tempTls.resize(tempTlsDataByte + kPerTlsReserve);
+			}
+			Msg_c.packTrafficLightData(Msg_c.TlsDataSend_um[sock][iT], tempTls.data(), &tempTlsDataByte);
+		}
+	}
+
+	// Detector
+	if (Msg_c.DetDataSend_um.size() > 0) {
+		for (size_t iD = 0; iD < Msg_c.DetDataSend_um[sock].size(); iD++) {
+			const int kPerDetReserve = 256;
+			if ((int)tempDet.size() < tempDetDataByte + kPerDetReserve) {
+				tempDet.resize(tempDetDataByte + kPerDetReserve);
+			}
+			Msg_c.packDetectorData(Msg_c.DetDataSend_um[sock][iD], tempDet.data(), &tempDetDataByte);
+		}
+	}
+
+	// Total
+	const int totalSize = MSG_HEADER_SIZE + tempVehDataByte + tempTlsDataByte + tempDetDataByte;
+
+	std::vector<char> sendBuffer(totalSize);
+	int iByte = 0;
+	Msg_c.packHeader(simStateSend, simTimeSend, totalSize, sendBuffer.data(), &iByte);
+
+	int offset = MSG_HEADER_SIZE;
+	if (tempVehDataByte > 0) {
+		memcpy(sendBuffer.data() + offset, tempVeh.data(), tempVehDataByte);
+		offset += tempVehDataByte;
+	}
+	if (tempTlsDataByte > 0) {
+		memcpy(sendBuffer.data() + offset, tempTls.data(), tempTlsDataByte);
+		offset += tempTlsDataByte;
+	}
+	if (tempDetDataByte > 0) {
+		memcpy(sendBuffer.data() + offset, tempDet.data(), tempDetDataByte);
+		offset += tempDetDataByte;
+	}
+
+	// sendAll
+	if (sendAll(sock, sendBuffer.data(), totalSize) < 0) {
+#ifdef WIN32
+		fprintf(stderr, "%s: %d\n", "send() failed", WSAGetLastError());
+#else
+		fprintf(stderr, "%s:\n", "send() failed");
+#endif
+		return -1;
+	}
+
+	return 0;
+}
+
+
+
+/*
 int SocketHelper::sendData(int sock, int iClient, float simTimeSend, uint8_t simStateSend, MsgHelper Msg_c) {
 	// for each socket, send only the message relevant to that socket
 
@@ -923,7 +1108,7 @@ int SocketHelper::sendData(int sock, int iClient, float simTimeSend, uint8_t sim
 
 	// initialize send data buffer
 	//char tempVehDataBuffer[8096];
-	char* tempVehDataBuffer = new char[65536];
+	char* tempVehDataBuffer = new char[524288];
 	int tempVehDataByte = 0;
 
 	char* tempTlsDataBuffer = new char[8096];
@@ -933,7 +1118,7 @@ int SocketHelper::sendData(int sock, int iClient, float simTimeSend, uint8_t sim
 	int tempDetDataByte = 0;
 
 	int sendMsgSize = 0;
-	char* sendBuffer = new char[81728];
+	char* sendBuffer = new char[540480];
 
 	// Pack vehicle and detector data
 	if (Msg_c.VehDataSend_um.size() > 0) {
@@ -971,7 +1156,7 @@ int SocketHelper::sendData(int sock, int iClient, float simTimeSend, uint8_t sim
 	sendMsgSize += tempDetDataByte;
 
 	// send all vehicle and detector data
-	if (send(sock, sendBuffer, sendMsgSize, 0) != sendMsgSize) {
+	if (sendAll(sock, sendBuffer, sendMsgSize) < 0 ) {   //if (send(sock, sendBuffer, sendMsgSize, 0) != sendMsgSize) 
 #ifdef WIN32
 		fprintf(stderr, "%s: %d\n", "send() failed", WSAGetLastError());
 #else
@@ -992,4 +1177,5 @@ int SocketHelper::sendData(int sock, int iClient, float simTimeSend, uint8_t sim
 
 	return 0;
 
-}
\ No newline at end of file
+}
+*/
\ No newline at end of file
diff --git a/CommonLib/SocketHelper.h b/CommonLib/SocketHelper.h
index 378b438c..2f54f927 100644
--- a/CommonLib/SocketHelper.h
+++ b/CommonLib/SocketHelper.h
@@ -147,12 +147,10 @@ class SocketHelper
 
 	std::vector <struct sockaddr_in> selfServerAddr; /* Local a ddress */
 	std::vector <struct sockaddr_in> clientAddr; /* Client address */
-#ifdef RS_DSPACE
-	std::vector <socklen_t> clientAddrLen;            /* Length of client address data structure */
-#elif WIN32
-	std::vector <int> clientAddrLen;            /* Length of client address data structure */
-#else
+#ifndef WIN32
 	std::vector <size_t> clientAddrLen;            /* Length of client address data structure */
+#else
+	std::vector <int> clientAddrLen;            /* Length of client address data structure */
 #endif
 	std::vector <int> sendClientByte;
 	std::vector <int> recvClientMsgSize;                    /* Size of received message */
diff --git a/CommonLib/SocketHelper.py b/CommonLib/SocketHelper.py
index 590c34fe..b2bec226 100644
--- a/CommonLib/SocketHelper.py
+++ b/CommonLib/SocketHelper.py
@@ -1,5 +1,6 @@
 from struct import unpack, pack
 from CommonLib.VehDataMsgDefs import VehData
+from CommonLib.VehDataMsgDefs import TrafficLightData
 from CommonLib.MsgHelper import MsgHelper, MessageType
 from CommonLib.ConfigHelper import ConfigHelper
 import typing
@@ -27,7 +28,7 @@ def __init__(self, config_helper: ConfigHelper, msg_helper: MsgHelper):
 
         # initialize lists to store received data
         self.vehicle_data_receive_list: typing.List[VehData] = []
-        self.traffic_light_data_receive_list = []
+        self.traffic_light_data_receive_list: typing.List[TrafficLightData] = []
         self.detector_data_receive_list = []
 
     def clear_data(self):
@@ -87,16 +88,86 @@ def depack_detector_data(self,buffer):
             DetDataRecv_v.append(DetData_d)
 
         return DetDataRecv_v
+    
+
+    def recvall(self, sock, n: int) -> bytes:
+        """Read exactly n bytes from TCP stream, or raise ConnectionError/TimeoutError."""
+        data = bytearray()
+        while len(data) < n:
+            chunk = sock.recv(n - len(data))
+            if not chunk:
+                raise ConnectionError(f"Socket closed while reading {n} bytes (got {len(data)})")
+            data.extend(chunk)
+        return bytes(data)
+    
+
+    # def recv_data(self, sock):
+    #     # initialize return lists
+        
+        
+    #     # get header for entire message
+    #     received_buffer = sock.recv(self.msg_header_size)
+        
+
+    #     sim_state, sim_time, total_msg_size = self.msg_helper.depack_msg_header(received_buffer)
+    #     msg_processed_size = 0
+    #     msg_processed_size = msg_processed_size + self.msg_header_size
+    #     # total message size is the data to be received
+    #     # save received_buffer to local log for debugging
+    #     log_file_path = "received_header_buffer.log"
+    #     with open(log_file_path, 'ab') as log_file:  # 'ab' for appending binary data
+    #         log_file.write(received_buffer)
+    #         log_file.write(b'\n')
+        
+    #     while (msg_processed_size < total_msg_size):
+    #         # get message type header
+    #         received_buffer = sock.recv(self.msg_each_header_size)
+    #         msg_size, msg_type = self.msg_helper.depack_msg_type(received_buffer)
+
+    #         log_file_path = "received_each_header_buffer.log"
+    #         with open(log_file_path, 'ab') as log_file:  # 'ab' for appending binary data
+    #             log_file.write(received_buffer)
+    #             log_file.write(b'\n')
+
+    #         # get message it self
+    #         received_buffer = sock.recv(msg_size - self.msg_each_header_size)
+
+    #         log_file_path = "received_msg_buffer.log"
+    #         with open(log_file_path, 'ab') as log_file:  # 'ab' for appending binary data
+    #             log_file.write(received_buffer)
+    #             log_file.write(b'\n')
+
+    #         # unpack message based on type identifier
+    #         if msg_type == MessageType.vehicle_data:
+    #             aa = 1
+    #             vehicle_data_received = self.msg_helper.depack_veh_data(received_buffer)
+    #             self.vehicle_data_receive_list.append(vehicle_data_received)
+    #         elif msg_type == MessageType.traffic_light_data:               
+    #             aa = 1
+    #             tls = self.msg_helper.depack_traffic_light_data(received_buffer)
+    #             self.traffic_light_data_receive_list.append(tls)
+
+    #         elif msg_type == MessageType.detector_data:
+    #             # DetDataRecv_v = self.depackDetectorData(received_buffer) 
+    #             aa = 1
+    #         else:
+    #             aa = 1
+
+    #         msg_processed_size = msg_processed_size + msg_size
+
+    #     return sim_state, sim_time,
+
+
 
     def recv_data(self, sock):
         # initialize return lists
         
         
         # get header for entire message
-        received_buffer = sock.recv(self.msg_header_size)
-        
-
+        # 1) header
+        received_buffer = self.recvall(sock, self.msg_header_size)
         sim_state, sim_time, total_msg_size = self.msg_helper.depack_msg_header(received_buffer)
+
         msg_processed_size = 0
         msg_processed_size = msg_processed_size + self.msg_header_size
         # total message size is the data to be received
@@ -116,53 +187,123 @@ def recv_data(self, sock):
                 with open(log_file_path, 'a', encoding='utf-8') as log_file:  # 'a' for appending text
                     log_file.write(f"[MSG_HEADER] Size: {msg_size}, Type: {msg_type} | Hex: {received_buffer.hex()}\n")
 
-            # get message it self
-            received_buffer = sock.recv(msg_size - self.msg_each_header_size)
+        msg_processed_size = self.msg_header_size
+
+        # 2) sub information
+        while msg_processed_size < total_msg_size:
+            each_hdr = self.recvall(sock, self.msg_each_header_size)
+            msg_size, msg_type = self.msg_helper.depack_msg_type(each_hdr)
+
+
+            body_size = msg_size - self.msg_each_header_size
+            if body_size <= 0:
+                raise RuntimeError(f"Invalid body_size={body_size}, msg_size={msg_size}, msg_type={msg_type}")
+
 
             if self.enable_verbose_log:
                 log_file_path = "received_msg_buffer.log"
                 with open(log_file_path, 'a', encoding='utf-8') as log_file:  # 'a' for appending text
                     log_file.write(f"[MSG_DATA] Size: {len(received_buffer)}, Type: {msg_type} | Hex: {received_buffer.hex()}\n")
 
-            # unpack message based on type identifier
-            if msg_type == MessageType.vehicle_data:
-                aa = 1
-                vehicle_data_received = self.msg_helper.depack_veh_data(received_buffer)
-                self.vehicle_data_receive_list.append(vehicle_data_received)
-            elif msg_type == MessageType.traffic_light_data:               
-                aa = 1
+            body = self.recvall(sock, body_size)
+
 
+            # unpack
+            if msg_type == MessageType.vehicle_data:
+                vd = self.msg_helper.depack_veh_data(body)
+                self.vehicle_data_receive_list.append(vd)
+            elif msg_type == MessageType.traffic_light_data:
+                tls = self.msg_helper.depack_traffic_light_data(body)
+                self.traffic_light_data_receive_list.append(tls)
             elif msg_type == MessageType.detector_data:
-                # DetDataRecv_v = self.depackDetectorData(received_buffer) 
-                aa = 1
-            else:
-                aa = 1
+                pass
+
+            msg_processed_size += msg_size
+
+        return sim_state, sim_time
+
 
-            msg_processed_size = msg_processed_size + msg_size
 
-        return sim_state, sim_time,
         
 
-    def sendData(self, simState, simTime, sock):
-        byte_index = 0
-        total_msg_size = 0
-        total_msg_size = total_msg_size + self.msg_header_size
-        byte_index = byte_index + self.msg_header_size
-        vehicle_byte_index = 0
-        vehicle_data_buffer = bytearray(65536) 
-        # TODO: add traffic light and detector data
-        traffic_light_data_buffer = bytearray(8096)
-        detector_data_buffer = bytearray(8096)
-        data_buffer = bytearray(81728)
-        for vehicle_data in self.vehicle_data_send_list:
-            vehicle_data, vehicle_msg_size, vehicle_byte_index = self.msg_helper.pack_veh_data(vehicle_data_buffer, 
-                                                                                                   vehicle_byte_index, 
-                                                                                                   vehicle_data)
-            total_msg_size = total_msg_size + vehicle_msg_size
+    # def sendData(self, simState, simTime, sock):
+    #     byte_index = 0
+    #     total_msg_size = 0
+    #     total_msg_size = total_msg_size + self.msg_header_size
+    #     byte_index = byte_index + self.msg_header_size
+    #     vehicle_byte_index = 0
+    #     vehicle_data_buffer = bytearray(65536) 
+    #     # TODO: add traffic light and detector data
+    #     traffic_light_data_buffer = bytearray(8096)
+    #     detector_data_buffer = bytearray(8096)
+    #     data_buffer = bytearray(81728)
+    #     for vehicle_data in self.vehicle_data_send_list:
+    #         vehicle_data, vehicle_msg_size, vehicle_byte_index = self.msg_helper.pack_veh_data(vehicle_data_buffer, 
+    #                                                                                                vehicle_byte_index, 
+    #                                                                                                vehicle_data)
+    #         total_msg_size = total_msg_size + vehicle_msg_size
         
-        data_buffer, byte_index = self.msg_helper.pack_msg_header(data_buffer, simState, simTime, total_msg_size)
-        data_buffer[byte_index:byte_index+vehicle_byte_index] = vehicle_data_buffer[0:vehicle_byte_index]
-        byte_index = byte_index + vehicle_byte_index
+    #     data_buffer, byte_index = self.msg_helper.pack_msg_header(data_buffer, simState, simTime, total_msg_size)
+    #     data_buffer[byte_index:byte_index+vehicle_byte_index] = vehicle_data_buffer[0:vehicle_byte_index]
+    #     byte_index = byte_index + vehicle_byte_index
+
+    #     # send data
+    #     sock.sendall(data_buffer[0:byte_index])
+
+
+    # def sendData(self, simState, simTime, sock):
+    #     vehicle_byte_index = 0
+    #     vehicle_data_buffer = bytearray(524288)
+
+    #     for vehicle_data in self.vehicle_data_send_list:
+    #         vehicle_data, vehicle_msg_size, vehicle_byte_index = self.msg_helper.pack_veh_data(
+    #             vehicle_data_buffer, vehicle_byte_index, vehicle_data
+    #         )
+
+    #         if vehicle_byte_index > len(vehicle_data_buffer):
+    #             raise RuntimeError(...)
+
+
+    #     total_msg_size = self.msg_header_size + vehicle_byte_index
+
+    #     data_buffer = bytearray(total_msg_size)
+    #     data_buffer, byte_index = self.msg_helper.pack_msg_header(data_buffer, simState, simTime, total_msg_size)
+
+
+    #     assert byte_index == self.msg_header_size
+
+    #     data_buffer[byte_index:byte_index + vehicle_byte_index] = vehicle_data_buffer[:vehicle_byte_index]
+    #     sock.sendall(data_buffer)
+
+    def sendData(self, simState, simTime, sock):
+
+        vehicle_payload = bytearray()
+
+        for veh in self.vehicle_data_send_list:
+
+            n = self.msg_helper.calc_veh_msg_size_bytes(veh)
+
+            tmp = bytearray(n)
+            tmp, msg_size_or_payload_size, used = self.msg_helper.pack_veh_data(tmp, 0, veh)
+
+
+            if used != n:
+                raise RuntimeError(f"pack_veh_data size mismatch: expected {n}, got used={used}")
+
+            vehicle_payload.extend(tmp[:used])
+
+
+        total_msg_size = self.msg_header_size + len(vehicle_payload)
+
+        data_buffer = bytearray(total_msg_size)
+        data_buffer, hdr_used = self.msg_helper.pack_msg_header(data_buffer, simState, simTime, total_msg_size)
+
+
+        if hdr_used != self.msg_header_size:
+            raise RuntimeError(f"msg_header_size mismatch: expected {self.msg_header_size}, got {hdr_used}")
+
+        data_buffer[hdr_used:hdr_used + len(vehicle_payload)] = vehicle_payload
+
+        sock.sendall(data_buffer)
+
 
-        # send data
-        sock.sendall(data_buffer[0:byte_index])
\ No newline at end of file
diff --git a/CommonLib/TrafficHelper.cpp b/CommonLib/TrafficHelper.cpp
index d2864abb..20683696 100644
--- a/CommonLib/TrafficHelper.cpp
+++ b/CommonLib/TrafficHelper.cpp
@@ -1,1744 +1,1802 @@
-#include "TrafficHelper.h"
-#include <stdexcept>
-
-
-//const unsigned short selfServerPort[NSERVER] = { 420 };
-
-using namespace std;
-
-//CentralCtrl CentralCtrl_g;
-
-// The convention is that, Server always provide service to the Client. 
-// i.e. Manager will be the Server to provide traffic data service to all possible clients, e.g. Controller, Communication, Vehicle. 
-// The Controller can be another service provider to Vehicle to command desired speed
-// The Communication will be another service provider to Controller to provide traffic information. 
-// 
-// [              ] - [Controller] - [      ]
-// [Another Server] - [Controller] - [Client]
-// [              ] - [Controller] - [      ]
-//
-// First, connect to other server
-// Then, wait all clients connect
-// Next, signal other server to start service
-// Last, start own service => return to the caller of Socket initConnection
-TrafficHelper::TrafficHelper(){
-}
-
-void TrafficHelper::connectionSetup(string trafficIp, int trafficPort, int nClientInput, int order) {
-
-	nClient = nClientInput;
-
-	edgeHasSubscribed = false;
-	//nEdgeSubscribe = 0;
-
-	//system("sumo -c \"C:\Users\y0n\Dropbox (ORNL)\2_projects\1_2_sumo\1_4_speedHarmTest\speedHarmTest.sumocfg\" --remote-port 1337 ");
-
-	// ===========================================================================
-	// Auto-launch SUMO if configured
-	// ===========================================================================
-	if (Config_c->SumoSetup.EnableAutoLaunch) {
-		printf("Auto-launch SUMO enabled\n");
-		printf("  Config file: %s\n", Config_c->SumoSetup.SumoConfigFile.c_str());
-		printf("  Num clients: %d\n", Config_c->SumoSetup.NumClients);
-
-#ifdef ENABLE_LIBSUMO
-		// -----------------------------------------------------------------------
-		// OPTION A: Direct launch via libsumo (headless only, no GUI)
-		// This is the active implementation
-		// -----------------------------------------------------------------------
-		try {
-			std::vector<std::string> cmd = {"sumo", "-c", Config_c->SumoSetup.SumoConfigFile, "--start", "--step-length", "0.1",
-				"--num-clients", std::to_string(Config_c->SumoSetup.NumClients)};
-
-			printf("Launching SUMO via libsumo::Simulation::start()...\n");
-			libsumo::Simulation::start(cmd);
-			libsumo::Simulation::setOrder(order);
-			printf("SUMO launched successfully via libsumo\n");
-		}
-		catch (const std::exception& e) {
-			printf("ERROR: Failed to start SUMO via libsumo: %s\n", e.what());
-			throw;
-		}
-#else
-		// -----------------------------------------------------------------------
-		// OPTION B: External process launch with sumo-gui (supports GUI)
-		// -----------------------------------------------------------------------
-		try {
-			// Build command line for sumo-gui with multi-client support
-			std::string sumoCmd = "sumo-gui -c \"" + Config_c->SumoSetup.SumoConfigFile +
-				"\" --remote-port " + std::to_string(trafficPort) +
-				" --num-clients " + std::to_string(Config_c->SumoSetup.NumClients) +
-				" --step-length 0.1 --start";
-
-			printf("Launching SUMO-GUI as external process...\n");
-			printf("  Command: %s\n", sumoCmd.c_str());
-
-			// Launch in background (platform-specific)
-#ifdef WIN32
-			std::string launchCmd = "start /B " + sumoCmd;  // Windows: use start /B
-#else
-			std::string launchCmd = sumoCmd + " &";         // Linux: append &
-#endif
-			int result = system(launchCmd.c_str());
-			if (result != 0) {
-				printf("ERROR: Failed to launch SUMO-GUI (exit code: %d)\n", result);
-				throw std::runtime_error("Failed to launch SUMO-GUI process");
-			}
-
-			printf("SUMO-GUI process started successfully\n");
-
-			// Wait a bit for SUMO to initialize
-			printf("Waiting for SUMO to initialize...\n");
-#ifdef WIN32
-			Sleep(3000); // Wait 3 seconds (Windows)
-#else
-			sleep(3); // Wait 3 seconds (Linux)
-#endif
-
-			// Now connect via TraCI as usual
-			printf("Connecting to SUMO via TraCI...\n");
-			SUMO_TRACI_NAMESPACE::Simulation::init(trafficPort, libsumo::DEFAULT_NUM_RETRIES, trafficIp);
-			SUMO_TRACI_NAMESPACE::Simulation::setOrder(order);
-			printf("Connected to SUMO successfully\n");
-		}
-		catch (const std::exception& e) {
-			printf("ERROR: Failed to auto-launch SUMO: %s\n", e.what());
-			throw;
-		}
-#endif
-	}
-	else {
-		// Original behavior: connect to existing SUMO instance
-		try {
-			SUMO_TRACI_NAMESPACE::Simulation::init(trafficPort, libsumo::DEFAULT_NUM_RETRIES, trafficIp);
-			SUMO_TRACI_NAMESPACE::Simulation::setOrder(order);
-		}
-		catch (const std::exception& e) {
-			printf("ERROR: Failed to connect to SUMO at %s:%d - %s\n", trafficIp.c_str(), trafficPort, e.what());
-			throw;
-		}
-	}
-
-	/********************************************
-	* GET VEH SUBSCRIPTION
-	*********************************************/
-	//VehDataSubscribeList.push_back(libsumo::TRACI_ID_LIST);
-	VehDataSubscribeList.push_back(libsumo::VAR_TYPE);
-	VehDataSubscribeList.push_back(libsumo::VAR_SPEED);
-	VehDataSubscribeList.push_back(libsumo::VAR_POSITION3D);
-	VehDataSubscribeList.push_back(libsumo::VAR_ANGLE); // north 0 deg, clockwise
-	VehDataSubscribeList.push_back(libsumo::VAR_COLOR);
-	VehDataSubscribeList.push_back(libsumo::VAR_ROAD_ID);
-	VehDataSubscribeList.push_back(libsumo::VAR_LANE_INDEX);
-	VehDataSubscribeList.push_back(libsumo::VAR_DISTANCE);
-	VehDataSubscribeList.push_back(libsumo::VAR_LANEPOSITION);
-
-	VehDataSubscribeList.push_back(libsumo::VAR_LANE_ID);
-	VehDataSubscribeList.push_back(libsumo::VAR_VEHICLECLASS);
-	VehDataSubscribeList.push_back(libsumo::VAR_ROUTE_INDEX);
-
-	// testing new data
-	VehDataSubscribeList.push_back(libsumo::VAR_ACCELERATION);
-	//// no speedDesired accelerationDesired
-	//// will return id and distance
-	//VehDataSubscribeList.push_back(libsumo::VAR_LEADER);
-	//// then need to query speed of that vehicle
-	//// or use followSpeed??
-	//VehDataSubscribeList.push_back(libsumo::VAR_FOLLOW_SPEED);
-	// signal light
-	// will return tlsID (signal light id), tlsIndex (signal head id), distance, state
-	//VehDataSubscribeList.push_back(libsumo::VAR_NEXT_TLS);
-	// speed limit
-	VehDataSubscribeList.push_back(libsumo::VAR_ALLOWED_SPEED);
-	VehDataSubscribeList.push_back(libsumo::VAR_SPEED_FACTOR);
-	// get next speed limit
-	// handle it at beginning when this vehicle enters
-	VehDataSubscribeList.push_back(libsumo::VAR_VIA);
-	// next link
-	// retrieved as part of the speed limit information
-	
-	//// lane change
-	//// need to check if this even works
-	//VehDataSubscribeList.push_back(libsumo::CMD_CHANGELANE);
-
-	// get slope of the road
-	VehDataSubscribeList.push_back(libsumo::VAR_SLOPE);
-
-	// get signals
-	//Name	Bit
-	//VEH_SIGNAL_BLINKER_RIGHT	0
-	//VEH_SIGNAL_BLINKER_LEFT	1
-	//VEH_SIGNAL_BLINKER_EMERGENCY	2
-	//VEH_SIGNAL_BRAKELIGHT	3
-	//VEH_SIGNAL_FRONTLIGHT	4
-	//VEH_SIGNAL_FOGLIGHT	5
-	//VEH_SIGNAL_HIGHBEAM	6
-	//VEH_SIGNAL_BACKDRIVE	7
-	//VEH_SIGNAL_WIPER	8
-	//VEH_SIGNAL_DOOR_OPEN_LEFT	9
-	//VEH_SIGNAL_DOOR_OPEN_RIGHT	10
-	//VEH_SIGNAL_EMERGENCY_BLUE	11
-	//VEH_SIGNAL_EMERGENCY_RED	12
-	//VEH_SIGNAL_EMERGENCY_YELLOW	13
-	VehDataSubscribeList.push_back(libsumo::VAR_SIGNALS);
-
-	// Return the length, width, height of the vehicle
-	VehDataSubscribeList.push_back(libsumo::VAR_LENGTH);
-	VehDataSubscribeList.push_back(libsumo::VAR_WIDTH);
-	VehDataSubscribeList.push_back(libsumo::VAR_HEIGHT);
-	// -------------------
-	// These variables are subscribed for testing purposes
-	// -------------------
-	VehDataSubscribeList.push_back(libsumo::VAR_SPEED_WITHOUT_TRACI);
-
-
-
-	//VehDataSubscribeList.push_back(libsumo::VAR_NEXT_TLS); // Returns upcoming traffic lights, along with distanceand state [(tlsID, tlsIndex, distance, state), ...]
-
-	//VehDataSubscribeList.push_back(libsumo::VAR_LEADER);/*Returns the id of the leading vehicleand its distance, if the string is empty, no leader was found within the given range.Only vehicles ahead on the currently list of best lanes are considered(see above).This means, the leader is only valid until the next lane - change maneuver.The returned distance is measured from the ego vehicle front bumper + minGap to the back bumper of the leader vehicle.*/
-
-	//VehDataSubscribeList.push_back(libsumo::VAR_ALLOWED_SPEED); //Returns the maximum allowed speed on the current lane regarding speed factor in m/s for this vehicle.
-	//
-	//VehDataSubscribeList.push_back(libsumo::VAR_SPEED_FACTOR); // Returns the road speed multiplier for this vehicle[double]
-
-	//VehDataSubscribeList.push_back(libsumo::VAR_SLOPE); // Retrieves the slope at the current vehicle position in degrees
-
-
-
-
-
-	/********************************************
-	* GET DETECTOR SUBSCRIPTION
-	*********************************************/
-	// obtain detector ids of the selected intersection and subscribe to results
-	if (ENABLE_DET_SUB) {
-		//string detSelectPattern = DET_SEL_PATTERN; // select which intersection we want to do signal control in the loop SCIP
-		////===================================================
-		//// Retreive DETECTOR configuration for the scenario BEFORE simulation starts
-		////===================================================
-		//vector <string> detAreaAllId_v = traci.lanearea.getIDList();
-		//vector <string> detInductAllId_v = traci.inductionloop.getIDList();
-
-		//vector <int> detSubscribeList;
-
-		//detSubscribeList.push_back(libsumo::LAST_STEP_VEHICLE_NUMBER);
-		//for (int iD = 0; iD < detAreaAllId_v.size(); iD++) {
-		//	if (detAreaAllId_v[iD].find(detSelectPattern) != std::string::npos) {
-		//		detSelectId_v.push_back(detAreaAllId_v[iD]);
-		//		traci.lanearea.subscribe(detAreaAllId_v[iD], detSubscribeList, 0, tSimuEnd);
-		//	}
-		//}
-	}
-
-
-
-	/********************************************
-	* GET Speed Limit of every edge, lane
-	*********************************************/
-	// Get list of all lanes and edges in the network
-	vector <string> laneList = SUMO_TRACI_NAMESPACE::Lane::getIDList();
-
-	// Build vehicle class list from all vehicle types defined in SUMO
-	vector <string> vehClassList;
-
-	vector <string> vehTypeList = SUMO_TRACI_NAMESPACE::VehicleType::getIDList();
-	for (int i = 0; i < vehTypeList.size(); i++) {
-		string vehType = vehTypeList[i];
-		string vehClass = SUMO_TRACI_NAMESPACE::VehicleType::getVehicleClass(vehType);
-
-		vehClassList.push_back(vehClass);
-	}
-
-	// Only retrieve speed limits if speedLimit or speedLimitNext is defined in VehicleMessageField
-	// This avoids expensive queries when speed limit data is not needed
-	if (VehicleMessageField_set.find("speedLimit") != VehicleMessageField_set.end() ||
-		VehicleMessageField_set.find("speedLimitNext") != VehicleMessageField_set.end()) {
-		for (int i = 0; i < laneList.size(); i++) {
-			string laneId = laneList[i];
-			string edgeId = SUMO_TRACI_NAMESPACE::Lane::getEdgeID(laneId);
-
-			vector <string> allowClassList = SUMO_TRACI_NAMESPACE::Lane::getAllowed(laneId);
-			vector <string> disallowClassList = SUMO_TRACI_NAMESPACE::Lane::getDisallowed(laneId);
-
-			// If no specific vehicle class restrictions, apply to all vehicle classes
-			if (allowClassList.size() == 0 && disallowClassList.size() == 0) {
-				for (int iC = 0; iC < vehClassList.size(); iC++) {
-					string vClass = vehClassList[iC];
-					LaneVehClass2SpeedLimit_um[make_pair(laneId, vClass)] = SUMO_TRACI_NAMESPACE::Lane::getMaxSpeed(laneId);
-
-					EdgeVehClass2SpeedLimit_um[make_pair(edgeId, vClass)] = SUMO_TRACI_NAMESPACE::Lane::getMaxSpeed(laneId);
-				}
-			}
-			// Otherwise, only apply to allowed vehicle classes
-			else {
-				for (int iC = 0; iC < allowClassList.size(); iC++) {
-					string vClass = allowClassList[iC];
-					LaneVehClass2SpeedLimit_um[make_pair(laneId, vClass)] = SUMO_TRACI_NAMESPACE::Lane::getMaxSpeed(laneId);
-
-					EdgeVehClass2SpeedLimit_um[make_pair(edgeId, vClass)] = SUMO_TRACI_NAMESPACE::Lane::getMaxSpeed(laneId);
-				}
-			}
-		}
-	}
-
-	vector <string> edgeList = SUMO_TRACI_NAMESPACE::Edge::getIDList();
-	for (int i = 0; i < edgeList.size(); i++) {
-		AllEdgeList.insert(edgeList[i]);
-	}
-
-}
-
-void TrafficHelper::connectionSetup(int nClientInput) {
-
-	nClient = nClientInput;
-
-	edgeHasSubscribed = false;
-	//nEdgeSubscribe = 0;
-
-}
-
-void TrafficHelper::enableVehSub() {
-	ENABLE_VEH_SUB = true;
-}
-
-void TrafficHelper::disableVehSub() {
-	ENABLE_VEH_SUB = false;
-}
-
-void TrafficHelper::enableDetSub() {
-	ENABLE_DET_SUB = true;
-}
-
-void TrafficHelper::disableDetSub() {
-	ENABLE_DET_SUB = false;
-}
-
-void TrafficHelper::selectVISSIM(vector <int> vissimSockInput, vector <string> vissimSockNames) {
-	SUMO_OR_VISSIM = "VISSIM";
-
-	for (int i = 0; i < vissimSockInput.size(); i++) {
-		vissimSock.push_back(vissimSockInput[i]);
-		vissimSockName_um[vissimSockInput[i]] = vissimSockNames[i];
-	}
-	//for (int i = 0; i < clientSockInput.size(); i++) {
-	//	clientSock.push_back(clientSockInput[i]);
-	//}
-}
-
-void TrafficHelper::selectSUMO() {
-	SUMO_OR_VISSIM = "SUMO";
-}
-
-void TrafficHelper::close() {
-	if (SUMO_OR_VISSIM.compare("SUMO") == 0) {
-		SUMO_TRACI_NAMESPACE::Simulation::close();
-	}
-	else if (SUMO_OR_VISSIM.compare("VISSIM") == 0) {
-
-	}
-	else {
-
-	}
-}
-
-
-void TrafficHelper::getConfig() {
-
-	if (Config_c->SimulationSetup.EnableVerboseLog) {
-		ENABLE_VERBOSE = true;
-	}
-	else {
-		ENABLE_VERBOSE = false;
-	}
-
-	if (Config_c->ApplicationSetup.VehicleSubscription.size() > 0) {
-		ENABLE_VEH_SUB = true;
-	}
-	else {
-		if (Config_c->XilSetup.EnableXil) {
-			ENABLE_VEH_SUB = true;
-		}
-		else {
-			ENABLE_VEH_SUB = false;
-		}
-	}
-	if (Config_c->ApplicationSetup.DetectorSubscription.size() > 0) {
-		ENABLE_DET_SUB = true;
-	}
-	else {
-		if (Config_c->XilSetup.DetectorSubscription.size() > 0) {
-			ENABLE_DET_SUB = true;
-		}
-		else{
-			ENABLE_DET_SUB = false;
-		}
-	}
-	if (Config_c->ApplicationSetup.SignalSubscription.size() > 0) {
-		ENABLE_SIG_SUB = true;
-	}
-	else {
-		if (Config_c->XilSetup.SignalSubscription.size() > 0) {
-			ENABLE_SIG_SUB = true;
-		}
-		else {
-			ENABLE_SIG_SUB = false;
-		}
-	}
-
-	//if (Config_c->CarMakerSetup.EnableCosimulation && Config_c->CarMakerSetup.SynchronizeTrafficSignal) {
-	//	ENABLE_SIG_SUB = true;
-	//}
-
-	if (Config_c->SimulationSetup.EnableExternalDynamics) {
-		ENABLE_EXT_DYN = true;
-	}
-	else {
-		ENABLE_EXT_DYN = false;
-	}
-	tSimuEnd = Config_c->SimulationSetup.SimulationEndTime;
-
-	// get vehicle message that needs to be sent out
-	VehicleMessageField_v = Config_c->SimulationSetup.VehicleMessageField;
-
-	for (int i = 0; i < VehicleMessageField_v.size(); i++) {
-		VehicleMessageField_set.insert(VehicleMessageField_v[i]);
-	}
-
-	// get subscription information
-	// variable to store subscription that need to check
-	// if application layer is disabled, xil is enabled then use subscription of xil, this means traffic layer directly connects to xil
-	if (!Config_c->ApplicationSetup.EnableApplicationLayer && Config_c->XilSetup.EnableXil) {
-		Config_c->getVehSubscriptionList(Config_c->XilSetup.VehicleSubscription, edgeSubscribeId_v, vehicleSubscribeId_v, subscribeAllVehicle, pointSubscribeId_v, vehicleTypeSubscribedId_v);
-	}
-	// otherwise find out the subscription of application layer
-	else {
-		Config_c->getVehSubscriptionList(Config_c->ApplicationSetup.VehicleSubscription, edgeSubscribeId_v, vehicleSubscribeId_v, subscribeAllVehicle, pointSubscribeId_v, vehicleTypeSubscribedId_v);
-	}
-	
-	vehicleHasSubscribed_v.clear();
-	vehicleHasSubscribed_v.resize(vehicleSubscribeId_v.size());
-	fill(vehicleHasSubscribed_v.begin(), vehicleHasSubscribed_v.end(), false);
-
-	Config_c->CarMakerSetup.TrafficRefreshRate = 0.123;
-
-	int aa = 1;
-}
-
-
-int TrafficHelper::sendToTrafficSimulator(double simTime, MsgHelper Msg_c) {
-	
-	int sendStatus = 0;
-
-	if (SUMO_OR_VISSIM.compare("SUMO") == 0) {
-		sendStatus = this->sendToSUMO(simTime, Msg_c);
-	}
-	else if (SUMO_OR_VISSIM.compare("VISSIM") == 0) {
-		sendStatus = this->sendToVISSIM(simTime, Msg_c);
-	}
-	else {
-
-	}
-
-	return sendStatus;
-}
-
-
-int TrafficHelper::addEgoVehicle(double simTime) {
-
-	if (SUMO_OR_VISSIM.compare("SUMO") == 0) {
-		if (ENABLE_VEH_SIMULATOR) {
-			// !!!!check if what received is ego vehicle
-			// use default type if not specified!!
-			string idStr = Config_c->CarMakerSetup.EgoId;
-
-			// Get all vehicle IDs from SUMO
-			vector<string> vehicleIds = SUMO_TRACI_NAMESPACE::Vehicle::getIDList();
-
-			// Check if ego vehicle already exists
-			bool vehicleExist = (find(vehicleIds.begin(), vehicleIds.end(), idStr) != vehicleIds.end());
-
-			// if ego not exist yet, add it
-			if (!vehicleExist) {
-				string typeStr = Config_c->CarMakerSetup.EgoType;
-
-				// if is empty
-				if (typeStr.size() == 0) {
-					SUMO_TRACI_NAMESPACE::Vehicle::add(idStr, "");
-				}
-				else {
-					SUMO_TRACI_NAMESPACE::Vehicle::add(idStr, "", typeStr);
-				}
-				SUMO_TRACI_NAMESPACE::Vehicle::setColor(idStr, libsumo::TraCIColor(255, 0, 0));
-			}
-		}
-
-		return 1;
-	}
-	else {
-		return 0;
-	}
-
-}
-
-int TrafficHelper::addEgoVehicleFromXY(double simTime, std::string vehicleId, std::string vehicleType, double positionX, double positionY) {
-
-	if (SUMO_OR_VISSIM.compare("SUMO") == 0) {
-		if (ENABLE_VEH_SIMULATOR) {
-			// Map the x y positon to an edge for spawing the ego vehicle
-			libsumo::TraCIRoadPosition edgePosition = SUMO_TRACI_NAMESPACE::Simulation::convertRoad(positionX, positionY, false);
-			// Create a dummy route for the ego vehicle
-			std::string dummyedgeID = edgePosition.edgeID;
-			double lanePos = edgePosition.pos; // position along the edge
-			int laneIndex = edgePosition.laneIndex;
-			std::string dummyRouteId = "route_" + vehicleId;
-			std::vector<std::string> dummyRoute;
-			dummyRoute.push_back(dummyedgeID);
-			SUMO_TRACI_NAMESPACE::Route::add(dummyRouteId, dummyRoute);
-
-			SUMO_TRACI_NAMESPACE::Vehicle::add(vehicleId, dummyRouteId, vehicleType);
-			SUMO_TRACI_NAMESPACE::Vehicle::setColor(vehicleId, libsumo::TraCIColor(255, 0, 0));
-		}
-
-		return 1;
-	}
-	else {
-		return 0;
-	}
-
-}
-
-
-int TrafficHelper::checkIfEgoExist(double* simTime) {
-
-	if (SUMO_OR_VISSIM.compare("SUMO") == 0) {
-		*simTime = SUMO_TRACI_NAMESPACE::Simulation::getTime();
-		vector <string> VehIdInSimulator = SUMO_TRACI_NAMESPACE::Vehicle::getIDList();
-
-		// check if subscribed vheicle is in the network
-		for (auto& iter : vehicleSubscribeId_v) {
-			string idStr = iter.first;
-
-			// if any one of vehicle has not been subscribed yet
-			if (find(VehIdInSimulator.begin(), VehIdInSimulator.end(), idStr) != VehIdInSimulator.end()) {
-				return 1;
-			}
-			else {
-				return 0;
-			}
-
-			// only check the first vehicle, which considered as the ego vehicle
-			// break;
-		}
-	}
-	else {
-		return 0;
-	}
-
-}
-
-int TrafficHelper::getSimulationTime(double* simTime) {
-	if (SUMO_OR_VISSIM.compare("SUMO") == 0) {
-		*simTime = SUMO_TRACI_NAMESPACE::Simulation::getTime();
-		return 1;
-	}
-	else {
-		return 0;
-	}
-}
-
-int TrafficHelper::runSimulation(double endTime) {
-	if (SUMO_OR_VISSIM.compare("SUMO") == 0) {
-		SUMO_TRACI_NAMESPACE::Simulation::step(endTime);
-		return 1;
-	}
-	else {
-		return 0;
-	}
-}
-
-
-int TrafficHelper::sendToSUMO(double simTime, MsgHelper Msg_c) {
-
-	int sendStatus = 0;
-
-	if (ENABLE_VERBOSE) {
-		printf("sending to SUMO\n ");
-
-		FILE* f = fopen(MasterLogName.c_str(), "a");
-		fprintf(f, "sending to SUMO\n ");
-		fclose(f);
-
-	}
-
-	//int nVeh = VehIdInSimulator.size();
-
-	//if (nVeh > 0) {
-	//	for (int iV = 0; iV < nVeh; iV++) {
-	//		string idStr = VehIdInSimulator[iV];
-	//		//unsigned int id = stoi(idStr.substr(12));
-
-	//		//if (idStr.compare("flow_0.10") == 0) {
-	//		//	traci.vehicle.setColor(idStr, libsumo::TraCIColor(255, 0, 0, 255));
-	//		//}
-	//		//if (idStr.compare("flow_1.9") == 0) {
-	//		//	traci.vehicle.setColor(idStr, libsumo::TraCIColor(0, 166, 255, 255));
-	//		//}
-
-	//		//if (std::find(Msg_c.VehIdRecv_v.begin(), Msg_c.VehIdRecv_v.end(), idStr) != Msg_c.VehIdRecv_v.end()) {
-	//		if (Msg_c.VehDataRecv_um.find(idStr) != Msg_c.VehDataRecv_um.end()) {
-	//			traci.vehicle.setSpeed(idStr, Msg_c.VehDataRecv_um[idStr].speed);
-	//			traci.vehicle.setSpeedMode(idStr, 0); // most checks off
-
-
-	//			if (ENABLE_VERBOSE) {
-	//				//cout << "setSpeed ego # " << idStr << " speed " << VehCmdRecv_um[idStr].speed << endl;
-	//				printf("setSpeed ego %s speed %.4f\n", idStr.c_str(), Msg_c.VehDataRecv_um[idStr].speed);
-	//				//cout << "setSpeed ego # " << idStr << " speed " << VehCmdRecv_um[idStr].speed << '\n';
-	//			}
-	//		}
-	//		else {
-	//			//traci.vehicle.setSpeedMode(idStr, 31);
-	//		}
-	//	}
-
-	//	if (Msg_c.TlsIdRecv_v.size() > 0) {
-	//		if (Msg_c.TlsDataRecv_um.find(tlsSelect) != Msg_c.TlsDataRecv_um.end()) {
-	//			string setTlsState = Msg_c.TlsDataRecv_um[tlsSelect].state;
-	//			traci.trafficlights.setRedYellowGreenState(tlsSelect, setTlsState);
-
-	//			if (ENABLE_VERBOSE) {
-	//				//cout << "set " << " time " << simTime << " traffic light " << tlsSelect << " state " << setTlsState << endl;
-	//				printf("set time %.1f traffic light %s state %s \n", simTime, tlsSelect.c_str(), setTlsState.c_str());
-	//			}
-	//		}
-	//	}
-	//}
-
-	try {
-		vector <string> VehIdInSimulator = SUMO_TRACI_NAMESPACE::Vehicle::getIDList();
-		//uint32_t color = 4278190335;
-		//uint8_t r = (color >> 24) & 0xFF;
-		//uint8_t g = (color >> 16) & 0xFF;
-		//uint8_t b = (color >> 8) & 0xFF;
-		//uint8_t a = (color) & 0xFF;
-		//traci.vehicle.setColor("flow_0.0", libsumo::TraCIColor(r, g, b, a));
-
-		for (int iV = 0; iV < VehIdInSimulator.size(); iV++) {
-			//traci.vehicle.setSpeedMode(VehIdInSimulator[iV], 31); // default speed mode
-		}
-
-		for (int iV = 0; iV < Msg_c.VehDataSend_um[0].size(); iV++) {
-			string idStr = Msg_c.VehDataSend_um[0][iV].id;
-			
-			double speed;
-			if (VehicleMessageField_set.find("speedDesired") != VehicleMessageField_set.end()) {
-				speed = Msg_c.VehDataSend_um[0][iV].speedDesired;
-			}
-			else {
-				//double accel = std::any_cast<float>(Msg_c.VehDataSend_um[0][iV]["accelerationDesired"]);
-				//speed = 
-				printf("ERROR: SUMO does not support control by accelerationDesired yet\n");
-				printf("\tPlease select speedDesired in VehicleMessageField instead\n");
-				exit(-1);
-			}
-
-
-			if (ENABLE_VERBOSE) {
-				if (!ENABLE_VEH_SIMULATOR && find(VehIdInSimulator.begin(), VehIdInSimulator.end(), idStr) != VehIdInSimulator.end()) {
-					double speedOld = SUMO_TRACI_NAMESPACE::Vehicle::getSpeed(idStr);
-					printf("Set SUMO id %s from speed %.4f to speed %.4f\n", idStr.c_str(), speedOld, speed);
-
-					FILE* f = fopen(MasterLogName.c_str(), "a");
-					fprintf(f, "Set SUMO id %s from speed %.4f to speed %.4f\n", idStr.c_str(), speedOld, speed);
-					fclose(f);
-				}
-			}
-
-			// if vehicle simulator and is ego
-			if (ENABLE_VEH_SIMULATOR && idStr.compare(Config_c->CarMakerSetup.EgoId) == 0) {
-				// !!!!check if what received is ego vehicle 
-				// use default type if not specified!!
-				
-				//// if ego not exist yet, add it
-				//if (find(VehIdInSimulator.begin(), VehIdInSimulator.end(), idStr) == VehIdInSimulator.end()) {
-				//	string typeStr = Msg_c.VehDataSend_um[0][iV].type;
-
-				//	// if is empty
-				//	if (typeStr.size() == 0) {
-				//		traci.vehicle.add(idStr, "");
-				//	}
-				//	else {
-				//		traci.vehicle.add(idStr, "", typeStr);
-				//	}
-				//	traci.vehicle.setColor(idStr, libsumo::TraCIColor(255, 0, 0));
-				//}
-				// otherwise, move it
-				{
-					if (ENABLE_EXT_DYN) {
-						SUMO_TRACI_NAMESPACE::Vehicle::setPreviousSpeed(idStr, speed); // setting speed at (k) will be reflected at (k) "immediately", i.e., be considered in the next integration
-					}
-					else {
-
-						double positionX = (double)Msg_c.VehDataSend_um[0][iV].positionX;
-						double positionY = (double)Msg_c.VehDataSend_um[0][iV].positionY;
-						double positionZ = (double)Msg_c.VehDataSend_um[0][iV].positionZ;
-						double heading = (double)Msg_c.VehDataSend_um[0][iV].heading;
-
-						SUMO_TRACI_NAMESPACE::Vehicle::moveToXY(idStr, "", -1, positionX, positionY, heading, 6); // keepRoute 110 => 6
-						//bit0(keepRoute = 1 when only this bit is set)
-							//1: The vehicle is mapped to the closest edge within it's existing route. If no suitable position is found within 100m mapping fails with an error.
-							//0 : The vehicle is mapped to the closest edge within the network.If that edge does not belong to the original route, the current route is replaced by a new route which consists of that edge only.If no suitable position is found within 100m mapping fails with an error.When using the sublane model the best lateral position that is fully within the lane will be used.Otherwise, the vehicle will drive in the center of the closest lane.
-						//bit1(keepRoute = 2 when only this bit is set)
-							//1 : The vehicle is mapped to the exact position in the network(including the exact lateral position).If that position lies outside the road network, the vehicle stops moving on it's own accord until it is placed back into the network with another TraCI command. (if keeproute = 3, the position must still be within 100m of the vehicle route)
-							//0 : The vehicle is always on a road
-						//bit2(keepRoute = 4 when only this bit is set)
-							//1 : lane permissions are ignored when mapping
-							//0 : The vehicle is mapped only to lanes that allow it's vehicle class
-
-					}
-
-					if (VehicleMessageField_set.find("lightIndicators") != VehicleMessageField_set.end()) {
-						SUMO_TRACI_NAMESPACE::Vehicle::setSignals(idStr, (int)Msg_c.VehDataSend_um[0][iV].lightIndicators);
-					}
-
-				}
-			}
-			// if carla is enabled and the reveiced id is within the interested ids
-			else if (ENABLE_CARLA&&ENABLE_CARLA_EXTERNAL_CONTROL&&find(Config_c->CarlaSetup.InterestedIds.begin(), Config_c->CarlaSetup.InterestedIds.end(), idStr) != Config_c->CarlaSetup.InterestedIds.end()) {
-
-				double positionX = (double)Msg_c.VehDataSend_um[0][iV].positionX;
-				double positionY = (double)Msg_c.VehDataSend_um[0][iV].positionY;
-				double positionZ = (double)Msg_c.VehDataSend_um[0][iV].positionZ;
-				double heading = (double)Msg_c.VehDataSend_um[0][iV].heading;
-				string vehicleType = Msg_c.VehDataSend_um[0][iV].type;
-				// If the Intertested Vehicle is not in sumo
-				bool vehicleExist = false;
-				for (const std::string& vehId : VehIdInSimulator) {
-					if (vehId == idStr) {
-						vehicleExist = true;
-					}
-				}
-				if (!vehicleExist) {
-					addEgoVehicleFromXY(simTime, idStr, vehicleType, positionX, positionY);
-				}
-				SUMO_TRACI_NAMESPACE::Vehicle::moveToXY(idStr, "", -1, positionX, positionY, heading, 6);
-				SUMO_TRACI_NAMESPACE::Vehicle::setSpeed(idStr, speed);
-
-			}
-			else {
-				if (1 && find(VehIdInSimulator.begin(), VehIdInSimulator.end(), idStr) != VehIdInSimulator.end()) {
-					if (ENABLE_EXT_DYN) {
-						SUMO_TRACI_NAMESPACE::Vehicle::setPreviousSpeed(idStr, speed); // setting speed at (k) will be reflected at (k) "immediately", i.e., be considered in the next integration
-					}
-					else {
-						SUMO_TRACI_NAMESPACE::Vehicle::setSpeed(idStr, speed); // speed set at (k) essentially will be reflected at (k+1), not considered in the integration
-
-						/*
-						bit0: Regard safe speed
-						bit1 : Regard maximum acceleration
-						bit2 : Regard maximum deceleration
-						bit3 : Regard right of way at intersections(only applies to approaching foe vehicles outside the intersection)
-						bit4 : Brake hard to avoid passing a red light
-						bit5 : Disregard right of way within intersections(only applies to foe vehicles that have entered the intersection).
-						*/
-
-						SUMO_TRACI_NAMESPACE::Vehicle::setSpeedMode(idStr, Config_c->SumoSetup.SpeedMode); // 000000 most checks off
-						//SUMO_TRACI_NAMESPACE::Vehicle::setSpeedMode(idStr, 0); // 000000 most checks off
-						//SUMO_TRACI_NAMESPACE::Vehicle::setSpeedMode(idStr, 24); // 011000
-						//SUMO_TRACI_NAMESPACE::Vehicle::setSpeedMode(idStr, 8); // 001000
-
-						//SUMO_TRACI_NAMESPACE::Vehicle::setSpeedFactor(idStr, 1);
-					}
-
-					// change vehicle color if needed
-					if (VehicleMessageField_set.find("color") != VehicleMessageField_set.end()) {
-						uint32_t color = Msg_c.VehDataSend_um[0][iV].color;
-						uint8_t r = (color >> 24) & 0xFF;
-						uint8_t g = (color >> 16) & 0xFF;
-						uint8_t b = (color >> 8) & 0xFF;
-						uint8_t a = (color) & 0xFF;
-						SUMO_TRACI_NAMESPACE::Vehicle::setColor(idStr, libsumo::TraCIColor(r, g, b, a));
-					}
-
-				}
-			}
-
-
-		}
-
-
-		if (ENABLE_SIG_SUB) {
-			for (int iS = 0; iS < Msg_c.TlsDataSend_um[0].size(); iS++) {
-				string idStr = Msg_c.TlsDataSend_um[0][iS].name;
-
-				SUMO_TRACI_NAMESPACE::TrafficLight::setRedYellowGreenState(idStr, Msg_c.TlsDataSend_um[0][iS].state);
-			}
-		}
-
-
-		if (ENABLE_VERBOSE) {
-			printf("send SUMO complete\n ");
-
-			FILE* f = fopen(MasterLogName.c_str(), "a");
-			fprintf(f, "send SUMO complete\n ");
-			fclose(f);
-		}
-
-	}
-	catch (const std::exception& e) {
-		std::cout << e.what();
-		return -1;
-	}
-	catch (...) {
-		printf("UNKNOWN ERROR: send to SUMO fails\n");
-		return -1;
-	}
-
-		//if (Msg_c.TlsIdRecv_v.size() > 0) {
-		//	if (Msg_c.TlsDataRecv_um.find(tlsSelect) != Msg_c.TlsDataRecv_um.end()) {
-		//		string setTlsState = Msg_c.TlsDataRecv_um[tlsSelect].state;
-		//		traci.trafficlights.setRedYellowGreenState(tlsSelect, setTlsState);
-
-		//		if (ENABLE_VERBOSE) {
-		//			//cout << "set " << " time " << simTime << " traffic light " << tlsSelect << " state " << setTlsState << endl;
-		//			printf("set time %.1f traffic light %s state %s \n", simTime, tlsSelect.c_str(), setTlsState.c_str());
-		//		}
-		//	}
-		//}
-
-
-	return sendStatus;
-}
-
-int TrafficHelper::sendToVISSIM(double simTime, MsgHelper Msg_c) {
-
-	int sendStatus = 0;
-
-	SocketHelper Sock_temp;
-	
-	try {
-		for (int iClient = 0; iClient < vissimSock.size(); iClient++) {
-			if (ENABLE_VERBOSE) {
-				printf("send to VISSIM %s\n", vissimSockName_um[vissimSock[iClient]].c_str());
-
-				FILE* f = fopen(MasterLogName.c_str(), "a");
-				fprintf(f, "send to VISSIM %s\n", vissimSockName_um[vissimSock[iClient]].c_str());
-				fclose(f);
-
-			}
-
-			//if (vissimSock.size() > 1 && vissimSockName_um[vissimSock[i]].find(SIGNAL_SOCK_PATTERN) != string::npos) {
-			//	// if is signal controller
-			//	if (abs(round(simTime / (vissimSignalStep * vissimBaseDt)) - simTime / (vissimSignalStep * vissimBaseDt)) < 1e-5) {
-			//		Sock_temp.sendData(vissimSock[i], i, simTime, 1, Msg_c);
-			//	}
-			//}
-			//else {
-			//	Sock_temp.sendData(vissimSock[i], i, simTime, 1, Msg_c);
-			//}
-			sendStatus = Sock_temp.sendData(vissimSock[iClient], iClient, simTime, 1, Msg_c);
-		}
-
-		if (ENABLE_VERBOSE) {
-			printf("send VISSIM complete\n");
-
-			FILE* f = fopen(MasterLogName.c_str(), "a");
-			fprintf(f, "send VISSIM complete\n");
-			fclose(f);
-
-		}
-
-	}
-	catch (const std::exception& e) {
-		std::cout << e.what();
-		return -1;
-	}
-	catch (...) {
-		printf("UNKNOWN ERROR: send to VISSIM fails\n");
-		return -1;
-	}
-
-	return sendStatus;
-
-}
-
-void TrafficHelper::parseSendMsg(MsgHelper MsgIn_c, MsgHelper& MsgOut_c) {
-
-	// MsgClient_c/MsgIn recv => MsgServer_c/MsgOut send
-
-	if (SUMO_OR_VISSIM.compare("SUMO") == 0) {
-		// SUMO use a dummy socket index 0
-		MsgOut_c.VehDataSend_um[0] = {}; 
-		MsgOut_c.TlsDataSend_um[0] = {}; 
-		MsgOut_c.DetDataSend_um[0] = {}; 
-		for (auto it : MsgIn_c.VehDataRecv_um) {
-			MsgOut_c.VehDataSend_um[0].push_back(it.second);
-		}
-		for (auto it : MsgIn_c.TlsDataRecv_um) {
-			MsgOut_c.TlsDataSend_um[0].push_back(it.second);
-		}
-		for (auto it : MsgIn_c.DetDataRecv_um) {
-			MsgOut_c.DetDataSend_um[0].push_back(it.second);
-		}
-	}
-	else if (SUMO_OR_VISSIM.compare("VISSIM") == 0) {
-		for (int i = 0; i < vissimSock.size(); i++) {
-			// if current socket is signal, then only send signal
-			if (vissimSockName_um[vissimSock[i]].find(SIGNAL_SOCK_PATTERN) != string::npos) {
-				MsgOut_c.VehDataSend_um[vissimSock[i]] = {};
-				MsgOut_c.TlsDataSend_um[vissimSock[i]] = {};
-				MsgOut_c.DetDataSend_um[vissimSock[i]] = {};
-				for (auto it : MsgIn_c.TlsDataRecv_um) {
-					MsgOut_c.TlsDataSend_um[vissimSock[i]].push_back(it.second);
-				}
-				for (auto it : MsgIn_c.DetDataRecv_um) {
-					MsgOut_c.DetDataSend_um[vissimSock[i]].push_back(it.second);
-				}
-			}
-			else {
-				MsgOut_c.VehDataSend_um[vissimSock[i]] = {};
-				MsgOut_c.TlsDataSend_um[vissimSock[i]] = {};
-				MsgOut_c.DetDataSend_um[vissimSock[i]] = {};
-				for (auto it : MsgIn_c.VehDataRecv_um) {
-					MsgOut_c.VehDataSend_um[vissimSock[i]].push_back(it.second);
-				}
-			}
-		}
-	}
-	
-}
-
-void TrafficHelper::runOneStepSimulation() {
-	if (SUMO_OR_VISSIM.compare("SUMO") == 0) {
-		SUMO_TRACI_NAMESPACE::Simulation::step();
-	}
-	else if (SUMO_OR_VISSIM.compare("VISSIM") == 0) {
-
-	}
-	else {
-
-	}
-}
-
-int TrafficHelper::recvFromTrafficSimulator(double* simTime, MsgHelper& Msg_c) {
-	int recvStatus = 0;
-
-	if (SUMO_OR_VISSIM.compare("SUMO") == 0) {
-		recvStatus = this->recvFromSUMO(simTime, Msg_c);
-	}
-	else if (SUMO_OR_VISSIM.compare("VISSIM") == 0) {
-		recvStatus = this->recvFromVISSIM(simTime, Msg_c);
-	}
-	else {
-
-	}
-
-	return recvStatus;
-}
-
-int TrafficHelper::recvFromSUMO(double* simTime, MsgHelper& Msg_c) {
-	int recvStatus = 0;
-
-	Msg_c.clearSendStorage();
-
-	VehIdInSimulator.clear();
-
-	*simTime = SUMO_TRACI_NAMESPACE::Simulation::getTime();
-	VehIdInSimulator = SUMO_TRACI_NAMESPACE::Vehicle::getIDList();
-	
-	int nVeh = VehIdInSimulator.size(); // number of vehicles
-
-	if (ENABLE_VERBOSE) {
-		printf("SUMO time step %f\n", *simTime);
-
-		FILE* f = fopen(MasterLogName.c_str(), "a");
-		fprintf(f, "SUMO time step %f\n", *simTime);
-		fclose(f);
-
-	}
-
-	//=================
-	// SUMO will have vehicles except for the first few time steps
-	//=================
-	int nVehSend = 0;
-
-	if (ENABLE_VEH_SUB && nVeh > 0) {
-
-		// only subscribe once
-		if (!edgeHasSubscribed) {
-			// (self, objectID, domain, dist, varIDs=None, begin=-1073741824.0, end=-1073741824.0, parameters=None)
-
-
-			// -------------------
-			//  subscribe edge
-			// -------------------
-			for (auto& iter: edgeSubscribeId_v) {
-				double radius = 0;
-				string id = iter;
-
-				SUMO_TRACI_NAMESPACE::Edge::subscribeContext(id, libsumo::CMD_GET_VEHICLE_VARIABLE, 100, VehDataSubscribeList, 0, tSimuEnd);
-			}
-
-			edgeHasSubscribed = true;
-
-		}
-
-		if (!pointHasSubscribed) {
-			// (self, objectID, domain, dist, varIDs=None, begin=-1073741824.0, end=-1073741824.0, parameters=None)
-
-			// -------------------
-			//  subscribe point
-			// -------------------
-			//pointNamePoi_v.clear();
-			int i = 0; 
-			for (auto& iter: pointSubscribeId_v) {
-				double x = get<0>(iter.second);
-				double y = get<1>(iter.second);
-				double z = get<2>(iter.second);
-				double r = get<3>(iter.second);
-
-				/*Both polygonsand points of interest may be located within a "layer".Shapes with lowest layer values are below those with a higher layer number.The network itself is drawn as layer 0. An additional file may contain definitions for both points of interestand polygons.*/
-				//imgFile	string	A bitmap to use for rendering this polygon
-				//angle	float	angle of rendered image in degree
-				//lineWidth	double	Drawing width of unfilled polygons in m, default 1
-
-				//string poiName = "RealSimPOI_"+to_string(i);
-				string poiName = iter.first;
-				/*int 	r,
-					int 	g,
-					int 	b,
-					int 	a = 255*/
-				libsumo::TraCIColor color(255,0,255); // use magenta as color
-				string type = "RealSim";
-				int layer = 999; // put it as a very high layer
-				string imgFile = ""; // no image file
-				double width = 0; // width	float	width of rendered image in meters
-				double height = 0; // height	float	height of rendered image in meters
-				double angle = 0; // angle	float	angle of rendered image in degree
-				SUMO_TRACI_NAMESPACE::POI::add(poiName, x, y, color, type, layer, imgFile, width, height, angle);
-				SUMO_TRACI_NAMESPACE::POI::subscribeContext(poiName, libsumo::CMD_GET_VEHICLE_VARIABLE, r, VehDataSubscribeList, 0, tSimuEnd);
-
-				//pointNamePoi_v.push_back(poiName);
-				i++;
-			}
-
-			pointHasSubscribed = true;
-
-		}
-
-		if (!allVehicleHasSubscribed) {
-			// -------------------
-			//  subscribe vehicle
-			// -------------------
-			
-			// get list of all vehicles entered network
-			vector <string> vehDepartedId_v = SUMO_TRACI_NAMESPACE::Simulation::getDepartedIDList();
-			allVehicleHasSubscribed = true;
-			// only able to get vehicle subscription for vehicles already in the network
-			int i = 0;
-			for (auto & iter: vehicleSubscribeId_v) {
-				// if any one of vehicle has not been subscribed yet
-				if (!vehicleHasSubscribed_v[i]) {
-					allVehicleHasSubscribed = false;
-					// id of the vehicle to be subscribed
-					string id = iter.first;
-					// if the vehicle to subscribe just entered the network
-					if (find(vehDepartedId_v.begin(), vehDepartedId_v.end(), id)!=vehDepartedId_v.end()) {
-						double radius = iter.second;
-
-						SUMO_TRACI_NAMESPACE::Vehicle::subscribeContext(id, libsumo::CMD_GET_VEHICLE_VARIABLE, radius, VehDataSubscribeList, 0, tSimuEnd);
-
-						vehicleHasSubscribed_v[i] = true;
-					}
-
-					i++;
-				}
-
-			}
-
-			// !!! need to sub all vehicles
-
-			//while Simulation::getMinExpectedNumber() > 0:
-			//for veh_id in Simulation::getDepartedIDList() :
-			//	traci.vehicle.subscribe(veh_id, [traci.constants.VAR_POSITION])
-			//	positions = traci.vehicle.getAllSubscriptionResults()
-			//	traci.simulationStep()
-
-
-		}
-		else {
-			int aa = 1;
-
-		}
-
-		// this might make it slightly faster to not get repeated vehicles
-		unordered_set <string> processedVehId_us;
-
-		// temp buffer to store all vehicle received
-		std::unordered_map <std::string, VehFullData_t> VehDataRecv_um_tmp;
-		// ===========================================================================
-		// 			GET SUBSCRIBED VEHICLE
-		// ===========================================================================
-		libsumo::ContextSubscriptionResults VehicleSubscribeRaw;
-		VehicleSubscribeRaw = SUMO_TRACI_NAMESPACE::Vehicle::getAllContextSubscriptionResults();
-
-		//{
-		//int i = 0;
-		//for (auto& it : vehicleSubscribeId_v) {
-		//	if (vehicleHasSubscribed_v[i]) {
-		for (auto& it : VehicleSubscribeRaw) {
-			//string id = it.first;
-			libsumo::SubscriptionResults VehDataSubscribeResults = it.second;
-
-			for (auto& iter : VehDataSubscribeResults) {
-
-				string tempvehId = iter.first;
-
-				VehFullData_t CurVehData;
-
-				if (processedVehId_us.find(tempvehId) == processedVehId_us.end()) {
-					processedVehId_us.insert(tempvehId);
-				}
-				else {
-					continue;
-				}
-
-				this->parserSumoSubscription(iter.second, tempvehId, CurVehData);
-				//libsumo::TraCIResults VehDataSubscribeTraciResults = VehDataSubscribeResults[tempvehId]
-
-				//=================
-				// save to Msg_c recv buffer
-				//=================
-				//Msg_c.VehDataRecvAll_v.push_back(CurVehData);
-				VehDataRecv_um_tmp[tempvehId] = CurVehData;
-
-				if (ENABLE_VERBOSE) {
-					float speed = CurVehData.speed;
-					printf("recv SUMO veh id %s veh speed %.4f\n", tempvehId.c_str(), speed);
-
-					FILE* f = fopen(MasterLogName.c_str(), "a");
-					fprintf(f, "recv SUMO veh id %s veh speed %.4f\n", tempvehId.c_str(), speed);
-					fclose(f);
-
-				}
-
-			}
-		}
-
-	/*		i++;
-
-		}
-		}*/
-
-		// ===========================================================================
-		// 			GET SUBSCRIBED point
-		// ===========================================================================
-		libsumo::ContextSubscriptionResults PointSubscribeRaw;
-		PointSubscribeRaw = SUMO_TRACI_NAMESPACE::POI::getAllContextSubscriptionResults();
-
-		for (auto& it : PointSubscribeRaw) {
-			string poiName = it.first;
-			libsumo::SubscriptionResults VehDataSubscribeResults = it.second;
-
-			for (auto& iter : VehDataSubscribeResults) {
-
-				string tempvehId = iter.first;
-
-				VehFullData_t CurVehData;
-
-				if (processedVehId_us.find(tempvehId) == processedVehId_us.end()) {
-					processedVehId_us.insert(tempvehId);
-				}
-				else {
-					continue;
-				}
-
-				this->parserSumoSubscription(iter.second, tempvehId, CurVehData);
-				//libsumo::TraCIResults VehDataSubscribeTraciResults = VehDataSubscribeResults[tempvehId]
-
-				//=================
-				// save to Msg_c recv buffer
-				//=================
-				//Msg_c.VehDataRecvAll_v.push_back(CurVehData);
-				VehDataRecv_um_tmp[tempvehId] = CurVehData;
-
-				if (ENABLE_VERBOSE) {
-					float speed = CurVehData.speed;
-					printf("recv SUMO veh id %s veh speed %.4f\n", tempvehId.c_str(), speed);
-
-					FILE* f = fopen(MasterLogName.c_str(), "a");
-					fprintf(f, "recv SUMO veh id %s veh speed %.4f\n", tempvehId.c_str(), speed);
-					fclose(f);
-				}
-			}
-		}
-
-
-		// ===========================================================================
-		// 			GET SUBSCRIBED EDGE
-		// ===========================================================================
-		libsumo::ContextSubscriptionResults EdgeSubscribeRaw;
-		EdgeSubscribeRaw = SUMO_TRACI_NAMESPACE::Edge::getAllContextSubscriptionResults();
-
-
-		if (edgeHasSubscribed) {
-			nVehSend = 0;
-
-			int temp = 1;
-
-			for (auto & it: EdgeSubscribeRaw) {
-				libsumo::SubscriptionResults VehDataSubscribeResults = it.second;
-
-				auto iter = VehDataSubscribeResults.begin();
-				nVehSend = min((int)VehDataSubscribeResults.size(), 200);
-
-				vector <string> tempVehIdList;
-				//tempVehIdList.push_back(egoIdVec[iC]);
-				for (int iV = 0; iV < VehDataSubscribeResults.size(); iV++) {
-					//if (iter->first != egoIdVec[iC]) {
-					tempVehIdList.push_back(iter->first);
-					//}
-					iter++;
-				}
-
-				for (int iV = 0; iV < nVehSend; iV++) {
-					VehFullData_t CurVehData;
-
-					string tempvehId = tempVehIdList[iV];
-
-					if (processedVehId_us.find(tempvehId) == processedVehId_us.end()) {
-						processedVehId_us.insert(tempvehId);
-					}
-					else {
-						continue;
-					}
-
-					this->parserSumoSubscription(VehDataSubscribeResults[tempvehId], tempvehId, CurVehData);
-					//libsumo::TraCIResults VehDataSubscribeTraciResults = VehDataSubscribeResults[tempvehId]
-
-
-
-					//Msg_c.packVehData(CurVehData, tempVehDataBuffer[iC], &tempVehDataByte[iC]);
-					//tempVehDataSend_v.push_back(CurVehData);
-
-					//tempVehIdSend_v.push_back(tempvehId);
-
-					//=================
-					// save to Msg_c recv buffer          
-					//=================
-					//Msg_c.VehDataRecvAll_v.push_back(CurVehData);
-					VehDataRecv_um_tmp[tempvehId] = CurVehData;
-
-					if (ENABLE_VERBOSE) {
-						float speed = CurVehData.speed;
-						printf("recv SUMO veh id %s veh speed %.4f\n", tempvehId.c_str(), speed);
-
-						FILE* f = fopen(MasterLogName.c_str(), "a");
-						fprintf(f, "recv SUMO veh id %s veh speed %.4f\n", tempvehId.c_str(), speed);
-						fclose(f);
-
-					}
-
-					//}
-					/*VehData_t iVehFullData;
-					Sock_c.depackVehData(tempVehDataBuffer[iC]+3, &iVehFullData);*/
-				}
-			}
-			//nVehSend = tempVehIdSend_v.size();
-		}
-
-
-		// !!!temporary fix
-		// if doing vehicle simulator, e.g., CarMaker, only send limited number of vehicles
-		if (ENABLE_VEH_SIMULATOR) {
-			libsumo::TraCIPosition posEgo = SUMO_TRACI_NAMESPACE::Vehicle::getPosition(Config_c->CarMakerSetup.EgoId);
-
-			// sort distance, pair distance to ego, vehId
-			vector <pair <double, string>> dist2ego_v;
-			for (auto& it : VehDataRecv_um_tmp) {
-				dist2ego_v.push_back(make_pair(pow(it.second.positionX - posEgo.x, 2) + pow(it.second.positionY - posEgo.y, 2), it.first));
-			}
-			sort(dist2ego_v.begin(), dist2ego_v.end());
-
-			for (auto&it: dist2ego_v) {
-				string tempvehId = it.second;
-
-				if (this->shouldSendVehicle(tempvehId, *simTime)) {
-					Msg_c.VehDataRecv_um[tempvehId] = VehDataRecv_um_tmp[tempvehId];
-				}
-
-				if (Msg_c.VehDataRecv_um.size() >= N_MAX_VEH) {
-					break;
-				}
-			}
-				
-		}
-		else {
-			for (auto& it : VehDataRecv_um_tmp) {
-				string tempvehId = it.first;
-
-				// if not doing driving simulator, shouldSendVehicle will currently always return 1
-				if (this->shouldSendVehicle(tempvehId, *simTime)) {
-					Msg_c.VehDataRecv_um[tempvehId] = VehDataRecv_um_tmp[tempvehId];
-				}
-			}
-		}
-
-
-		//=================
-		// remove vehicle from list
-		//=================
-		vector <string> vehArrivedIdList = SUMO_TRACI_NAMESPACE::Simulation::getArrivedIDList();
-		for (int i = 0; i < vehArrivedIdList.size(); i++) {
-			VehicleId2EdgeList_um.erase(vehArrivedIdList[i]);
-		}
-
-	}
-
-	//char tempDetDataBuffer[NCLIENT_MACRO][8096];
-	//int tempDetDataByte[NCLIENT_MACRO] = { 0 };
-
-	if (ENABLE_DET_SUB) {
-		if (!DetectorSubscriptionFlags.patternHasSubscribed) {
-			//===================================================
-			// Retreive DETECTOR configuration for the scenario BEFORE simulation starts
-			//===================================================
-			vector <string> detAreaAllId_v = SUMO_TRACI_NAMESPACE::LaneArea::getIDList();
-			vector <string> detInductAllId_v = SUMO_TRACI_NAMESPACE::InductionLoop::getIDList();
-
-			// obtain detector ids of the selected intersection and subscribe to results
-
-			vector <int> detSubscribeList = {libsumo::LAST_STEP_VEHICLE_NUMBER};
-
-			for (auto it: Config_c->SubscriptionDetectorList.pattern_v){
-				for (int iD = 0; iD < detAreaAllId_v.size(); iD++) {
-					if (detAreaAllId_v[iD].find(it) != std::string::npos) {
-						SUMO_TRACI_NAMESPACE::LaneArea::subscribe(detAreaAllId_v[iD], detSubscribeList, 0, tSimuEnd);
-					}
-				}
-			}
-
-			DetectorSubscriptionFlags.patternHasSubscribed = true;
-
-		}
-
-		libsumo::SubscriptionResults DetSubscribeRaw;
-		DetSubscribeRaw = SUMO_TRACI_NAMESPACE::LaneArea::getAllSubscriptionResults();
-
-
-		vector <DetectorData_t> tempDetData_v;
-		
-		std::shared_ptr<libsumo::TraCIInt> tempIntPtr;
-
-		for (auto& it : DetSubscribeRaw) {
-			DetectorData_t curDet;
-
-			curDet.id = 0;
-			curDet.name = it.first;
-			tempIntPtr = static_pointer_cast<libsumo::TraCIInt> (it.second[libsumo::LAST_STEP_VEHICLE_NUMBER]);
-			if (tempIntPtr->value > 0) {
-				curDet.state = 1;
-			}
-			else {
-				curDet.state = 0;
-			};
-
-			tempDetData_v.push_back(curDet);
-		}
-
-		Msg_c.DetDataRecv_um["NA"] = make_tuple(0, "NA", tempDetData_v);
-
-	}
-
-	if (ENABLE_SIG_SUB) {
-		if (!SignalSubscriptionFlags.idHasSubscribed) {
-			//===================================================
-			// Retreive DETECTOR configuration for the scenario BEFORE simulation starts
-			//===================================================
-			vector <string> sigAllId_v = SUMO_TRACI_NAMESPACE::TrafficLight::getIDList();
-
-			// obtain detector ids of the selected intersection and subscribe to results
-
-			vector <int> sigSubscribeList = { libsumo::TL_RED_YELLOW_GREEN_STATE };
-
-			if (!Config_c->SubscriptionSignalList.subAllSignalFlag) {
-				for (auto it : Config_c->SubscriptionSignalList.signalId_v) {
-					SUMO_TRACI_NAMESPACE::TrafficLight::subscribe(it.c_str(), sigSubscribeList, 0, tSimuEnd);
-				}
-			}
-			else {
-				for (auto it : sigAllId_v) {
-					SUMO_TRACI_NAMESPACE::TrafficLight::subscribe(it.c_str(), sigSubscribeList, 0, tSimuEnd);
-				}
-			}
-
-			SignalSubscriptionFlags.idHasSubscribed = true;
-
-		}
-
-		// if already subscribed, then get signal data out
-		libsumo::SubscriptionResults SigSubscribeRaw;
-		SigSubscribeRaw = SUMO_TRACI_NAMESPACE::TrafficLight::getAllSubscriptionResults();
-
-		vector <TrafficLightData_t> tempSigData_v;
-
-		std::shared_ptr<libsumo::TraCIString> tempStringPtr;
-
-		int idx = 0;
-		for (auto& it : SigSubscribeRaw) {
-			TrafficLightData_t curSig;
-
-			curSig.id = idx++;
-			curSig.name = it.first;
-			tempStringPtr = static_pointer_cast<libsumo::TraCIString> (it.second[libsumo::TL_RED_YELLOW_GREEN_STATE]);
-			curSig.state = tempStringPtr->value;
-
-			Msg_c.TlsDataRecv_um[curSig.name] = curSig;
-
-		}
-
-
-
-	}
-
-	return recvStatus;
-}
-
-
-// this is only for visualization
-int TrafficHelper::shouldSendVehicle(std::string vehicleId, double simTime) {
-	int shouldSendFlag = 0;
-
-	if (!ENABLE_VEH_SIMULATOR) {
-		shouldSendFlag = 1;
-	}
-	else if (ENABLE_VEH_SIMULATOR){
-		if (vehicleId.compare(Config_c->CarMakerSetup.EgoId) == 0) {
-			shouldSendFlag = 1;
-		}
-		else {
-			// if does not have yet, add it
-			if (VehicleId2SubCount_um.find(vehicleId) == VehicleId2SubCount_um.end()) {
-				VehicleId2SubCount_um[vehicleId] = 0;
-			}
-			else {
-				// if last sub and this one is continous, then add count by 1, otherwise reset count
-				if (abs(VehicleId2LastSubTime_um[vehicleId] - simTime) <= SIM_STEP + 1e-5) {
-					VehicleId2SubCount_um[vehicleId] = VehicleId2SubCount_um[vehicleId] + 1;
-				}
-				else {
-					VehicleId2SubCount_um[vehicleId] = 0;
-				}
-			}
-			VehicleId2LastSubTime_um[vehicleId] = simTime;
-
-			// check last sub time
-			if (VehicleId2SubCount_um[vehicleId] > SUB_CONT_TIME_THLD / SIM_STEP) {
-				shouldSendFlag = 1;
-			}
-		}
-	}
-
-	return shouldSendFlag;
-}
-
-
-void TrafficHelper::parserSumoSubscription(libsumo::TraCIResults VehDataSubscribeTraciResults, std::string vehId, VehFullData_t& CurVehData) {
-
-	// if does not have this vehicle yet
-	if (VehicleId2EdgeList_um.find(vehId) == VehicleId2EdgeList_um.end()) {
-		vector <string> edgeList = SUMO_TRACI_NAMESPACE::Vehicle::getRoute(vehId);
-		VehicleId2EdgeList_um[vehId] = edgeList;
-		//vector <libsumo::TraCIConnection> nextLinkList = traci.vehicle.getNextLinks(vehId);
-		//int aa = 1;
-	}
-
-	//libsumo::TraCIResults VehDataSubscribeTraciResults = VehDataSubscribeResults[tempvehId];
-	std::shared_ptr<libsumo::TraCIString> tempStringPtr;
-	std::shared_ptr<libsumo::TraCIInt> tempIntPtr;
-	std::shared_ptr<libsumo::TraCIDouble> tempDoublePtr;
-	std::shared_ptr<libsumo::TraCIPosition> tempPositionPtr;
-	std::shared_ptr<libsumo::TraCIColor> tempColorPtr;
-	std::shared_ptr<libsumo::TraCIDouble> tempDoublePtr2;
-
-	CurVehData.id = vehId;
-
-	//for (int iD = 0; iD < VehDataSubscribeList.size(); iD++) {
-	tempStringPtr = static_pointer_cast<libsumo::TraCIString> (VehDataSubscribeTraciResults[libsumo::VAR_TYPE]);
-	CurVehData.type = tempStringPtr->value;
-
-	tempDoublePtr = static_pointer_cast<libsumo::TraCIDouble> (VehDataSubscribeTraciResults[libsumo::VAR_SPEED]);
-	CurVehData.speed = tempDoublePtr->value;
-
-	tempDoublePtr = static_pointer_cast<libsumo::TraCIDouble> (VehDataSubscribeTraciResults[libsumo::VAR_SPEED_WITHOUT_TRACI]);
-	float speedWithoutTraci = tempDoublePtr->value;
-
-
-	tempPositionPtr = static_pointer_cast<libsumo::TraCIPosition> (VehDataSubscribeTraciResults[libsumo::VAR_POSITION3D]);
-	CurVehData.positionX = tempPositionPtr->x;
-	CurVehData.positionY = tempPositionPtr->y;
-	CurVehData.positionZ = tempPositionPtr->z;
-
-	tempDoublePtr = static_pointer_cast<libsumo::TraCIDouble> (VehDataSubscribeTraciResults[libsumo::VAR_ANGLE]);
-	CurVehData.heading = tempDoublePtr->value;
-
-	tempColorPtr = static_pointer_cast<libsumo::TraCIColor> (VehDataSubscribeTraciResults[libsumo::VAR_COLOR]);
-	uint8_t r = tempColorPtr->r;
-	uint8_t g = tempColorPtr->g;
-	uint8_t b = tempColorPtr->b;
-	uint8_t a = tempColorPtr->a;
-	CurVehData.color = (r << 24) + (g << 16) + (b << 8) + a;
-
-	tempStringPtr = static_pointer_cast<libsumo::TraCIString> (VehDataSubscribeTraciResults[libsumo::VAR_ROAD_ID]);
-	CurVehData.linkId = tempStringPtr->value;
-
-	tempIntPtr = static_pointer_cast<libsumo::TraCIInt> (VehDataSubscribeTraciResults[libsumo::VAR_LANE_INDEX]);
-	CurVehData.laneId = (int32_t) tempIntPtr->value;
-
-	tempDoublePtr = static_pointer_cast<libsumo::TraCIDouble> (VehDataSubscribeTraciResults[libsumo::VAR_DISTANCE]);
-	//tempDoublePtr = static_pointer_cast<libsumo::TraCIDouble> (VehDataSubscribeTraciResults[libsumo::VAR_LANEPOSITION]);
-	CurVehData.distanceTravel = (float) tempDoublePtr->value;
-
-
-	tempDoublePtr = static_pointer_cast<libsumo::TraCIDouble> (VehDataSubscribeTraciResults[libsumo::VAR_ACCELERATION]);
-	CurVehData.acceleration = tempDoublePtr->value;
-
-	CurVehData.speedDesired = CurVehData.speed;
-	CurVehData.accelerationDesired = 0.0;
-
-	//=================
-	// get preceding vehicle
-	//=================
-	pair<string, double> leaderIdNSpeed = SUMO_TRACI_NAMESPACE::Vehicle::getLeader(vehId, 1000);
-	CurVehData.precedingVehicleId = get<0>(leaderIdNSpeed);
-	CurVehData.precedingVehicleDistance = get<1>(leaderIdNSpeed);
-	CurVehData.hasPrecedingVehicle = 0;
-	CurVehData.precedingVehicleSpeed = -1.0;
-	if (CurVehData.precedingVehicleId.compare("") != 0) {
-		CurVehData.hasPrecedingVehicle = 1;
-		CurVehData.precedingVehicleSpeed = SUMO_TRACI_NAMESPACE::Vehicle::getSpeed(CurVehData.precedingVehicleId);
-	}
-
-	//=================
-	// get signal information
-	//=================
-	vector <libsumo::TraCINextTLSData> nextTlsList = SUMO_TRACI_NAMESPACE::Vehicle::getNextTLS(vehId);
-
-	if (nextTlsList.size() > 0) {
-		CurVehData.signalLightId = nextTlsList[0].id;
-		CurVehData.signalLightHeadId = nextTlsList[0].tlIndex;
-		CurVehData.signalLightDistance = nextTlsList[0].dist;
-		char tlsState = nextTlsList[0].state;
-		CurVehData.signalLightColor = 0;
-
-		if (tlsState == 'r') {
-			CurVehData.signalLightColor = 1;
-		}
-		else if (tlsState == 'y') {
-			CurVehData.signalLightColor = 2;
-		}
-		else if (tlsState == 'g' || tlsState == 'G') {
-			CurVehData.signalLightColor = 3;
-		}
-		else if (tlsState == 'u') {
-			CurVehData.signalLightColor = 4;
-		}
-		else if (tlsState == 'o') {
-			CurVehData.signalLightColor = 5;
-		}
-		else if (tlsState == 'O') {
-			CurVehData.signalLightColor = 6;
-		}
-		else if (tlsState == 's') {
-			CurVehData.signalLightColor = 7;
-		}
-
-		if (nextTlsList.size() > 1) {
-			if (ENABLE_VERBOSE) {
-
-				FILE* f = fopen(MasterLogName.c_str(), "a");
-				fprintf(f, "received more than 1 nextTLS vehId %s \n", vehId.c_str());
-				fclose(f);
-
-			}
-		}
-	}
-	else {
-		CurVehData.signalLightId = "";
-		CurVehData.signalLightHeadId = -1;
-		CurVehData.signalLightDistance = -1.0;
-		CurVehData.signalLightColor = -1;
-	}
-
-	//=================
-	// get speed limit
-	//=================
-	// retrieve current speed limit
-	tempDoublePtr = static_pointer_cast<libsumo::TraCIDouble> (VehDataSubscribeTraciResults[libsumo::VAR_ALLOWED_SPEED]);
-	tempDoublePtr2 = static_pointer_cast<libsumo::TraCIDouble> (VehDataSubscribeTraciResults[libsumo::VAR_SPEED_FACTOR]);
-	CurVehData.speedLimit = tempDoublePtr->value / tempDoublePtr2->value;
-
-	// retrieve next speed limit
-	vector <string> edgeRouteList = VehicleId2EdgeList_um[vehId];
-
-	tempIntPtr = static_pointer_cast<libsumo::TraCIInt> (VehDataSubscribeTraciResults[libsumo::VAR_ROUTE_INDEX]);
-	int edgeListIdx = tempIntPtr->value;
-	CurVehData.linkIdNext = "";
-	if (edgeListIdx + 1 < edgeRouteList.size()) {
-		CurVehData.linkIdNext = edgeRouteList[edgeListIdx + 1];
-	}
-
-	tempStringPtr = static_pointer_cast<libsumo::TraCIString> (VehDataSubscribeTraciResults[libsumo::VAR_VEHICLECLASS]);
-	CurVehData.vehicleClass = tempStringPtr->value;
-
-	CurVehData.speedLimitNext = -1;
-	// if there is speed limit for next link and vehclass
-	if (EdgeVehClass2SpeedLimit_um.find(make_pair(CurVehData.linkIdNext, CurVehData.vehicleClass)) != EdgeVehClass2SpeedLimit_um.end()) {
-		CurVehData.speedLimitNext = EdgeVehClass2SpeedLimit_um[make_pair(CurVehData.linkIdNext, CurVehData.vehicleClass)];
-	}
-
-	// for following information, need to get them one by one rather than through subscription
-	CurVehData.speedLimitChangeDistance = -1;
-	//if (AllEdgeList.find(CurVehData.linkIdNext) != AllEdgeList.end()) {
-	//	CurVehData.speedLimitChangeDistance = max(traci.vehicle.getDrivingDistance(vehId, CurVehData.linkIdNext, 0), -1.0);
-	//}
-
-	//=================
-	// grade
-	//=================
-	tempDoublePtr = static_pointer_cast<libsumo::TraCIDouble> (VehDataSubscribeTraciResults[libsumo::VAR_SLOPE]);
-	CurVehData.grade = tempDoublePtr->value * M_PI/180;
-
-	//=================
-	// get lane change
-	//=================
-	//vector <libsumo::TraCIBestLanesData> bestLanesData = traci.vehicle.getBestLanes(vehId);
-	CurVehData.activeLaneChange = 0;
-
-
-	//=================
-	// get length, width, height
-	//=================
-	tempDoublePtr = static_pointer_cast<libsumo::TraCIDouble> (VehDataSubscribeTraciResults[libsumo::VAR_LENGTH]);
-	CurVehData.length = tempDoublePtr->value;
-
-	tempDoublePtr = static_pointer_cast<libsumo::TraCIDouble> (VehDataSubscribeTraciResults[libsumo::VAR_WIDTH]);
-	CurVehData.width = tempDoublePtr->value;
-
-	tempDoublePtr = static_pointer_cast<libsumo::TraCIDouble> (VehDataSubscribeTraciResults[libsumo::VAR_HEIGHT]);
-	CurVehData.height = tempDoublePtr->value;
-	//=================
-	// get vehicle indicators
-	//=================
-	//Name	Bit
-	//VEH_SIGNAL_BLINKER_RIGHT	0
-	//VEH_SIGNAL_BLINKER_LEFT	1
-	//VEH_SIGNAL_BLINKER_EMERGENCY	2
-	//VEH_SIGNAL_BRAKELIGHT	3
-	//VEH_SIGNAL_FRONTLIGHT	4
-	//VEH_SIGNAL_FOGLIGHT	5
-	//VEH_SIGNAL_HIGHBEAM	6
-	//VEH_SIGNAL_BACKDRIVE	7
-	//VEH_SIGNAL_WIPER	8
-	//VEH_SIGNAL_DOOR_OPEN_LEFT	9
-	//VEH_SIGNAL_DOOR_OPEN_RIGHT	10
-	//VEH_SIGNAL_EMERGENCY_BLUE	11
-	//VEH_SIGNAL_EMERGENCY_RED	12
-	//VEH_SIGNAL_EMERGENCY_YELLOW	13
-	tempIntPtr = static_pointer_cast<libsumo::TraCIInt> (VehDataSubscribeTraciResults[libsumo::VAR_SIGNALS]);
-	// (n & (1 << k)) >> k
-	CurVehData.lightIndicators = tempIntPtr->value;
-}
-
-
-
-int TrafficHelper::recvFromVISSIM(double* simTime, MsgHelper& Msg_c) {
-	int recvStatus = 0;
-
-	SocketHelper Sock_temp;
-	
-	int simStateRecv;
-	float simTimeRecv;
-
-	if (ENABLE_VERBOSE) {
-		printf("receive VISSIM ......\n");
-	
-		FILE* f = fopen(MasterLogName.c_str(), "a");
-		fprintf(f, "recv VISSIM\n");
-		fclose(f);
-	}
-
-	for (int i = 0; i < vissimSock.size(); i++) {
-		if (ENABLE_VERBOSE) {
-			printf("receive VISSIM %s \n", vissimSockName_um[vissimSock[i]].c_str());
-		
-			//FILE* f = fopen(MasterLogName.c_str(), "a");
-			//fprintf(f, "receive VISSIM %s \n", vissimSockName_um[vissimSock[i]].c_str());
-			//fclose(f);
-		}
-
-		//if (vissimSock.size() > 1 && vissimSockName_um[vissimSock[i]].find(SIGNAL_SOCK_PATTERN) != string::npos) {
-		//	// if is signal controller
-		//	// if simTime has reached steps for SC
-		//	if (abs(round((*simTime- vissimBaseDt) / (vissimSignalStep * vissimBaseDt)) - (*simTime - vissimBaseDt) / (vissimSignalStep * vissimBaseDt)) < 1e-5) {
-		//		// if not simTime = 0
-		//		if (*simTime > (vissimSignalStep * vissimBaseDt) - 1e-5) {
-		//			Sock_temp.recvData(vissimSock[i], &simStateRecv, &simTimeRecv, Msg_c);
-		//			*simTime = simTimeRecv;
-		//		}
-		//	}
-		//}
-		//else {
-		//	Sock_temp.recvData(vissimSock[i], &simStateRecv, &simTimeRecv, Msg_c);
-		//	*simTime = simTimeRecv;
-		//}
-		recvStatus = Sock_temp.recvData(vissimSock[i], &simStateRecv, &simTimeRecv, Msg_c);
-		*simTime = simTimeRecv;
-
-		if (recvStatus < 0) {
-			return -1;
-		}
-
-		if (ENABLE_VERBOSE) {
-			printf("\treceive simTime %f\n", *simTime);
-
-			FILE* f = fopen(MasterLogName.c_str(), "a");
-			fprintf(f, "\treceive simTime %f\n", *simTime);
-			fclose(f);
-
-			for (auto& it : Msg_c.VehDataRecv_um) {
-				printf("\treceive vehicle id %s\n", it.first.c_str());
-
-				FILE* f = fopen(MasterLogName.c_str(), "a");
-				fprintf(f, "\trecv veh id %s\n", it.first.c_str());
-				fclose(f);
-			}
-			for (auto& it : Msg_c.DetDataRecv_um) {
-				printf("\treceive detector at intersection id %s\n", it.first.c_str());
-
-				FILE* f = fopen(MasterLogName.c_str(), "a");
-				fprintf(f, "\treceive detector at intersection id %s\n", it.first.c_str());
-				fclose(f);
-			}
-			for (auto& it : Msg_c.TlsDataRecv_um) {
-				printf("\treceive signal intersection id %s\n", it.first.c_str());
-
-				FILE* f = fopen(MasterLogName.c_str(), "a");
-				fprintf(f, "\treceive signal intersection id %s\n", it.first.c_str());
-				fclose(f);
-			}
-		}
-	}
-
-	if (ENABLE_VERBOSE && recvStatus >= 0) {
-		printf("receive VISSIM complete \n");
-	
-		//FILE* f = fopen(MasterLogName.c_str(), "a");
-		//fprintf(f, "receive VISSIM complete \n");
-		//fclose(f);
-	}
-
-	int aa = 1;
-
-	return recvStatus;
+#include "TrafficHelper.h"
+
+
+//const unsigned short selfServerPort[NSERVER] = { 420 };
+
+using namespace std;
+
+//using namespace libtraci;
+
+//CentralCtrl CentralCtrl_g;
+
+// The convention is that, Server always provide service to the Client. 
+// i.e. Manager will be the Server to provide traffic data service to all possible clients, e.g. Controller, Communication, Vehicle. 
+// The Controller can be another service provider to Vehicle to command desired speed
+// The Communication will be another service provider to Controller to provide traffic information. 
+// 
+// [              ] - [Controller] - [      ]
+// [Another Server] - [Controller] - [Client]
+// [              ] - [Controller] - [      ]
+//
+// First, connect to other server
+// Then, wait all clients connect
+// Next, signal other server to start service
+// Last, start own service => return to the caller of Socket initConnection
+TrafficHelper::TrafficHelper(){
+}
+
+void TrafficHelper::connectionSetup(string trafficIp, int trafficPort, int nClientInput, int order) {
+
+	nClient = nClientInput;
+
+	edgeHasSubscribed = false;
+	//nEdgeSubscribe = 0;
+
+	//system("sumo -c \"C:\Users\y0n\Dropbox (ORNL)\2_projects\1_2_sumo\1_4_speedHarmTest\speedHarmTest.sumocfg\" --remote-port 1337 ");
+
+	//Simulation::init(trafficPort, 60, trafficIp);
+	//Simulation::setOrder(1);
+
+	traci.connect(trafficIp, trafficPort);
+
+	traci.setOrder(order);
+
+	/********************************************
+	* GET VEH SUBSCRIPTION
+	*********************************************/
+	//VehDataSubscribeList.push_back(libsumo::TRACI_ID_LIST);
+	VehDataSubscribeList.push_back(libsumo::VAR_TYPE);
+	VehDataSubscribeList.push_back(libsumo::VAR_SPEED);
+	VehDataSubscribeList.push_back(libsumo::VAR_POSITION3D);
+	VehDataSubscribeList.push_back(libsumo::VAR_ANGLE); // north 0 deg, clockwise
+	VehDataSubscribeList.push_back(libsumo::VAR_COLOR);
+	VehDataSubscribeList.push_back(libsumo::VAR_ROAD_ID);
+	VehDataSubscribeList.push_back(libsumo::VAR_LANE_INDEX);
+	VehDataSubscribeList.push_back(libsumo::VAR_DISTANCE);
+	VehDataSubscribeList.push_back(libsumo::VAR_LANEPOSITION);
+
+	VehDataSubscribeList.push_back(libsumo::VAR_LANE_ID);
+	VehDataSubscribeList.push_back(libsumo::VAR_VEHICLECLASS);
+	VehDataSubscribeList.push_back(libsumo::VAR_ROUTE_INDEX);
+
+	// testing new data
+	VehDataSubscribeList.push_back(libsumo::VAR_ACCELERATION);
+	//// no speedDesired accelerationDesired
+	//// will return id and distance
+	//VehDataSubscribeList.push_back(libsumo::VAR_LEADER);
+	//// then need to query speed of that vehicle
+	//// or use followSpeed??
+	//VehDataSubscribeList.push_back(libsumo::VAR_FOLLOW_SPEED);
+	// signal light
+	// will return tlsID (signal light id), tlsIndex (signal head id), distance, state
+	//VehDataSubscribeList.push_back(libsumo::VAR_NEXT_TLS);
+	// speed limit
+	VehDataSubscribeList.push_back(libsumo::VAR_ALLOWED_SPEED);
+	VehDataSubscribeList.push_back(libsumo::VAR_SPEED_FACTOR);
+	// get next speed limit
+	// handle it at beginning when this vehicle enters
+	VehDataSubscribeList.push_back(libsumo::VAR_VIA);
+	// next link
+	// retrieved as part of the speed limit information
+	
+	//// lane change
+	//// need to check if this even works
+	//VehDataSubscribeList.push_back(libsumo::CMD_CHANGELANE);
+
+	// get slope of the road
+	VehDataSubscribeList.push_back(libsumo::VAR_SLOPE);
+
+	// get signals
+	//Name	Bit
+	//VEH_SIGNAL_BLINKER_RIGHT	0
+	//VEH_SIGNAL_BLINKER_LEFT	1
+	//VEH_SIGNAL_BLINKER_EMERGENCY	2
+	//VEH_SIGNAL_BRAKELIGHT	3
+	//VEH_SIGNAL_FRONTLIGHT	4
+	//VEH_SIGNAL_FOGLIGHT	5
+	//VEH_SIGNAL_HIGHBEAM	6
+	//VEH_SIGNAL_BACKDRIVE	7
+	//VEH_SIGNAL_WIPER	8
+	//VEH_SIGNAL_DOOR_OPEN_LEFT	9
+	//VEH_SIGNAL_DOOR_OPEN_RIGHT	10
+	//VEH_SIGNAL_EMERGENCY_BLUE	11
+	//VEH_SIGNAL_EMERGENCY_RED	12
+	//VEH_SIGNAL_EMERGENCY_YELLOW	13
+	VehDataSubscribeList.push_back(libsumo::VAR_SIGNALS);
+
+	// Return the length, width, height of the vehicle
+	VehDataSubscribeList.push_back(libsumo::VAR_LENGTH);
+	VehDataSubscribeList.push_back(libsumo::VAR_WIDTH);
+	VehDataSubscribeList.push_back(libsumo::VAR_HEIGHT);
+	// -------------------
+	// These variables are subscribed for testing purposes
+	// -------------------
+	VehDataSubscribeList.push_back(libsumo::VAR_SPEED_WITHOUT_TRACI);
+
+
+
+	//VehDataSubscribeList.push_back(libsumo::VAR_NEXT_TLS); // Returns upcoming traffic lights, along with distanceand state [(tlsID, tlsIndex, distance, state), ...]
+
+	//VehDataSubscribeList.push_back(libsumo::VAR_LEADER);/*Returns the id of the leading vehicleand its distance, if the string is empty, no leader was found within the given range.Only vehicles ahead on the currently list of best lanes are considered(see above).This means, the leader is only valid until the next lane - change maneuver.The returned distance is measured from the ego vehicle front bumper + minGap to the back bumper of the leader vehicle.*/
+
+	//VehDataSubscribeList.push_back(libsumo::VAR_ALLOWED_SPEED); //Returns the maximum allowed speed on the current lane regarding speed factor in m/s for this vehicle.
+	//
+	//VehDataSubscribeList.push_back(libsumo::VAR_SPEED_FACTOR); // Returns the road speed multiplier for this vehicle[double]
+
+	//VehDataSubscribeList.push_back(libsumo::VAR_SLOPE); // Retrieves the slope at the current vehicle position in degrees
+
+
+
+
+
+	/********************************************
+	* GET DETECTOR SUBSCRIPTION
+	*********************************************/
+	// obtain detector ids of the selected intersection and subscribe to results
+	if (ENABLE_DET_SUB) {
+		//string detSelectPattern = DET_SEL_PATTERN; // select which intersection we want to do signal control in the loop SCIP
+		////===================================================
+		//// Retreive DETECTOR configuration for the scenario BEFORE simulation starts
+		////===================================================
+		//vector <string> detAreaAllId_v = traci.lanearea.getIDList();
+		//vector <string> detInductAllId_v = traci.inductionloop.getIDList();
+
+		//vector <int> detSubscribeList;
+
+		//detSubscribeList.push_back(libsumo::LAST_STEP_VEHICLE_NUMBER);
+		//for (int iD = 0; iD < detAreaAllId_v.size(); iD++) {
+		//	if (detAreaAllId_v[iD].find(detSelectPattern) != std::string::npos) {
+		//		detSelectId_v.push_back(detAreaAllId_v[iD]);
+		//		traci.lanearea.subscribe(detAreaAllId_v[iD], detSubscribeList, 0, tSimuEnd);
+		//	}
+		//}
+	}
+
+
+
+	/********************************************
+	* GET Speed Limit of every edge, lane
+	*********************************************/
+	vector <string> laneList = traci.lane.getIDList();
+
+	vector <string> vehClassList;
+
+	vector <string> vehTypeList = traci.vehicletype.getIDList();
+	for (int i = 0; i < vehTypeList.size(); i++) {
+		string vehType = vehTypeList[i];
+		string vehClass = traci.vehicletype.getVehicleClass(vehType);
+
+		vehClassList.push_back(vehClass);
+	}
+
+	for (int i = 0; i < laneList.size(); i++) {
+		string laneId = laneList[i];
+		string edgeId = traci.lane.getEdgeID(laneId);
+		
+		vector <string> allowClassList = traci.lane.getAllowed(laneId);
+		vector <string> disallowClassList = traci.lane.getDisallowed(laneId);
+
+		if (allowClassList.size() == 0 && disallowClassList.size() == 0) {
+			for (int iC = 0; iC < vehClassList.size(); iC++) {
+				string vClass = vehClassList[iC];
+				LaneVehClass2SpeedLimit_um[make_pair(laneId, vClass)] = traci.lane.getMaxSpeed(laneId);
+
+				EdgeVehClass2SpeedLimit_um[make_pair(edgeId, vClass)] = traci.lane.getMaxSpeed(laneId);
+			}
+		}
+		else {
+			for (int iC = 0; iC < allowClassList.size(); iC++) {
+				string vClass = allowClassList[iC];
+				LaneVehClass2SpeedLimit_um[make_pair(laneId, vClass)] = traci.lane.getMaxSpeed(laneId);
+
+				EdgeVehClass2SpeedLimit_um[make_pair(edgeId, vClass)] = traci.lane.getMaxSpeed(laneId);
+			}
+		}
+	}
+
+	vector <string> edgeList = traci.edge.getIDList();
+	for (int i = 0; i < edgeList.size(); i++) {
+		AllEdgeList.insert(edgeList[i]);
+	}
+
+}
+
+void TrafficHelper::connectionSetup(int nClientInput) {
+
+	nClient = nClientInput;
+
+	edgeHasSubscribed = false;
+	//nEdgeSubscribe = 0;
+
+}
+
+void TrafficHelper::enableVehSub() {
+	ENABLE_VEH_SUB = true;
+}
+
+void TrafficHelper::disableVehSub() {
+	ENABLE_VEH_SUB = false;
+}
+
+void TrafficHelper::enableDetSub() {
+	ENABLE_DET_SUB = true;
+}
+
+void TrafficHelper::disableDetSub() {
+	ENABLE_DET_SUB = false;
+}
+
+void TrafficHelper::selectVISSIM(vector <int> vissimSockInput, vector <string> vissimSockNames) {
+	SUMO_OR_VISSIM = "VISSIM";
+
+	for (int i = 0; i < vissimSockInput.size(); i++) {
+		vissimSock.push_back(vissimSockInput[i]);
+		vissimSockName_um[vissimSockInput[i]] = vissimSockNames[i];
+	}
+	//for (int i = 0; i < clientSockInput.size(); i++) {
+	//	clientSock.push_back(clientSockInput[i]);
+	//}
+}
+
+void TrafficHelper::selectSUMO() {
+	SUMO_OR_VISSIM = "SUMO";
+}
+
+void TrafficHelper::close() {
+	if (SUMO_OR_VISSIM.compare("SUMO") == 0) {
+		/*Simulation::close();*/
+		traci.close();
+	}
+	else if (SUMO_OR_VISSIM.compare("VISSIM") == 0) {
+
+	}
+	else {
+
+	}
+}
+
+
+void TrafficHelper::getConfig() {
+
+	if (Config_c->SimulationSetup.EnableVerboseLog) {
+		ENABLE_VERBOSE = true;
+	}
+	else {
+		ENABLE_VERBOSE = false;
+	}
+
+	if (Config_c->ApplicationSetup.VehicleSubscription.size() > 0) {
+		ENABLE_VEH_SUB = true;
+	}
+	else {
+		if (Config_c->XilSetup.EnableXil) {
+			ENABLE_VEH_SUB = true;
+		}
+		else {
+			ENABLE_VEH_SUB = false;
+		}
+	}
+	if (Config_c->ApplicationSetup.DetectorSubscription.size() > 0) {
+		ENABLE_DET_SUB = true;
+	}
+	else {
+		if (Config_c->XilSetup.DetectorSubscription.size() > 0) {
+			ENABLE_DET_SUB = true;
+		}
+		else{
+			ENABLE_DET_SUB = false;
+		}
+	}
+	if (Config_c->ApplicationSetup.SignalSubscription.size() > 0) {
+		ENABLE_SIG_SUB = true;
+	}
+	else {
+		if (Config_c->XilSetup.SignalSubscription.size() > 0) {
+			ENABLE_SIG_SUB = true;
+		}
+		else {
+			ENABLE_SIG_SUB = false;
+		}
+	}
+
+	//if (Config_c->CarMakerSetup.EnableCosimulation && Config_c->CarMakerSetup.SynchronizeTrafficSignal) {
+	//	ENABLE_SIG_SUB = true;
+	//}
+
+	if (Config_c->SimulationSetup.EnableExternalDynamics) {
+		ENABLE_EXT_DYN = true;
+	}
+	else {
+		ENABLE_EXT_DYN = false;
+	}
+	tSimuEnd = Config_c->SimulationSetup.SimulationEndTime;
+
+	// get vehicle message that needs to be sent out
+	VehicleMessageField_v = Config_c->SimulationSetup.VehicleMessageField;
+
+	for (int i = 0; i < VehicleMessageField_v.size(); i++) {
+		VehicleMessageField_set.insert(VehicleMessageField_v[i]);
+	}
+
+	// get subscription information
+	// variable to store subscription that need to check
+	// if application layer is disabled, xil is enabled then use subscription of xil, this means traffic layer directly connects to xil
+	if (!Config_c->ApplicationSetup.EnableApplicationLayer && Config_c->XilSetup.EnableXil) {
+		Config_c->getVehSubscriptionList(Config_c->XilSetup.VehicleSubscription, edgeSubscribeId_v, vehicleSubscribeId_v, subscribeAllVehicle, pointSubscribeId_v, vehicleTypeSubscribedId_v);
+	}
+	// otherwise find out the subscription of application layer
+	else {
+		Config_c->getVehSubscriptionList(Config_c->ApplicationSetup.VehicleSubscription, edgeSubscribeId_v, vehicleSubscribeId_v, subscribeAllVehicle, pointSubscribeId_v, vehicleTypeSubscribedId_v);
+	}
+	
+	vehicleHasSubscribed_v.clear();
+	vehicleHasSubscribed_v.resize(vehicleSubscribeId_v.size());
+	fill(vehicleHasSubscribed_v.begin(), vehicleHasSubscribed_v.end(), false);
+
+	Config_c->CarMakerSetup.TrafficRefreshRate = 0.123;
+
+	int aa = 1;
+}
+
+
+int TrafficHelper::sendToTrafficSimulator(double simTime, MsgHelper Msg_c) {
+	
+	int sendStatus = 0;
+
+	if (SUMO_OR_VISSIM.compare("SUMO") == 0) {
+		sendStatus = this->sendToSUMO(simTime, Msg_c);
+	}
+	else if (SUMO_OR_VISSIM.compare("VISSIM") == 0) {
+		sendStatus = this->sendToVISSIM(simTime, Msg_c);
+	}
+	else {
+
+	}
+
+	return sendStatus;
+}
+
+
+void TrafficHelper::fillTrafficLightStates(MsgHelper& msg)
+{
+	
+	msg.TlsDataRecv_um.clear();
+
+
+	std::vector<std::string> tlsIds = traci.trafficlights.getIDList();
+
+	for (const auto& tlsId : tlsIds) {
+		TrafficLightData_t t;
+		t.name = tlsId;
+
+
+		t.id = static_cast<uint16_t>(std::hash<std::string>{}(tlsId) & 0xFFFF);
+
+
+		try {
+			t.state = traci.trafficlights.getRedYellowGreenState(tlsId);
+		}
+		catch (...) {
+			t.state = "";
+		}
+
+		msg.TlsDataRecv_um[tlsId] = std::move(t);
+	}
+}
+
+
+
+
+void TrafficHelper::updateEgoLoopRoute()
+{
+	if (SUMO_OR_VISSIM.compare("SUMO") != 0)
+		return;
+
+	const std::string egoId = "ego";
+
+	// --- check existence ---
+	std::vector<std::string> idList = traci.vehicle.getIDList();
+	if (std::find(idList.begin(), idList.end(), egoId) == idList.end())
+		return;
+
+	std::string curEdge = traci.vehicle.getRoadID(egoId);
+	//std::cout << "[DBG] curEdge = " << curEdge << std::endl;
+
+	/*
+	static const std::vector<std::string> loopEdges = {
+		"-2801", "-280", "-307", "-327", "-3271", "-281", "-315", "-3151", "-321", "-300", "-2851", "-285", "-290", "-298", "-295",
+		"-312", "-293", "-297", "-288", "-2881", "-286", "-302", "-3221", "-322", "-313", "-284", "-2841", "-328", "-304", "-2801"
+	};
+	*/
+
+	static bool wasOn2801 = false;
+
+
+	if (curEdge != "-2801") {
+		wasOn2801 = false;     
+		return;                
+	}
+
+	if (!wasOn2801) {
+		wasOn2801 = true;
+
+		traci.vehicle.setRouteID(egoId, "route1");
+
+		auto newRoute = traci.vehicle.getRoute(egoId);
+		std::cout << "[EGO LOOP RESET ON ENTER] road=" << curEdge
+			<< " routeID=route1"
+			<< " newRouteSize=" << newRoute.size()
+			<< " first=" << (newRoute.empty() ? "EMPTY" : newRoute.front())
+			<< " last=" << (newRoute.empty() ? "EMPTY" : newRoute.back())
+			<< std::endl;
+
+		VehicleId2EdgeList_um[egoId] = newRoute;
+
+	}
+}
+
+
+
+
+
+
+int TrafficHelper::addEgoVehicle(double simTime) {
+
+	printf("[addEgoVehicle] called at t = %.3f, id = %s\n",
+		simTime, Config_c->SumoSetup.EgoID.c_str());
+
+	if (SUMO_OR_VISSIM.compare("SUMO") != 0) {
+		return 0;
+	}
+
+	std::string idStr = Config_c->SumoSetup.EgoID;   //  Config_c->CarMakerSetup.EgoId;
+	std::string typeStr = Config_c->CarMakerSetup.EgoType;
+	if (typeStr.empty()) {
+		typeStr = "EGO_TYPE";   
+	}
+
+	std::string routeId = "route1";   
+
+	try {
+		// Python:
+		//   vehID='ego', routeID='route1',
+		//   departPos='1', departLane='0', departSpeed='0.1'
+		traci.vehicle.add(
+			idStr,
+			routeId,
+			typeStr,
+			Config_c->SumoSetup.EgoDepart,
+			Config_c->SumoSetup.EgoDepartLane,
+			Config_c->SumoSetup.EgoDepartPos,
+			Config_c->SumoSetup.EgoDepartSpeed
+		);
+		printf("[addEgoVehicle] vehicle.add() succeeded\n");
+		traci.vehicle.setColor(idStr, libsumo::TraCIColor(255, 0, 0, 255));
+
+		int speedMode = Config_c->SumoSetup.SpeedMode;
+		traci.vehicle.setSpeedMode(idStr, speedMode);
+
+
+		double maxAcc = 2.0;  
+		traci.vehicletype.setAccel(typeStr, maxAcc);
+		traci.vehicletype.setDecel(typeStr, maxAcc);
+
+		return 1;
+	}
+	catch (const std::exception& e) {
+		printf("Error: addEgoVehicle failed: %s\n", e.what());
+		return -1;
+	}
+	catch (...) {
+		printf("Error: addEgoVehicle unknown exception\n");
+		return -1;
+	}
+}
+
+
+int TrafficHelper::addEgoVehicleFromXY(double simTime, std::string vehicleId, std::string vehicleType, double positionX, double positionY) {
+
+	if (SUMO_OR_VISSIM.compare("SUMO") == 0) {
+		if (ENABLE_VEH_SIMULATOR) {
+			// Map the x y positon to an edge for spawing the ego vehicle
+			libsumo::TraCIRoadPosition edgePosition = traci.simulation.convertRoad(positionX, positionY, false);
+			// Create a dummy route for the ego vehicle
+			std::string dummyedgeID = edgePosition.edgeID;
+			double lanePos = edgePosition.pos; // position along the edge
+			int laneIndex = edgePosition.laneIndex;
+			std::string dummyRouteId = "route_" + vehicleId;
+			std::vector<std::string> dummyRoute;
+			dummyRoute.push_back(dummyedgeID);
+			traci.route.add(dummyRouteId, dummyRoute);
+
+			traci.vehicle.add(vehicleId, dummyRouteId, vehicleType);
+			traci.vehicle.setColor(vehicleId, libsumo::TraCIColor(255, 0, 0));
+		}
+
+		return 1;
+	}
+	else {
+		return 0;
+	}
+
+}
+
+
+int TrafficHelper::checkIfEgoExist(double* simTime) {
+
+	if (SUMO_OR_VISSIM.compare("SUMO") == 0) {
+		/**simTime = Simulation::getTime();*/
+		*simTime = traci.simulation.getTime();
+		vector <string> VehIdInSimulator = traci.vehicle.getIDList();
+
+		// check if subscribed vheicle is in the network
+		for (auto& iter : vehicleSubscribeId_v) {
+			string idStr = iter.first;
+
+			// if any one of vehicle has not been subscribed yet
+			if (find(VehIdInSimulator.begin(), VehIdInSimulator.end(), idStr) != VehIdInSimulator.end()) {
+				return 1;
+			}
+			else {
+				return 0;
+			}
+
+			// only check the first vehicle, which considered as the ego vehicle
+			// break;
+		}
+	}
+	else {
+		return 0;
+	}
+
+}
+
+int TrafficHelper::getSimulationTime(double* simTime) {
+	if (SUMO_OR_VISSIM.compare("SUMO") == 0) {
+		/**simTime = Simulation::getTime();*/
+		*simTime = traci.simulation.getTime();
+		return 1;
+	}
+	else {
+		return 0;
+	}
+}
+
+int TrafficHelper::runSimulation(double endTime) {
+	if (SUMO_OR_VISSIM.compare("SUMO") == 0) {
+		/*Simulation::step(endTime);*/
+		traci.simulationStep(endTime);
+		return 1;
+	}
+	else {
+		return 0;
+	}
+}
+
+
+int TrafficHelper::sendToSUMO(double simTime, MsgHelper Msg_c) {
+
+	int sendStatus = 0;
+
+	if (ENABLE_VERBOSE) {
+		printf("sending to SUMO\n ");
+
+		FILE* f = fopen(MasterLogName.c_str(), "a");
+		fprintf(f, "sending to SUMO\n ");
+		fclose(f);
+
+	}
+
+	//int nVeh = VehIdInSimulator.size();
+
+	//if (nVeh > 0) {
+	//	for (int iV = 0; iV < nVeh; iV++) {
+	//		string idStr = VehIdInSimulator[iV];
+	//		//unsigned int id = stoi(idStr.substr(12));
+
+	//		//if (idStr.compare("flow_0.10") == 0) {
+	//		//	traci.vehicle.setColor(idStr, libsumo::TraCIColor(255, 0, 0, 255));
+	//		//}
+	//		//if (idStr.compare("flow_1.9") == 0) {
+	//		//	traci.vehicle.setColor(idStr, libsumo::TraCIColor(0, 166, 255, 255));
+	//		//}
+
+	//		//if (std::find(Msg_c.VehIdRecv_v.begin(), Msg_c.VehIdRecv_v.end(), idStr) != Msg_c.VehIdRecv_v.end()) {
+	//		if (Msg_c.VehDataRecv_um.find(idStr) != Msg_c.VehDataRecv_um.end()) {
+	//			traci.vehicle.setSpeed(idStr, Msg_c.VehDataRecv_um[idStr].speed);
+	//			traci.vehicle.setSpeedMode(idStr, 0); // most checks off
+
+
+	//			if (ENABLE_VERBOSE) {
+	//				//cout << "setSpeed ego # " << idStr << " speed " << VehCmdRecv_um[idStr].speed << endl;
+	//				printf("setSpeed ego %s speed %.4f\n", idStr.c_str(), Msg_c.VehDataRecv_um[idStr].speed);
+	//				//cout << "setSpeed ego # " << idStr << " speed " << VehCmdRecv_um[idStr].speed << '\n';
+	//			}
+	//		}
+	//		else {
+	//			//traci.vehicle.setSpeedMode(idStr, 31);
+	//		}
+	//	}
+
+	//	if (Msg_c.TlsIdRecv_v.size() > 0) {
+	//		if (Msg_c.TlsDataRecv_um.find(tlsSelect) != Msg_c.TlsDataRecv_um.end()) {
+	//			string setTlsState = Msg_c.TlsDataRecv_um[tlsSelect].state;
+	//			traci.trafficlights.setRedYellowGreenState(tlsSelect, setTlsState);
+
+	//			if (ENABLE_VERBOSE) {
+	//				//cout << "set " << " time " << simTime << " traffic light " << tlsSelect << " state " << setTlsState << endl;
+	//				printf("set time %.1f traffic light %s state %s \n", simTime, tlsSelect.c_str(), setTlsState.c_str());
+	//			}
+	//		}
+	//	}
+	//}
+
+	try {
+		vector <string> VehIdInSimulator = traci.vehicle.getIDList();
+		//uint32_t color = 4278190335;
+		//uint8_t r = (color >> 24) & 0xFF;
+		//uint8_t g = (color >> 16) & 0xFF;
+		//uint8_t b = (color >> 8) & 0xFF;
+		//uint8_t a = (color) & 0xFF;
+		//traci.vehicle.setColor("flow_0.0", libsumo::TraCIColor(r, g, b, a));
+
+		for (int iV = 0; iV < VehIdInSimulator.size(); iV++) {
+			//traci.vehicle.setSpeedMode(VehIdInSimulator[iV], 31); // default speed mode
+		}
+
+		for (int iV = 0; iV < Msg_c.VehDataSend_um[0].size(); iV++) {
+			string idStr = Msg_c.VehDataSend_um[0][iV].id;
+			
+			double speed;
+			if (VehicleMessageField_set.find("speedDesired") != VehicleMessageField_set.end()) {
+				speed = Msg_c.VehDataSend_um[0][iV].speedDesired;
+			}
+			else {
+				//double accel = std::any_cast<float>(Msg_c.VehDataSend_um[0][iV]["accelerationDesired"]);
+				//speed = 
+				printf("ERROR: SUMO does not support control by accelerationDesired yet\n");
+				printf("\tPlease select speedDesired in VehicleMessageField instead\n");
+				exit(-1);
+			}
+
+
+			if (ENABLE_VERBOSE) {
+				if (!ENABLE_VEH_SIMULATOR && find(VehIdInSimulator.begin(), VehIdInSimulator.end(), idStr) != VehIdInSimulator.end()) {
+					double speedOld = traci.vehicle.getSpeed(idStr);
+					printf("Set SUMO id %s from speed %.4f to speed %.4f\n", idStr.c_str(), speedOld, speed);
+
+					FILE* f = fopen(MasterLogName.c_str(), "a");
+					fprintf(f, "Set SUMO id %s from speed %.4f to speed %.4f\n", idStr.c_str(), speedOld, speed);
+					fclose(f);
+				}
+			}
+
+			// if vehicle simulator and is ego
+			if (ENABLE_VEH_SIMULATOR && idStr.compare(Config_c->CarMakerSetup.EgoId) == 0) {
+				// !!!!check if what received is ego vehicle 
+				// use default type if not specified!!
+				
+				//// if ego not exist yet, add it
+				//if (find(VehIdInSimulator.begin(), VehIdInSimulator.end(), idStr) == VehIdInSimulator.end()) {
+				//	string typeStr = Msg_c.VehDataSend_um[0][iV].type;
+
+				//	// if is empty
+				//	if (typeStr.size() == 0) {
+				//		traci.vehicle.add(idStr, "");
+				//	}
+				//	else {
+				//		traci.vehicle.add(idStr, "", typeStr);
+				//	}
+				//	traci.vehicle.setColor(idStr, libsumo::TraCIColor(255, 0, 0));
+				//}
+				// otherwise, move it
+				{
+					if (ENABLE_EXT_DYN) {
+						traci.vehicle.setPreviousSpeed(idStr, speed); // setting speed at (k) will be reflected at (k) "immediately", i.e., be considered in the next integration
+					}
+					else {
+
+						double positionX = (double)Msg_c.VehDataSend_um[0][iV].positionX;
+						double positionY = (double)Msg_c.VehDataSend_um[0][iV].positionY;
+						double positionZ = (double)Msg_c.VehDataSend_um[0][iV].positionZ;
+						double heading = (double)Msg_c.VehDataSend_um[0][iV].heading;
+
+						traci.vehicle.moveToXY(idStr, "", -1, positionX, positionY, heading, 6); // keepRoute 110 => 6
+						//bit0(keepRoute = 1 when only this bit is set)
+							//1: The vehicle is mapped to the closest edge within it's existing route. If no suitable position is found within 100m mapping fails with an error.
+							//0 : The vehicle is mapped to the closest edge within the network.If that edge does not belong to the original route, the current route is replaced by a new route which consists of that edge only.If no suitable position is found within 100m mapping fails with an error.When using the sublane model the best lateral position that is fully within the lane will be used.Otherwise, the vehicle will drive in the center of the closest lane.
+						//bit1(keepRoute = 2 when only this bit is set)
+							//1 : The vehicle is mapped to the exact position in the network(including the exact lateral position).If that position lies outside the road network, the vehicle stops moving on it's own accord until it is placed back into the network with another TraCI command. (if keeproute = 3, the position must still be within 100m of the vehicle route)
+							//0 : The vehicle is always on a road
+						//bit2(keepRoute = 4 when only this bit is set)
+							//1 : lane permissions are ignored when mapping
+							//0 : The vehicle is mapped only to lanes that allow it's vehicle class
+
+					}
+
+					if (VehicleMessageField_set.find("lightIndicators") != VehicleMessageField_set.end()) {
+						traci.vehicle.setSignals(idStr, (int)Msg_c.VehDataSend_um[0][iV].lightIndicators);
+					}
+
+				}
+			}
+			// if carla is enabled and the reveiced id is within the interested ids
+			else if (ENABLE_CARLA && find(Config_c->CarlaSetup.InterestedIds.begin(), Config_c->CarlaSetup.InterestedIds.end(), idStr) != Config_c->CarlaSetup.InterestedIds.end()) {
+				double positionX = (double)Msg_c.VehDataSend_um[0][iV].positionX;
+				double positionY = (double)Msg_c.VehDataSend_um[0][iV].positionY;
+				double positionZ = (double)Msg_c.VehDataSend_um[0][iV].positionZ;
+				double heading = (double)Msg_c.VehDataSend_um[0][iV].heading;
+				string vehicleType = Msg_c.VehDataSend_um[0][iV].type;
+				// If the Intertested Vehicle is not in sumo
+				bool vehicleExist = false;
+				for (const std::string& vehId : VehIdInSimulator) {
+					if (vehId == idStr) {
+						vehicleExist = true;
+					}
+				}
+				if (!vehicleExist) {
+					addEgoVehicleFromXY(simTime, idStr, vehicleType, positionX, positionY);
+				}
+				traci.vehicle.moveToXY(idStr, "", -1, positionX, positionY, heading, 6);
+				traci.vehicle.setSpeed(idStr, speed);
+				
+			}
+			else {
+				if (1 && find(VehIdInSimulator.begin(), VehIdInSimulator.end(), idStr) != VehIdInSimulator.end()) {
+					if (ENABLE_EXT_DYN) {
+						traci.vehicle.setPreviousSpeed(idStr, speed); // setting speed at (k) will be reflected at (k) "immediately", i.e., be considered in the next integration
+					}
+					else {
+						traci.vehicle.setSpeed(idStr, speed); // speed set at (k) essentially will be reflected at (k+1), not considered in the integration
+
+						/*
+						bit0: Regard safe speed
+						bit1 : Regard maximum acceleration
+						bit2 : Regard maximum deceleration
+						bit3 : Regard right of way at intersections(only applies to approaching foe vehicles outside the intersection)
+						bit4 : Brake hard to avoid passing a red light
+						bit5 : Disregard right of way within intersections(only applies to foe vehicles that have entered the intersection).
+						*/
+
+						traci.vehicle.setSpeedMode(idStr, Config_c->SumoSetup.SpeedMode); // 000000 most checks off
+						//traci.vehicle.setSpeedMode(idStr, 0); // 000000 most checks off
+						//traci.vehicle.setSpeedMode(idStr, 24); // 011000
+						//traci.vehicle.setSpeedMode(idStr, 8); // 001000
+
+						//traci.vehicle.setSpeedFactor(idStr, 1);
+					}
+
+					// change vehicle color if needed
+					if (VehicleMessageField_set.find("color") != VehicleMessageField_set.end()) {
+						uint32_t color = Msg_c.VehDataSend_um[0][iV].color;
+						uint8_t r = (color >> 24) & 0xFF;
+						uint8_t g = (color >> 16) & 0xFF;
+						uint8_t b = (color >> 8) & 0xFF;
+						uint8_t a = (color) & 0xFF;
+						traci.vehicle.setColor(idStr, libsumo::TraCIColor(r, g, b, a));
+					}
+
+				}
+			}
+
+
+		}
+
+
+		if (ENABLE_SIG_SUB) {
+			for (int iS = 0; iS < Msg_c.TlsDataSend_um[0].size(); iS++) {
+				string idStr = Msg_c.TlsDataSend_um[0][iS].name;
+
+				traci.trafficlights.setRedYellowGreenState(idStr, Msg_c.TlsDataSend_um[0][iS].state);
+			}
+		}
+
+
+		if (ENABLE_VERBOSE) {
+			printf("send SUMO complete\n ");
+
+			FILE* f = fopen(MasterLogName.c_str(), "a");
+			fprintf(f, "send SUMO complete\n ");
+			fclose(f);
+		}
+
+	}
+	catch (const std::exception& e) {
+		std::cout << e.what();
+		return -1;
+	}
+	catch (...) {
+		printf("UNKNOWN ERROR: send to SUMO fails\n");
+		return -1;
+	}
+
+		//if (Msg_c.TlsIdRecv_v.size() > 0) {
+		//	if (Msg_c.TlsDataRecv_um.find(tlsSelect) != Msg_c.TlsDataRecv_um.end()) {
+		//		string setTlsState = Msg_c.TlsDataRecv_um[tlsSelect].state;
+		//		traci.trafficlights.setRedYellowGreenState(tlsSelect, setTlsState);
+
+		//		if (ENABLE_VERBOSE) {
+		//			//cout << "set " << " time " << simTime << " traffic light " << tlsSelect << " state " << setTlsState << endl;
+		//			printf("set time %.1f traffic light %s state %s \n", simTime, tlsSelect.c_str(), setTlsState.c_str());
+		//		}
+		//	}
+		//}
+
+
+	return sendStatus;
+}
+
+int TrafficHelper::sendToVISSIM(double simTime, MsgHelper Msg_c) {
+
+	int sendStatus = 0;
+
+	SocketHelper Sock_temp;
+	
+	try {
+		for (int iClient = 0; iClient < vissimSock.size(); iClient++) {
+			if (ENABLE_VERBOSE) {
+				printf("send to VISSIM %s\n", vissimSockName_um[vissimSock[iClient]].c_str());
+
+				FILE* f = fopen(MasterLogName.c_str(), "a");
+				fprintf(f, "send to VISSIM %s\n", vissimSockName_um[vissimSock[iClient]].c_str());
+				fclose(f);
+
+			}
+
+			//if (vissimSock.size() > 1 && vissimSockName_um[vissimSock[i]].find(SIGNAL_SOCK_PATTERN) != string::npos) {
+			//	// if is signal controller
+			//	if (abs(round(simTime / (vissimSignalStep * vissimBaseDt)) - simTime / (vissimSignalStep * vissimBaseDt)) < 1e-5) {
+			//		Sock_temp.sendData(vissimSock[i], i, simTime, 1, Msg_c);
+			//	}
+			//}
+			//else {
+			//	Sock_temp.sendData(vissimSock[i], i, simTime, 1, Msg_c);
+			//}
+			sendStatus = Sock_temp.sendData(vissimSock[iClient], iClient, simTime, 1, Msg_c);
+		}
+
+		if (ENABLE_VERBOSE) {
+			printf("send VISSIM complete\n");
+
+			FILE* f = fopen(MasterLogName.c_str(), "a");
+			fprintf(f, "send VISSIM complete\n");
+			fclose(f);
+
+		}
+
+	}
+	catch (const std::exception& e) {
+		std::cout << e.what();
+		return -1;
+	}
+	catch (...) {
+		printf("UNKNOWN ERROR: send to VISSIM fails\n");
+		return -1;
+	}
+
+	return sendStatus;
+
+}
+
+void TrafficHelper::parseSendMsg(MsgHelper MsgIn_c, MsgHelper& MsgOut_c) {
+
+	// MsgClient_c/MsgIn recv => MsgServer_c/MsgOut send
+
+	if (SUMO_OR_VISSIM.compare("SUMO") == 0) {
+		// SUMO use a dummy socket index 0
+		MsgOut_c.VehDataSend_um[0] = {}; 
+		MsgOut_c.TlsDataSend_um[0] = {}; 
+		MsgOut_c.DetDataSend_um[0] = {}; 
+		for (auto it : MsgIn_c.VehDataRecv_um) {
+			MsgOut_c.VehDataSend_um[0].push_back(it.second);
+		}
+		for (auto it : MsgIn_c.TlsDataRecv_um) {
+			MsgOut_c.TlsDataSend_um[0].push_back(it.second);
+		}
+		for (auto it : MsgIn_c.DetDataRecv_um) {
+			MsgOut_c.DetDataSend_um[0].push_back(it.second);
+		}
+	}
+	else if (SUMO_OR_VISSIM.compare("VISSIM") == 0) {
+		for (int i = 0; i < vissimSock.size(); i++) {
+			// if current socket is signal, then only send signal
+			if (vissimSockName_um[vissimSock[i]].find(SIGNAL_SOCK_PATTERN) != string::npos) {
+				MsgOut_c.VehDataSend_um[vissimSock[i]] = {};
+				MsgOut_c.TlsDataSend_um[vissimSock[i]] = {};
+				MsgOut_c.DetDataSend_um[vissimSock[i]] = {};
+				for (auto it : MsgIn_c.TlsDataRecv_um) {
+					MsgOut_c.TlsDataSend_um[vissimSock[i]].push_back(it.second);
+				}
+				for (auto it : MsgIn_c.DetDataRecv_um) {
+					MsgOut_c.DetDataSend_um[vissimSock[i]].push_back(it.second);
+				}
+			}
+			else {
+				MsgOut_c.VehDataSend_um[vissimSock[i]] = {};
+				MsgOut_c.TlsDataSend_um[vissimSock[i]] = {};
+				MsgOut_c.DetDataSend_um[vissimSock[i]] = {};
+				for (auto it : MsgIn_c.VehDataRecv_um) {
+					MsgOut_c.VehDataSend_um[vissimSock[i]].push_back(it.second);
+				}
+			}
+		}
+	}
+	
+}
+
+void TrafficHelper::runOneStepSimulation() {
+	if (SUMO_OR_VISSIM.compare("SUMO") == 0) {
+		/*Simulation::step();*/
+		traci.simulationStep();
+	}
+	else if (SUMO_OR_VISSIM.compare("VISSIM") == 0) {
+
+	}
+	else {
+
+	}
+}
+
+int TrafficHelper::recvFromTrafficSimulator(double* simTime, MsgHelper& Msg_c) {
+	int recvStatus = 0;
+
+	if (SUMO_OR_VISSIM.compare("SUMO") == 0) {
+		recvStatus = this->recvFromSUMO(simTime, Msg_c);
+	}
+	else if (SUMO_OR_VISSIM.compare("VISSIM") == 0) {
+		recvStatus = this->recvFromVISSIM(simTime, Msg_c);
+	}
+	else {
+
+	}
+
+	return recvStatus;
+}
+
+int TrafficHelper::recvFromSUMO(double* simTime, MsgHelper& Msg_c) {
+	int recvStatus = 0;
+
+	Msg_c.clearSendStorage();
+
+	VehIdInSimulator.clear();
+	
+	/**simTime = Simulation::getTime();*/
+	*simTime = traci.simulation.getTime();
+	VehIdInSimulator = traci.vehicle.getIDList();
+
+	if (subscribeAllVehicle.first) {
+		
+		static std::unordered_set<std::string> allVehSubscribed_us;
+
+		for (const std::string& vid : VehIdInSimulator) {
+			if (allVehSubscribed_us.find(vid) == allVehSubscribed_us.end()) {
+
+				traci.vehicle.subscribe(vid, VehDataSubscribeList, 0, tSimuEnd);
+				allVehSubscribed_us.insert(vid);
+			}
+		}
+	}
+	
+	int nVeh = VehIdInSimulator.size(); // number of vehicles
+
+	if (ENABLE_VERBOSE) {
+		printf("SUMO time step %f\n", *simTime);
+
+		FILE* f = fopen(MasterLogName.c_str(), "a");
+		fprintf(f, "SUMO time step %f\n", *simTime);
+		fclose(f);
+
+	}
+
+	//=================
+	// SUMO will have vehicles except for the first few time steps
+	//=================
+	int nVehSend = 0;
+
+	if (ENABLE_VEH_SUB && nVeh > 0) {
+
+		// only subscribe once
+		if (!edgeHasSubscribed) {
+			// (self, objectID, domain, dist, varIDs=None, begin=-1073741824.0, end=-1073741824.0, parameters=None)
+
+
+			// -------------------
+			//  subscribe edge
+			// -------------------
+			for (auto& iter: edgeSubscribeId_v) {
+				double radius = 0;
+				string id = iter;
+
+				traci.edge.subscribeContext(id, libsumo::CMD_GET_VEHICLE_VARIABLE, 100, VehDataSubscribeList, 0, tSimuEnd);
+			}
+
+			edgeHasSubscribed = true;
+
+		}
+
+		if (!pointHasSubscribed) {
+			// (self, objectID, domain, dist, varIDs=None, begin=-1073741824.0, end=-1073741824.0, parameters=None)
+
+			// -------------------
+			//  subscribe point
+			// -------------------
+			//pointNamePoi_v.clear();
+			int i = 0; 
+			for (auto& iter: pointSubscribeId_v) {
+				double x = get<0>(iter.second);
+				double y = get<1>(iter.second);
+				double z = get<2>(iter.second);
+				double r = get<3>(iter.second);
+
+				/*Both polygonsand points of interest may be located within a "layer".Shapes with lowest layer values are below those with a higher layer number.The network itself is drawn as layer 0. An additional file may contain definitions for both points of interestand polygons.*/
+				//imgFile	string	A bitmap to use for rendering this polygon
+				//angle	float	angle of rendered image in degree
+				//lineWidth	double	Drawing width of unfilled polygons in m, default 1
+
+				//string poiName = "RealSimPOI_"+to_string(i);
+				string poiName = iter.first;
+				/*int 	r,
+					int 	g,
+					int 	b,
+					int 	a = 255*/
+				libsumo::TraCIColor color(255,0,255); // use magenta as color 
+				string type = "RealSim";
+				int layer = 999; // put it as a very high layer
+				string imgFile = ""; // no image file
+				double width = 0; // width	float	width of rendered image in meters
+				double height = 0; // height	float	height of rendered image in meters
+				double angle = 0; // angle	float	angle of rendered image in degree
+				traci.poi.add(poiName, x, y, color, type, layer, imgFile, width, height, angle);
+				traci.poi.subscribeContext(poiName, libsumo::CMD_GET_VEHICLE_VARIABLE, r, VehDataSubscribeList, 0, tSimuEnd);
+
+				//pointNamePoi_v.push_back(poiName);
+				i++;
+			}
+
+			pointHasSubscribed = true;
+
+		}
+
+		if (!allVehicleHasSubscribed) {
+			// -------------------
+			//  subscribe vehicle
+			// -------------------
+			
+			// get list of all vehicles entered network
+			/*vector <string> vehDepartedId_v = Simulation::getDepartedIDList();*/
+			vector <string> vehDepartedId_v = traci.simulation.getDepartedIDList();
+			allVehicleHasSubscribed = true;
+			// only able to get vehicle subscription for vehicles already in the network
+			int i = 0;
+			for (auto & iter: vehicleSubscribeId_v) {
+				// if any one of vehicle has not been subscribed yet
+				if (!vehicleHasSubscribed_v[i]) {
+					allVehicleHasSubscribed = false;
+					// id of the vehicle to be subscribed
+					string id = iter.first;
+					// if the vehicle to subscribe just entered the network
+					if (find(vehDepartedId_v.begin(), vehDepartedId_v.end(), id)!=vehDepartedId_v.end()) {
+						double radius = iter.second;
+
+						traci.vehicle.subscribeContext(id, libsumo::CMD_GET_VEHICLE_VARIABLE, radius, VehDataSubscribeList, 0, tSimuEnd);
+
+						vehicleHasSubscribed_v[i] = true;
+					}
+
+					i++;
+				}
+				
+			}
+
+			// !!! need to sub all vehicles
+
+			//while Simulation::getMinExpectedNumber() > 0:
+			//for veh_id in Simulation::getDepartedIDList() :
+			//	traci.vehicle.subscribe(veh_id, [traci.constants.VAR_POSITION])
+			//	positions = traci.vehicle.getAllSubscriptionResults()
+			//	traci.simulationStep()
+
+
+		}
+		else {
+			int aa = 1;
+
+		}
+
+		// this might make it slightly faster to not get repeated vehicles
+		unordered_set <string> processedVehId_us;
+
+		// temp buffer to store all vehicle received
+		std::unordered_map <std::string, VehFullData_t> VehDataRecv_um_tmp;
+		// ===========================================================================
+		// 			GET SUBSCRIBED VEHICLE
+		// ===========================================================================
+		if (subscribeAllVehicle.first) {
+			
+			libsumo::SubscriptionResults VehSubRes = traci.vehicle.getAllSubscriptionResults();
+
+			for (auto& it : VehSubRes) {
+				std::string tempvehId = it.first;
+
+				
+				if (processedVehId_us.find(tempvehId) != processedVehId_us.end())
+					continue;
+				processedVehId_us.insert(tempvehId);
+
+				VehFullData_t CurVehData;
+				this->parserSumoSubscription(it.second, tempvehId, CurVehData);
+
+				VehDataRecv_um_tmp[tempvehId] = CurVehData;
+
+				if (ENABLE_VERBOSE) {
+					float speed = CurVehData.speed;
+					printf("recv SUMO veh(id=%s) speed %.4f\n", tempvehId.c_str(), speed);
+				}
+			}
+		}
+		else {
+			libsumo::ContextSubscriptionResults VehicleSubscribeRaw;
+			VehicleSubscribeRaw = traci.vehicle.getAllContextSubscriptionResults();
+
+			//{
+			//int i = 0;
+			//for (auto& it : vehicleSubscribeId_v) {
+			//	if (vehicleHasSubscribed_v[i]) {
+			for (auto& it : VehicleSubscribeRaw) {
+				//string id = it.first;
+				libsumo::SubscriptionResults VehDataSubscribeResults = it.second;
+
+				for (auto& iter : VehDataSubscribeResults) {
+
+					string tempvehId = iter.first;
+
+					VehFullData_t CurVehData;
+
+					if (processedVehId_us.find(tempvehId) == processedVehId_us.end()) {
+						processedVehId_us.insert(tempvehId);
+					}
+					else {
+						continue;
+					}
+
+					this->parserSumoSubscription(iter.second, tempvehId, CurVehData);
+					//libsumo::TraCIResults VehDataSubscribeTraciResults = VehDataSubscribeResults[tempvehId]
+
+					//=================
+					// save to Msg_c recv buffer
+					//=================
+					//Msg_c.VehDataRecvAll_v.push_back(CurVehData);
+					VehDataRecv_um_tmp[tempvehId] = CurVehData;
+
+					if (ENABLE_VERBOSE) {
+						float speed = CurVehData.speed;
+						printf("recv SUMO veh id %s veh speed %.4f\n", tempvehId.c_str(), speed);
+
+						FILE* f = fopen(MasterLogName.c_str(), "a");
+						fprintf(f, "recv SUMO veh id %s veh speed %.4f\n", tempvehId.c_str(), speed);
+						fclose(f);
+
+					}
+
+				}
+			}
+
+			/*		i++;
+
+				}
+				}*/
+
+				// ===========================================================================
+				// 			GET SUBSCRIBED point
+				// ===========================================================================
+			libsumo::ContextSubscriptionResults PointSubscribeRaw;
+			PointSubscribeRaw = traci.poi.getAllContextSubscriptionResults();
+
+			for (auto& it : PointSubscribeRaw) {
+				string poiName = it.first;
+				libsumo::SubscriptionResults VehDataSubscribeResults = it.second;
+
+				for (auto& iter : VehDataSubscribeResults) {
+
+					string tempvehId = iter.first;
+
+					VehFullData_t CurVehData;
+
+					if (processedVehId_us.find(tempvehId) == processedVehId_us.end()) {
+						processedVehId_us.insert(tempvehId);
+					}
+					else {
+						continue;
+					}
+
+					this->parserSumoSubscription(iter.second, tempvehId, CurVehData);
+					//libsumo::TraCIResults VehDataSubscribeTraciResults = VehDataSubscribeResults[tempvehId]
+
+					//=================
+					// save to Msg_c recv buffer
+					//=================
+					//Msg_c.VehDataRecvAll_v.push_back(CurVehData);
+					VehDataRecv_um_tmp[tempvehId] = CurVehData;
+
+					if (ENABLE_VERBOSE) {
+						float speed = CurVehData.speed;
+						printf("recv SUMO veh id %s veh speed %.4f\n", tempvehId.c_str(), speed);
+
+						FILE* f = fopen(MasterLogName.c_str(), "a");
+						fprintf(f, "recv SUMO veh id %s veh speed %.4f\n", tempvehId.c_str(), speed);
+						fclose(f);
+					}
+				}
+			}
+
+
+			// ===========================================================================
+			// 			GET SUBSCRIBED EDGE
+			// ===========================================================================
+			libsumo::ContextSubscriptionResults EdgeSubscribeRaw;
+			EdgeSubscribeRaw = traci.edge.getAllContextSubscriptionResults();
+
+
+			if (edgeHasSubscribed) {
+				nVehSend = 0;
+
+				int temp = 1;
+
+				for (auto& it : EdgeSubscribeRaw) {
+					libsumo::SubscriptionResults VehDataSubscribeResults = it.second;
+
+					auto iter = VehDataSubscribeResults.begin();
+					nVehSend = min((int)VehDataSubscribeResults.size(), 200);
+
+					vector <string> tempVehIdList;
+					//tempVehIdList.push_back(egoIdVec[iC]);
+					for (int iV = 0; iV < VehDataSubscribeResults.size(); iV++) {
+						//if (iter->first != egoIdVec[iC]) {
+						tempVehIdList.push_back(iter->first);
+						//}
+						iter++;
+					}
+
+					for (int iV = 0; iV < nVehSend; iV++) {
+						VehFullData_t CurVehData;
+
+						string tempvehId = tempVehIdList[iV];
+
+						if (processedVehId_us.find(tempvehId) == processedVehId_us.end()) {
+							processedVehId_us.insert(tempvehId);
+						}
+						else {
+							continue;
+						}
+
+						this->parserSumoSubscription(VehDataSubscribeResults[tempvehId], tempvehId, CurVehData);
+						//libsumo::TraCIResults VehDataSubscribeTraciResults = VehDataSubscribeResults[tempvehId]
+
+
+
+						//Msg_c.packVehData(CurVehData, tempVehDataBuffer[iC], &tempVehDataByte[iC]);
+						//tempVehDataSend_v.push_back(CurVehData);
+
+						//tempVehIdSend_v.push_back(tempvehId);
+
+						//=================
+						// save to Msg_c recv buffer          
+						//=================
+						//Msg_c.VehDataRecvAll_v.push_back(CurVehData);
+						VehDataRecv_um_tmp[tempvehId] = CurVehData;
+
+						if (ENABLE_VERBOSE) {
+							float speed = CurVehData.speed;
+							printf("recv SUMO veh id %s veh speed %.4f\n", tempvehId.c_str(), speed);
+
+							FILE* f = fopen(MasterLogName.c_str(), "a");
+							fprintf(f, "recv SUMO veh id %s veh speed %.4f\n", tempvehId.c_str(), speed);
+							fclose(f);
+
+						}
+
+						//}
+						/*VehData_t iVehFullData;
+						Sock_c.depackVehData(tempVehDataBuffer[iC]+3, &iVehFullData);*/
+					}
+				}
+				//nVehSend = tempVehIdSend_v.size();
+			}
+		}
+
+		// !!!temporary fix
+		// if doing vehicle simulator, e.g., CarMaker, only send limited number of vehicles
+		if (ENABLE_VEH_SIMULATOR) {
+			libsumo::TraCIPosition posEgo = traci.vehicle.getPosition(Config_c->CarMakerSetup.EgoId);
+
+			// sort distance, pair distance to ego, vehId
+			vector <pair <double, string>> dist2ego_v;
+			for (auto& it : VehDataRecv_um_tmp) {
+				dist2ego_v.push_back(make_pair(pow(it.second.positionX - posEgo.x, 2) + pow(it.second.positionY - posEgo.y, 2), it.first));
+			}
+			sort(dist2ego_v.begin(), dist2ego_v.end());
+
+			for (auto&it: dist2ego_v) {
+				string tempvehId = it.second;
+
+				if (this->shouldSendVehicle(tempvehId, *simTime)) {
+					Msg_c.VehDataRecv_um[tempvehId] = VehDataRecv_um_tmp[tempvehId];
+				}
+
+				if (Msg_c.VehDataRecv_um.size() >= N_MAX_VEH) {
+					break;
+				}
+			}
+				
+		}
+		else {
+			for (auto& it : VehDataRecv_um_tmp) {
+				string tempvehId = it.first;
+
+				// if not doing driving simulator, shouldSendVehicle will currently always return 1
+				if (this->shouldSendVehicle(tempvehId, *simTime)) {
+					Msg_c.VehDataRecv_um[tempvehId] = VehDataRecv_um_tmp[tempvehId];
+				}
+			}
+		}
+
+
+		//=================
+		// remove vehicle from list
+		//=================
+		//vector <string> vehArrivedIdList = Simulation::getArrivedIDList();
+		vector <string> vehArrivedIdList =  traci.simulation.getArrivedIDList();
+		for (int i = 0; i < vehArrivedIdList.size(); i++) {
+			VehicleId2EdgeList_um.erase(vehArrivedIdList[i]);
+		}
+
+	}
+
+	//char tempDetDataBuffer[NCLIENT_MACRO][8096];
+	//int tempDetDataByte[NCLIENT_MACRO] = { 0 };
+
+	if (ENABLE_DET_SUB) {
+		if (!DetectorSubscriptionFlags.patternHasSubscribed) {
+			//===================================================
+			// Retreive DETECTOR configuration for the scenario BEFORE simulation starts
+			//===================================================
+			vector <string> detAreaAllId_v = traci.lanearea.getIDList();
+			vector <string> detInductAllId_v = traci.inductionloop.getIDList();
+
+			// obtain detector ids of the selected intersection and subscribe to results
+
+			vector <int> detSubscribeList = {libsumo::LAST_STEP_VEHICLE_NUMBER};
+
+			for (auto it: Config_c->SubscriptionDetectorList.pattern_v){
+				for (int iD = 0; iD < detAreaAllId_v.size(); iD++) {
+					if (detAreaAllId_v[iD].find(it) != std::string::npos) {
+						traci.lanearea.subscribe(detAreaAllId_v[iD], detSubscribeList, 0, tSimuEnd);
+					}
+				}
+			}
+
+			DetectorSubscriptionFlags.patternHasSubscribed = true;
+
+		}
+
+		libsumo::SubscriptionResults DetSubscribeRaw;
+		DetSubscribeRaw = traci.lanearea.getAllSubscriptionResults();
+
+
+		vector <DetectorData_t> tempDetData_v;
+		
+		std::shared_ptr<libsumo::TraCIInt> tempIntPtr;
+
+		for (auto& it : DetSubscribeRaw) {
+			DetectorData_t curDet;
+
+			curDet.id = 0;
+			curDet.name = it.first;
+			tempIntPtr = static_pointer_cast<libsumo::TraCIInt> (it.second[libsumo::LAST_STEP_VEHICLE_NUMBER]);
+			if (tempIntPtr->value > 0) {
+				curDet.state = 1;
+			}
+			else {
+				curDet.state = 0;
+			};
+
+			tempDetData_v.push_back(curDet);
+		}
+
+		Msg_c.DetDataRecv_um["NA"] = make_tuple(0, "NA", tempDetData_v);
+
+	}
+
+	if (ENABLE_SIG_SUB) {
+		if (!SignalSubscriptionFlags.idHasSubscribed) {
+			//===================================================
+			// Retreive DETECTOR configuration for the scenario BEFORE simulation starts
+			//===================================================
+			vector <string> sigAllId_v = traci.trafficlights.getIDList();
+
+			// obtain detector ids of the selected intersection and subscribe to results
+
+			vector <int> sigSubscribeList = { libsumo::TL_RED_YELLOW_GREEN_STATE };
+
+			if (!Config_c->SubscriptionSignalList.subAllSignalFlag) {
+				for (auto it : Config_c->SubscriptionSignalList.signalId_v) {
+					traci.trafficlights.subscribe(it.c_str(), sigSubscribeList, 0, tSimuEnd);
+				}
+			}
+			else {
+				for (auto it : sigAllId_v) {
+					traci.trafficlights.subscribe(it.c_str(), sigSubscribeList, 0, tSimuEnd);
+				}
+			}
+
+			SignalSubscriptionFlags.idHasSubscribed = true;
+
+		}
+
+		// if already subscribed, then get signal data out
+		libsumo::SubscriptionResults SigSubscribeRaw;
+		SigSubscribeRaw = traci.trafficlights.getAllSubscriptionResults();
+
+		vector <TrafficLightData_t> tempSigData_v;
+
+		std::shared_ptr<libsumo::TraCIString> tempStringPtr;
+
+		int idx = 0;
+		for (auto& it : SigSubscribeRaw) {
+			TrafficLightData_t curSig;
+
+			curSig.id = idx++;
+			curSig.name = it.first;
+			tempStringPtr = static_pointer_cast<libsumo::TraCIString> (it.second[libsumo::TL_RED_YELLOW_GREEN_STATE]);
+			curSig.state = tempStringPtr->value;
+
+			Msg_c.TlsDataRecv_um[curSig.name] = curSig;
+
+		}
+
+
+
+	}
+
+	return recvStatus;
+}
+
+
+// this is only for visualization
+int TrafficHelper::shouldSendVehicle(std::string vehicleId, double simTime) {
+	int shouldSendFlag = 0;
+
+	if (!ENABLE_VEH_SIMULATOR) {
+		shouldSendFlag = 1;
+	}
+	else if (ENABLE_VEH_SIMULATOR){
+		if (vehicleId.compare(Config_c->CarMakerSetup.EgoId) == 0) {
+			shouldSendFlag = 1;
+		}
+		else {
+			// if does not have yet, add it
+			if (VehicleId2SubCount_um.find(vehicleId) == VehicleId2SubCount_um.end()) {
+				VehicleId2SubCount_um[vehicleId] = 0;
+			}
+			else {
+				// if last sub and this one is continous, then add count by 1, otherwise reset count
+				if (abs(VehicleId2LastSubTime_um[vehicleId] - simTime) <= SIM_STEP + 1e-5) {
+					VehicleId2SubCount_um[vehicleId] = VehicleId2SubCount_um[vehicleId] + 1;
+				}
+				else {
+					VehicleId2SubCount_um[vehicleId] = 0;
+				}
+			}
+			VehicleId2LastSubTime_um[vehicleId] = simTime;
+
+			// check last sub time
+			if (VehicleId2SubCount_um[vehicleId] > SUB_CONT_TIME_THLD / SIM_STEP) {
+				shouldSendFlag = 1;
+			}
+		}
+	}
+
+	return shouldSendFlag;
+}
+
+
+void TrafficHelper::parserSumoSubscription(libsumo::TraCIResults VehDataSubscribeTraciResults, std::string vehId, VehFullData_t& CurVehData) {
+
+	// if does not have this vehicle yet
+	if (VehicleId2EdgeList_um.find(vehId) == VehicleId2EdgeList_um.end()) {
+		vector <string> edgeList = traci.vehicle.getRoute(vehId);
+		VehicleId2EdgeList_um[vehId] = edgeList;
+		//vector <libsumo::TraCIConnection> nextLinkList = traci.vehicle.getNextLinks(vehId);
+		//int aa = 1;
+	}
+	
+
+	//libsumo::TraCIResults VehDataSubscribeTraciResults = VehDataSubscribeResults[tempvehId];
+	std::shared_ptr<libsumo::TraCIString> tempStringPtr;
+	std::shared_ptr<libsumo::TraCIInt> tempIntPtr;
+	std::shared_ptr<libsumo::TraCIDouble> tempDoublePtr;
+	std::shared_ptr<libsumo::TraCIPosition> tempPositionPtr;
+	std::shared_ptr<libsumo::TraCIColor> tempColorPtr;
+	std::shared_ptr<libsumo::TraCIDouble> tempDoublePtr2;
+
+	CurVehData.id = vehId;
+
+	//for (int iD = 0; iD < VehDataSubscribeList.size(); iD++) {
+	tempStringPtr = static_pointer_cast<libsumo::TraCIString> (VehDataSubscribeTraciResults[libsumo::VAR_TYPE]);
+	CurVehData.type = tempStringPtr->value;
+
+	tempDoublePtr = static_pointer_cast<libsumo::TraCIDouble> (VehDataSubscribeTraciResults[libsumo::VAR_SPEED]);
+	CurVehData.speed = tempDoublePtr->value;
+
+	tempDoublePtr = static_pointer_cast<libsumo::TraCIDouble> (VehDataSubscribeTraciResults[libsumo::VAR_SPEED_WITHOUT_TRACI]);
+	float speedWithoutTraci = tempDoublePtr->value;
+
+
+	tempPositionPtr = static_pointer_cast<libsumo::TraCIPosition> (VehDataSubscribeTraciResults[libsumo::VAR_POSITION3D]);
+	CurVehData.positionX = tempPositionPtr->x;
+	CurVehData.positionY = tempPositionPtr->y;
+	CurVehData.positionZ = tempPositionPtr->z;
+
+	tempDoublePtr = static_pointer_cast<libsumo::TraCIDouble> (VehDataSubscribeTraciResults[libsumo::VAR_ANGLE]);
+	CurVehData.heading = tempDoublePtr->value;
+
+	tempColorPtr = static_pointer_cast<libsumo::TraCIColor> (VehDataSubscribeTraciResults[libsumo::VAR_COLOR]);
+	uint8_t r = tempColorPtr->r;
+	uint8_t g = tempColorPtr->g;
+	uint8_t b = tempColorPtr->b;
+	uint8_t a = tempColorPtr->a;
+	CurVehData.color = (r << 24) + (g << 16) + (b << 8) + a;
+
+	tempStringPtr = static_pointer_cast<libsumo::TraCIString> (VehDataSubscribeTraciResults[libsumo::VAR_ROAD_ID]);
+	CurVehData.linkId = tempStringPtr->value;
+
+	tempIntPtr = static_pointer_cast<libsumo::TraCIInt> (VehDataSubscribeTraciResults[libsumo::VAR_LANE_INDEX]);
+	CurVehData.laneId = (int32_t) tempIntPtr->value;
+
+	tempDoublePtr = static_pointer_cast<libsumo::TraCIDouble> (VehDataSubscribeTraciResults[libsumo::VAR_DISTANCE]);
+	//tempDoublePtr = static_pointer_cast<libsumo::TraCIDouble> (VehDataSubscribeTraciResults[libsumo::VAR_LANEPOSITION]);
+	CurVehData.distanceTravel = (float) tempDoublePtr->value;
+
+
+	tempDoublePtr = static_pointer_cast<libsumo::TraCIDouble> (VehDataSubscribeTraciResults[libsumo::VAR_ACCELERATION]);
+	CurVehData.acceleration = tempDoublePtr->value;
+
+	CurVehData.speedDesired = CurVehData.speed;
+	CurVehData.accelerationDesired = 0.0;
+
+	//=================
+	// get preceding vehicle
+	//=================
+	pair<string, double> leaderIdNSpeed = traci.vehicle.getLeader(vehId, 1000);
+	CurVehData.precedingVehicleId = get<0>(leaderIdNSpeed);
+	CurVehData.precedingVehicleDistance = get<1>(leaderIdNSpeed);
+	CurVehData.hasPrecedingVehicle = 0;
+	CurVehData.precedingVehicleSpeed = -1.0;
+	if (CurVehData.precedingVehicleId.compare("") != 0) {
+		CurVehData.hasPrecedingVehicle = 1;
+		CurVehData.precedingVehicleSpeed = traci.vehicle.getSpeed(CurVehData.precedingVehicleId);
+	}
+
+	//=================
+	// get signal information
+	//=================
+	vector <libsumo::TraCINextTLSData> nextTlsList = traci.vehicle.getNextTLS(vehId);
+
+	if (nextTlsList.size() > 0) {
+		CurVehData.signalLightId = nextTlsList[0].id;
+		CurVehData.signalLightHeadId = nextTlsList[0].tlIndex;
+		CurVehData.signalLightDistance = nextTlsList[0].dist;
+		char tlsState = nextTlsList[0].state;
+		CurVehData.signalLightColor = 0;
+
+		if (tlsState == 'r') {
+			CurVehData.signalLightColor = 1;
+		}
+		else if (tlsState == 'y') {
+			CurVehData.signalLightColor = 2;
+		}
+		else if (tlsState == 'g' || tlsState == 'G') {
+			CurVehData.signalLightColor = 3;
+		}
+		else if (tlsState == 'u') {
+			CurVehData.signalLightColor = 4;
+		}
+		else if (tlsState == 'o') {
+			CurVehData.signalLightColor = 5;
+		}
+		else if (tlsState == 'O') {
+			CurVehData.signalLightColor = 6;
+		}
+		else if (tlsState == 's') {
+			CurVehData.signalLightColor = 7;
+		}
+
+		if (nextTlsList.size() > 1) {
+			if (ENABLE_VERBOSE) {
+
+				FILE* f = fopen(MasterLogName.c_str(), "a");
+				fprintf(f, "received more than 1 nextTLS vehId %s \n", vehId.c_str());
+				fclose(f);
+
+			}
+		}
+	}
+	else {
+		CurVehData.signalLightId = "";
+		CurVehData.signalLightHeadId = -1;
+		CurVehData.signalLightDistance = -1.0;
+		CurVehData.signalLightColor = -1;
+	}
+
+	//=================
+	// get speed limit
+	//=================
+	// retrieve current speed limit
+	tempDoublePtr = static_pointer_cast<libsumo::TraCIDouble> (VehDataSubscribeTraciResults[libsumo::VAR_ALLOWED_SPEED]);
+	tempDoublePtr2 = static_pointer_cast<libsumo::TraCIDouble> (VehDataSubscribeTraciResults[libsumo::VAR_SPEED_FACTOR]);
+	CurVehData.speedLimit = tempDoublePtr->value / tempDoublePtr2->value;
+
+	// retrieve next speed limit
+	vector <string> edgeRouteList = VehicleId2EdgeList_um[vehId];
+
+	tempIntPtr = static_pointer_cast<libsumo::TraCIInt> (VehDataSubscribeTraciResults[libsumo::VAR_ROUTE_INDEX]);
+	int edgeListIdx = tempIntPtr->value;
+	CurVehData.linkIdNext = "";
+	if (edgeListIdx + 1 < edgeRouteList.size()) {
+		CurVehData.linkIdNext = edgeRouteList[edgeListIdx + 1];
+	}
+
+	tempStringPtr = static_pointer_cast<libsumo::TraCIString> (VehDataSubscribeTraciResults[libsumo::VAR_VEHICLECLASS]);
+	CurVehData.vehicleClass = tempStringPtr->value;
+
+	CurVehData.speedLimitNext = -1;
+	// if there is speed limit for next link and vehclass
+	if (EdgeVehClass2SpeedLimit_um.find(make_pair(CurVehData.linkIdNext, CurVehData.vehicleClass)) != EdgeVehClass2SpeedLimit_um.end()) {
+		CurVehData.speedLimitNext = EdgeVehClass2SpeedLimit_um[make_pair(CurVehData.linkIdNext, CurVehData.vehicleClass)];
+	}
+
+	// for following information, need to get them one by one rather than through subscription
+	CurVehData.speedLimitChangeDistance = -1;
+	//if (AllEdgeList.find(CurVehData.linkIdNext) != AllEdgeList.end()) {
+	//	CurVehData.speedLimitChangeDistance = max(traci.vehicle.getDrivingDistance(vehId, CurVehData.linkIdNext, 0), -1.0);
+	//}
+
+	//=================
+	// grade
+	//=================
+	tempDoublePtr = static_pointer_cast<libsumo::TraCIDouble> (VehDataSubscribeTraciResults[libsumo::VAR_SLOPE]);
+	CurVehData.grade = tempDoublePtr->value * M_PI/180;
+
+	//=================
+	// get lane change
+	//=================
+	//vector <libsumo::TraCIBestLanesData> bestLanesData = traci.vehicle.getBestLanes(vehId);
+	CurVehData.activeLaneChange = 0;
+
+
+	//=================
+	// get length, width, height
+	//=================
+	tempDoublePtr = static_pointer_cast<libsumo::TraCIDouble> (VehDataSubscribeTraciResults[libsumo::VAR_LENGTH]);
+	CurVehData.length = tempDoublePtr->value;
+
+	tempDoublePtr = static_pointer_cast<libsumo::TraCIDouble> (VehDataSubscribeTraciResults[libsumo::VAR_WIDTH]);
+	CurVehData.width = tempDoublePtr->value;
+
+	tempDoublePtr = static_pointer_cast<libsumo::TraCIDouble> (VehDataSubscribeTraciResults[libsumo::VAR_HEIGHT]);
+	CurVehData.height = tempDoublePtr->value;
+	//=================
+	// get vehicle indicators
+	//=================
+	//Name	Bit
+	//VEH_SIGNAL_BLINKER_RIGHT	0
+	//VEH_SIGNAL_BLINKER_LEFT	1
+	//VEH_SIGNAL_BLINKER_EMERGENCY	2
+	//VEH_SIGNAL_BRAKELIGHT	3
+	//VEH_SIGNAL_FRONTLIGHT	4
+	//VEH_SIGNAL_FOGLIGHT	5
+	//VEH_SIGNAL_HIGHBEAM	6
+	//VEH_SIGNAL_BACKDRIVE	7
+	//VEH_SIGNAL_WIPER	8
+	//VEH_SIGNAL_DOOR_OPEN_LEFT	9
+	//VEH_SIGNAL_DOOR_OPEN_RIGHT	10
+	//VEH_SIGNAL_EMERGENCY_BLUE	11
+	//VEH_SIGNAL_EMERGENCY_RED	12
+	//VEH_SIGNAL_EMERGENCY_YELLOW	13
+	tempIntPtr = static_pointer_cast<libsumo::TraCIInt> (VehDataSubscribeTraciResults[libsumo::VAR_SIGNALS]);
+	// (n & (1 << k)) >> k
+	CurVehData.lightIndicators = tempIntPtr->value;
+}
+
+
+
+int TrafficHelper::recvFromVISSIM(double* simTime, MsgHelper& Msg_c) {
+	int recvStatus = 0;
+
+	SocketHelper Sock_temp;
+	
+	int simStateRecv;
+	float simTimeRecv;
+
+	if (ENABLE_VERBOSE) {
+		printf("receive VISSIM ......\n");
+	
+		FILE* f = fopen(MasterLogName.c_str(), "a");
+		fprintf(f, "recv VISSIM\n");
+		fclose(f);
+	}
+
+	for (int i = 0; i < vissimSock.size(); i++) {
+		if (ENABLE_VERBOSE) {
+			printf("receive VISSIM %s \n", vissimSockName_um[vissimSock[i]].c_str());
+		
+			//FILE* f = fopen(MasterLogName.c_str(), "a");
+			//fprintf(f, "receive VISSIM %s \n", vissimSockName_um[vissimSock[i]].c_str());
+			//fclose(f);
+		}
+
+		//if (vissimSock.size() > 1 && vissimSockName_um[vissimSock[i]].find(SIGNAL_SOCK_PATTERN) != string::npos) {
+		//	// if is signal controller
+		//	// if simTime has reached steps for SC
+		//	if (abs(round((*simTime- vissimBaseDt) / (vissimSignalStep * vissimBaseDt)) - (*simTime - vissimBaseDt) / (vissimSignalStep * vissimBaseDt)) < 1e-5) {
+		//		// if not simTime = 0
+		//		if (*simTime > (vissimSignalStep * vissimBaseDt) - 1e-5) {
+		//			Sock_temp.recvData(vissimSock[i], &simStateRecv, &simTimeRecv, Msg_c);
+		//			*simTime = simTimeRecv;
+		//		}
+		//	}
+		//}
+		//else {
+		//	Sock_temp.recvData(vissimSock[i], &simStateRecv, &simTimeRecv, Msg_c);
+		//	*simTime = simTimeRecv;
+		//}
+		recvStatus = Sock_temp.recvData(vissimSock[i], &simStateRecv, &simTimeRecv, Msg_c);
+		*simTime = simTimeRecv;
+
+		if (recvStatus < 0) {
+			return -1;
+		}
+
+		if (ENABLE_VERBOSE) {
+			printf("\treceive simTime %f\n", *simTime);
+
+			FILE* f = fopen(MasterLogName.c_str(), "a");
+			fprintf(f, "\treceive simTime %f\n", *simTime);
+			fclose(f);
+
+			for (auto& it : Msg_c.VehDataRecv_um) {
+				printf("\treceive vehicle id %s\n", it.first.c_str());
+
+				FILE* f = fopen(MasterLogName.c_str(), "a");
+				fprintf(f, "\trecv veh id %s\n", it.first.c_str());
+				fclose(f);
+			}
+			for (auto& it : Msg_c.DetDataRecv_um) {
+				printf("\treceive detector at intersection id %s\n", it.first.c_str());
+
+				FILE* f = fopen(MasterLogName.c_str(), "a");
+				fprintf(f, "\treceive detector at intersection id %s\n", it.first.c_str());
+				fclose(f);
+			}
+			for (auto& it : Msg_c.TlsDataRecv_um) {
+				printf("\treceive signal intersection id %s\n", it.first.c_str());
+
+				FILE* f = fopen(MasterLogName.c_str(), "a");
+				fprintf(f, "\treceive signal intersection id %s\n", it.first.c_str());
+				fclose(f);
+			}
+		}
+	}
+
+	if (ENABLE_VERBOSE && recvStatus >= 0) {
+		printf("receive VISSIM complete \n");
+	
+		//FILE* f = fopen(MasterLogName.c_str(), "a");
+		//fprintf(f, "receive VISSIM complete \n");
+		//fclose(f);
+	}
+
+	int aa = 1;
+
+	return recvStatus;
 }
\ No newline at end of file
diff --git a/CommonLib/TrafficHelper.h b/CommonLib/TrafficHelper.h
index 64774fc7..c588e3bd 100644
--- a/CommonLib/TrafficHelper.h
+++ b/CommonLib/TrafficHelper.h
@@ -46,6 +46,9 @@ class TrafficHelper
 	int addEgoVehicle(double simTime);
 	int addEgoVehicleFromXY(double simTime, std::string vehicleId, std::string vehicleType, double positionX, double positionY);
 	int checkIfEgoExist(double* simTime);
+	void updateEgoLoopRoute();
+	void fillTrafficLightStates(MsgHelper& msg);
+
 
 	int getSimulationTime(double* simTime);
 
@@ -138,7 +141,6 @@ class TrafficHelper
 	bool ENABLE_VEH_SIMULATOR = false;
 
 	bool ENABLE_CARLA = false;
-	bool ENABLE_CARLA_EXTERNAL_CONTROL = false;
 
 	double tSimuEnd = 90000;
 
diff --git a/CommonLib/VehDataMsgDefs.h b/CommonLib/VehDataMsgDefs.h
index 38946a21..26bc2265 100644
--- a/CommonLib/VehDataMsgDefs.h
+++ b/CommonLib/VehDataMsgDefs.h
@@ -4,9 +4,6 @@
 #include <unordered_map>
 #include <vector>
 #include <string>
-#ifdef RS_DSPACE
-#include <cstdint>
-#endif
 // MESSAGE IDENTIFIER 1
 // Full vehicle data structure that will be shared between SUMO and other simulators
 // !!! This does not necessary mean all data directly communicated between simulators
diff --git a/CommonLib/libRealSimDsLib_2021b.a b/CommonLib/libRealSimDsLib_2021b.a
new file mode 100644
index 00000000..abf218b8
Binary files /dev/null and b/CommonLib/libRealSimDsLib_2021b.a differ
diff --git a/CommonLib/libRealSimDsLib_2024a.a b/CommonLib/libRealSimDsLib_2024a.a
new file mode 100644
index 00000000..963f19ad
Binary files /dev/null and b/CommonLib/libRealSimDsLib_2024a.a differ
diff --git a/ProprietaryFiles b/ProprietaryFiles
index 2f6d8b8d..dfcebac1 160000
--- a/ProprietaryFiles
+++ b/ProprietaryFiles
@@ -1 +1 @@
-Subproject commit 2f6d8b8dcaa8403ad1aaa87a903ac09a67d71615
+Subproject commit dfcebac1f0ea2aa5148954da3d5d8651b203269f
diff --git a/TrafficLayer/TrafficLayer/mainTrafficLayer.cpp b/TrafficLayer/TrafficLayer/mainTrafficLayer.cpp
index 3bc5008f..dbc36bbe 100644
--- a/TrafficLayer/TrafficLayer/mainTrafficLayer.cpp
+++ b/TrafficLayer/TrafficLayer/mainTrafficLayer.cpp
@@ -1,138 +1,28 @@
-#include <string>
+#include <iostream>
+#include <fstream>
+#include <unordered_map>
+#include <unordered_set>
+#include <conio.h>
+
+
+//#include "TraCIAPI.h"
+
+
+//#include <event2/bufferevent.h>
+//#include <event2/util.h>
+//#include <event2/event.h>
+
+//#define TRACI_ENABLED 1
 #include <ctime>
 #include <chrono>
-#include <filesystem>
-#include <system_error>
-#include <cstdlib>
-#include <cstring>
+
 
 #include "TrafficHelper.h"
 
 #include "RealSimVersion.h"
 
-// Uncomment the line below to enable performance timing
-// #define ENABLE_PERF_TIMING
-#include "PerformanceTimer.h"
-
 using namespace std;
 
-// Global shutdown state - single point of access for cleanup coordination
-struct ShutdownState {
-    volatile bool shutdownRequested = false;
-    bool initialized = false;
-
-    // Pointers to resources that need cleanup (set after initialization)
-    TrafficHelper* traffic = nullptr;
-    SocketHelper* socket = nullptr;
-    MsgHelper* msgClient = nullptr;
-    vector<int>* clientSockets = nullptr;
-    double* simTime = nullptr;
-
-    // Configuration flags (set during initialization)
-    bool enableVissim = false;
-    bool enableClient = false;
-    bool enableRealSim = false;
-
-    // Track if traffic simulator connection was already closed
-    bool trafficSimulatorClosed = false;
-
-    // Reset all state
-    void reset() {
-        shutdownRequested = false;
-        initialized = false;
-        traffic = nullptr;
-        socket = nullptr;
-        msgClient = nullptr;
-        clientSockets = nullptr;
-        simTime = nullptr;
-        enableVissim = false;
-        enableClient = false;
-        enableRealSim = false;
-        trafficSimulatorClosed = false;
-    }
-};
-
-// Single global instance
-static ShutdownState g_shutdown;
-
-namespace {
-std::string GetExecutableDirectory() {
-#ifdef WIN32
-    std::vector<char> buffer(MAX_PATH);
-    DWORD length = 0;
-    while (true) {
-        length = GetModuleFileNameA(nullptr, buffer.data(), static_cast<DWORD>(buffer.size()));
-        if (length == 0) {
-            return std::string();
-        }
-        if (length < buffer.size() - 1) {
-            return std::filesystem::path(std::string(buffer.data(), length)).parent_path().string();
-        }
-        buffer.resize(buffer.size() * 2);
-    }
-#else
-    std::vector<char> buffer(PATH_MAX);
-    ssize_t length = readlink("/proc/self/exe", buffer.data(), buffer.size() - 1);
-    if (length == -1) {
-        return std::string();
-    }
-    buffer[length] = '\0';
-    return std::filesystem::path(std::string(buffer.data())).parent_path().string();
-#endif
-}
-
-void ConfigureSumoLibraryPath(const ConfigHelper& config) {
-    static bool configured = false;
-    if (configured) return;
-    configured = true;
-
-    std::filesystem::path exeDir(GetExecutableDirectory());
-    std::vector<std::filesystem::path> candidates;
-
-    // Override path first
-    if (!config.SumoSetup.RuntimeLibraryPath.empty()) {
-        std::filesystem::path override(config.SumoSetup.RuntimeLibraryPath);
-        candidates.push_back(override.is_absolute() ? override : exeDir / override);
-    }
-
-    // Standard locations
-    if (!exeDir.empty()) {
-        candidates.push_back(exeDir / "libsumo" / "bin");
-        std::filesystem::path parent = exeDir.parent_path();
-        for (int i = 0; i < 5 && !parent.empty(); ++i, parent = parent.parent_path()) {
-            candidates.push_back(parent / "CommonLib" / "libsumo" / "bin");
-        }
-    }
-    candidates.push_back(std::filesystem::current_path() / "CommonLib" / "libsumo" / "bin");
-
-    // Try each candidate
-    for (const auto& path : candidates) {
-        std::error_code ec;
-        if (!std::filesystem::is_directory(path, ec)) continue;
-
-        std::string pathStr = path.string();
-#ifdef WIN32
-        if (!SetDllDirectoryA(pathStr.c_str())) continue;
-#else
-        std::string newPath = pathStr;
-        if (const char* current = std::getenv("LD_LIBRARY_PATH")) {
-            newPath += ':' + std::string(current);
-        }
-        setenv("LD_LIBRARY_PATH", newPath.c_str(), 1);
-#endif
-        printf("Using SUMO library directory: %s\n", pathStr.c_str());
-        return;
-    }
-
-    // Only error if SUMO is selected as traffic simulator
-    if (config.SimulationSetup.SelectedTrafficSimulator != "VISSIM") {
-        printf("ERROR: Unable to locate SUMO library directory.\n");
-        printf("Please check SumoSetup.RuntimeLibraryPath in your configuration file.\n");
-        exit(-1);
-    }
-}
-}
-
 //!!!!! NEED TO
 // -multithread
 // -add lane position
@@ -158,86 +48,6 @@ SocketHelper Sock_c;
 
 
 
-// Perform graceful cleanup with proper shutdown sequence
-void performCleanup(bool emergencyShutdown) {
-	if (emergencyShutdown) {
-		printf("\nEmergency shutdown initiated...\n");
-	}
-	else {
-		printf("\nGraceful shutdown initiated...\n");
-	}
-
-	// Step 1: Notify all connected clients about shutdown (state=0)
-	if (g_shutdown.initialized && g_shutdown.enableClient &&
-		g_shutdown.socket && g_shutdown.msgClient &&
-		g_shutdown.clientSockets && g_shutdown.simTime) {
-
-		try {
-			printf("Notifying clients of shutdown...\n");
-			MsgHelper* msgClient = g_shutdown.msgClient;
-			msgClient->clearSendStorage();
-
-			float simTimeSend = *g_shutdown.simTime;
-			uint8_t simStateSend = 0; // 0 = shutdown signal
-
-			for (unsigned int iC = 0; iC < g_shutdown.clientSockets->size(); iC++) {
-				try {
-					g_shutdown.socket->sendData((*g_shutdown.clientSockets)[iC], iC,
-						simTimeSend, simStateSend, *msgClient);
-				}
-				catch (...) {
-					// Continue notifying other clients even if one fails
-				}
-			}
-			printf("Client notification complete.\n");
-
-			// Give clients time to receive and process shutdown notification
-			Sleep(100);
-		}
-		catch (const std::exception& e) {
-			printf("Warning: Error notifying clients: %s\n", e.what());
-		}
-		catch (...) {
-			printf("Warning: Unknown error notifying clients.\n");
-		}
-	}
-
-	// Step 2: Close SUMO/VISSIM connection gracefully
-	if (g_shutdown.initialized && g_shutdown.traffic && g_shutdown.enableRealSim && !g_shutdown.trafficSimulatorClosed) {
-		try {
-			printf("Closing traffic simulator connection...\n");
-			g_shutdown.traffic->close();
-			printf("Traffic simulator connection closed.\n");
-		}
-		catch (const std::exception& e) {
-			printf("Warning: Error closing traffic simulator: %s\n", e.what());
-		}
-		catch (...) {
-			printf("Warning: Unknown error closing traffic simulator.\n");
-		}
-	}
-	else if (g_shutdown.trafficSimulatorClosed) {
-		printf("Traffic simulator already closed by user.\n");
-	}
-
-	// Step 3: Close all sockets
-	if (g_shutdown.socket) {
-		try {
-			printf("Closing sockets...\n");
-			g_shutdown.socket->socketShutdown();
-			printf("Sockets closed.\n");
-		}
-		catch (const std::exception& e) {
-			printf("Warning: Error closing sockets: %s\n", e.what());
-		}
-		catch (...) {
-			printf("Warning: Unknown error closing sockets.\n");
-		}
-	}
-
-	printf("Shutdown complete.\n");
-}
-
 BOOL WINAPI CtrlHandler(DWORD fdwCtrlType)
 {
 	switch (fdwCtrlType)
@@ -245,31 +55,10 @@ BOOL WINAPI CtrlHandler(DWORD fdwCtrlType)
 		// Handle the CTRL-C signal.
 	case CTRL_C_EVENT:
 		printf("Ctrl-C event caught\n\n");
-		g_shutdown.shutdownRequested = true;
-		return TRUE;
-
-	// Handle Ctrl-Break signal
-	case CTRL_BREAK_EVENT:
-		printf("Ctrl-Break event caught\n\n");
-		g_shutdown.shutdownRequested = true;
-		return TRUE;
-
-	// Handle console window close (X button)
-	case CTRL_CLOSE_EVENT:
-		printf("Console close event caught\n\n");
-		performCleanup(true); // Emergency cleanup (Windows gives ~5 seconds)
-		return TRUE;
-
-	// Handle user logoff
-	case CTRL_LOGOFF_EVENT:
-		printf("User logoff event caught\n\n");
-		performCleanup(true); // Emergency cleanup
-		return TRUE;
+		printf("RealSim Exits, Please stop VISSIM/SUMO manually\n\n");
+		Sock_c.socketShutdown();
+		exit(-1);
 
-	// Handle system shutdown
-	case CTRL_SHUTDOWN_EVENT:
-		printf("System shutdown event caught\n\n");
-		performCleanup(true); // Emergency cleanup
 		return TRUE;
 
 	default:
@@ -286,94 +75,11 @@ static void show_usage(std::string name)
 		<< std::endl;
 }
 
-#ifdef WIN32
-// Find all YAML files in a directory recursively (Windows implementation)
-void findYamlFilesRecursive(const std::string& directory, std::vector<std::string>& yamlFiles) {
-	WIN32_FIND_DATAA findData;
-	std::string searchPath = directory + "\\*";
-	HANDLE hFind = FindFirstFileA(searchPath.c_str(), &findData);
-
-	if (hFind == INVALID_HANDLE_VALUE) {
-		return;
-	}
-
-	do {
-		std::string fileName = findData.cFileName;
-		if (fileName == "." || fileName == "..") continue;
-
-		std::string fullPath = directory + "\\" + fileName;
-
-		if (findData.dwFileAttributes & FILE_ATTRIBUTE_DIRECTORY) {
-			// Recursively search subdirectories
-			findYamlFilesRecursive(fullPath, yamlFiles);
-		}
-		else {
-			// Check if file has .yaml or .yml extension
-			size_t dotPos = fileName.find_last_of('.');
-			if (dotPos != std::string::npos) {
-				std::string ext = fileName.substr(dotPos);
-				if (ext == ".yaml" || ext == ".yml") {
-					yamlFiles.push_back(fullPath);
-				}
-			}
-		}
-	} while (FindNextFileA(hFind, &findData));
-
-	FindClose(hFind);
-}
-#else
-// Find all YAML files in a directory recursively (POSIX implementation)
-#include <dirent.h>
-#include <sys/stat.h>
-void findYamlFilesRecursive(const std::string& directory, std::vector<std::string>& yamlFiles) {
-	DIR* dir = opendir(directory.c_str());
-	if (!dir) return;
-
-	struct dirent* entry;
-	while ((entry = readdir(dir)) != nullptr) {
-		std::string fileName = entry->d_name;
-		if (fileName == "." || fileName == "..") continue;
-
-		std::string fullPath = directory + "/" + fileName;
-
-		struct stat statbuf;
-		if (stat(fullPath.c_str(), &statbuf) == 0) {
-			if (S_ISDIR(statbuf.st_mode)) {
-				// Recursively search subdirectories
-				findYamlFilesRecursive(fullPath, yamlFiles);
-			}
-			else if (S_ISREG(statbuf.st_mode)) {
-				// Check if file has .yaml or .yml extension
-				size_t dotPos = fileName.find_last_of('.');
-				if (dotPos != std::string::npos) {
-					std::string ext = fileName.substr(dotPos);
-					if (ext == ".yaml" || ext == ".yml") {
-						yamlFiles.push_back(fullPath);
-					}
-				}
-			}
-		}
-	}
-	closedir(dir);
-}
-#endif
-
-std::vector<std::string> findYamlFiles(const std::string& directory) {
-	std::vector<std::string> yamlFiles;
-	findYamlFilesRecursive(directory, yamlFiles);
-	return yamlFiles;
-}
-
 int main(int argc, char* argv[]) {
 
 	printf("==================================================\n");
 	printf("\t\tRealSim Interface\n");
 	printf("\t\t    v%s\n",REALSIM_VERSION_STRING);
-#ifdef _DEBUG
-	printf("\t\t    (DEBUG build)\n");
-#else
-	printf("\t\t    (RELEASE build)\n");
-#endif
 	printf("==================================================\n");
 
 	// control-c handles
@@ -447,10 +153,7 @@ int main(int argc, char* argv[]) {
 		}
 	}
 
-	string configPath;
-	bool configSpecified = false;
-
-	// Parse command-line arguments
+	string configPath = ".\\ecodrivingConfig.yaml";
 	for (int i = 1; i < argc; i++) {
 		string arg = argv[i];
 		if (arg == "-h" || arg == "--help") {
@@ -460,7 +163,6 @@ int main(int argc, char* argv[]) {
 		else if (arg == "-f" || arg == "--file") {
 			if (i + 1 < argc) {
 				configPath = argv[++i];
-				configSpecified = true;
 			}
 			else {
 				std::cerr << "--path option requires one argument." << std::endl;
@@ -474,43 +176,6 @@ int main(int argc, char* argv[]) {
 		}
 	}
 
-	// Auto-discover config if not specified
-	if (!configSpecified) {
-		// First, check for TrafficLayer/.active_config file
-		std::ifstream activeConfigFile("TrafficLayer/.active_config");
-		if (activeConfigFile.good()) {
-			std::getline(activeConfigFile, configPath);
-			activeConfigFile.close();
-			printf("Using config from TrafficLayer/.active_config: %s\n", configPath.c_str());
-		}
-		else {
-			// Auto-discover in tests/UserScenarios/
-			std::vector<std::string> yamlFiles = findYamlFiles("tests/UserScenarios");
-
-			if (yamlFiles.empty()) {
-				printf("ERROR: No configuration specified and no YAML found in tests/UserScenarios/\n");
-				printf("Options:\n");
-				printf("  - Use: -f path/to/config.yaml\n");
-				printf("  - Add scenario to tests/UserScenarios/\n");
-				printf("  - Create TrafficLayer/.active_config with path to your config\n");
-				return -1;
-			}
-			else if (yamlFiles.size() == 1) {
-				configPath = yamlFiles[0];
-				printf("Auto-discovered config: %s\n", configPath.c_str());
-			}
-			else {
-				printf("ERROR: Multiple YAML files found in tests/UserScenarios/\n");
-				printf("Please create TrafficLayer/.active_config file with one of these paths:\n");
-				for (const auto& yaml : yamlFiles) {
-					printf("  - %s\n", yaml.c_str());
-				}
-				printf("\nExample: echo tests/UserScenarios/issue_85/config.yaml > TrafficLayer/.active_config\n");
-				return -1;
-			}
-		}
-	}
-
 	// ===========================================================================
 	// 			READ Config File
 	// ===========================================================================
@@ -541,7 +206,7 @@ int main(int argc, char* argv[]) {
 	// ENABLE_CARLA: CARLA simulator
 	bool ENABLE_VEH_SIMULATOR = false;
 	bool ENABLE_CARLA = false;
-	bool ENABLE_CARLA_EXTERNAL_CONTROL = false;
+
 	vector <int> selfServerPortAll = {};
 	vector <string> serverAddr = {};
 	vector <int> serverPort = {};
@@ -563,20 +228,19 @@ int main(int argc, char* argv[]) {
 	}
 	if (Config_c.SimulationSetup.SelectedTrafficSimulator.compare("VISSIM") == 0) {
 		ENABLE_VISSIM = true;
-		printf("Traffic Simulator: VISSIM\n");
+		printf("Selected Traffic Simulator VISSIM\n\n");
 	}
 	else {
 		ENABLE_VISSIM = false;
-		printf("Traffic Simulator: SUMO\n");
-		// Configure SUMO library path for runtime DLL loading
-		ConfigureSumoLibraryPath(Config_c);
+		printf("Selected Traffic Simulator SUMO\n\n");
 	}
 	if (Config_c.SimulationSetup.EnableVerboseLog) {
 		ENABLE_VERBOSE = true;
-		printf("Verbose logging: Enabled\n");
+		printf("Enable verbose logging\n\n");
 	}
 	else {
 		ENABLE_VERBOSE = false;
+		printf("No verbose logging\n\n");
 	}
 	if (Config_c.ApplicationSetup.EnableApplicationLayer) {
 		ENABLE_CLIENT = true;
@@ -599,7 +263,7 @@ int main(int argc, char* argv[]) {
 
 	ENABLE_VEH_SIMULATOR = Config_c.CarMakerSetup.EnableCosimulation;
 	ENABLE_CARLA = Config_c.CarlaSetup.EnableCosimulation;
-	ENABLE_CARLA_EXTERNAL_CONTROL = Config_c.CarlaSetup.EnableExternalControl;
+
 	if (ENABLE_VERBOSE) {
 		//FILE* f = fopen(MasterLogName.c_str(), "a");
 		//fprintf(f, "\n============================================");
@@ -613,6 +277,7 @@ int main(int argc, char* argv[]) {
 			serverPort.push_back(Config_c.SimulationSetup.TrafficSimulatorPort);
 			if (ENABLE_VISSIM) {
 				serverNames.push_back("vissimDriver");
+				printf("VISSIM driver model dll selected as server \n");
 			}
 		}
 		if (Config_c.XilSetup.DetectorSubscription.size() > 0) {
@@ -620,6 +285,7 @@ int main(int argc, char* argv[]) {
 			serverPort.push_back(Config_c.SimulationSetup.TrafficSimulatorPort+1);
 			if (ENABLE_VISSIM) {
 				serverNames.push_back("vissimSignal");
+				printf("VISSIM signal dll selected as server \n");
 			}
 		}
 	}
@@ -629,6 +295,7 @@ int main(int argc, char* argv[]) {
 			serverPort.push_back(Config_c.SimulationSetup.TrafficSimulatorPort);
 			if (ENABLE_VISSIM) {
 				serverNames.push_back("vissimDriver");
+				printf("VISSIM driver model dll selected as server \n");
 			}
 		}
 		if (Config_c.ApplicationSetup.DetectorSubscription.size() > 0) {
@@ -636,6 +303,7 @@ int main(int argc, char* argv[]) {
 			serverPort.push_back(Config_c.SimulationSetup.TrafficSimulatorPort+1);
 			if (ENABLE_VISSIM) {
 				serverNames.push_back("vissimSignal");
+				printf("VISSIM signal dll selected as server \n");
 			}
 		}
 	}
@@ -657,6 +325,7 @@ int main(int argc, char* argv[]) {
 				port_v = get<3>(Config_c.XilSetup.VehicleSubscription[i]);
 				for (int iP = 0; iP < port_v.size(); iP++) {
 					selfServerPortAll.push_back(port_v[iP]);
+					printf("\nTrafficLayer broadcast to client at port %d \n", port_v[iP]);
 				}
 			}
 			for (int i = 0; i < Config_c.XilSetup.SignalSubscription.size(); i++) {
@@ -680,6 +349,7 @@ int main(int argc, char* argv[]) {
 				port_v = get<3>(Config_c.ApplicationSetup.VehicleSubscription[i]);
 				for (int iP = 0; iP < port_v.size(); iP++) {
 					selfServerPortAll.push_back(port_v[iP]);
+					printf("\nTrafficLayer broadcast to client at port %d \n", port_v[iP]);
 				}
 			}
 			for (int i = 0; i < Config_c.ApplicationSetup.SignalSubscription.size(); i++) {
@@ -703,7 +373,6 @@ int main(int argc, char* argv[]) {
 	// number of ports equal to size of Sock_c.clientSock
 	// map each port to an element of Sock_c.clientSock
 	selfServerPortUserInput = selfServerPortAll;
-	sort(selfServerPortUserInput.begin(), selfServerPortUserInput.end());
 	auto it = unique(selfServerPortUserInput.begin(), selfServerPortUserInput.end());
 	selfServerPortUserInput.resize(distance(selfServerPortUserInput.begin(), it));
 	for (int i = 0; i < selfServerPortUserInput.size(); i++) {
@@ -717,21 +386,11 @@ int main(int argc, char* argv[]) {
 
 	for (int i = 0; i < selfServerPortUserInput.size(); i++) {
 		if (selfServerPortUserInput[i] == Config_c.SimulationSetup.TrafficSimulatorPort || selfServerPortUserInput[i] == Config_c.SimulationSetup.TrafficSimulatorPort+1) {
-			printf("ERROR: %d and %d are reserved ports, please select other ports for Application Layer!\n", Config_c.SimulationSetup.TrafficSimulatorPort, Config_c.SimulationSetup.TrafficSimulatorPort+1);
+			printf("ERROR: %d and %d are reserved port, please select other ports for Application Layer!!\n\n", Config_c.SimulationSetup.TrafficSimulatorPort, Config_c.SimulationSetup.TrafficSimulatorPort+1);
 			exit(-1);
 		}
 	}
 
-	// Print client ports summary
-	if (selfServerPortUserInput.size() > 0) {
-		printf("Client ports: ");
-		for (int i = 0; i < selfServerPortUserInput.size(); i++) {
-			if (i > 0) printf(", ");
-			printf("%d", selfServerPortUserInput[i]);
-		}
-		printf("\n");
-	}
-
 	double simTime = 0;
 	int ii = 0;
 
@@ -836,10 +495,6 @@ int main(int argc, char* argv[]) {
 	}
 
 
-	// pass configuration to Traffic_c
-	Traffic_c.Config_c = &Config_c;
-	Traffic_c.getConfig();
-
 	/********************************************
 	* Traffic Simulator send and recv setups
 	*********************************************/
@@ -851,8 +506,6 @@ int main(int argc, char* argv[]) {
 		else {
 			string trafficIp = Config_c.SimulationSetup.TrafficSimulatorIP;
 			int trafficPort = Config_c.SimulationSetup.TrafficSimulatorPort;
-
-
 			Traffic_c.connectionSetup(trafficIp, trafficPort, Sock_c.NCLIENT, Config_c.SumoSetup.ExecutionOrder);
 			Traffic_c.selectSUMO();
 		}
@@ -867,11 +520,15 @@ int main(int argc, char* argv[]) {
 		exit(-1);
 	}
 
+
+	// pass configuration to Traffic_c
+	Traffic_c.Config_c = &Config_c;
+	Traffic_c.getConfig();
+
 	Traffic_c.MasterLogName = MasterLogName;
 
 	Traffic_c.ENABLE_VEH_SIMULATOR = ENABLE_VEH_SIMULATOR;
 	Traffic_c.ENABLE_CARLA = ENABLE_CARLA;
-	Traffic_c.ENABLE_CARLA_EXTERNAL_CONTROL = ENABLE_CARLA_EXTERNAL_CONTROL;
 	/********************************************
 	* Message Setups
 	*********************************************/
@@ -889,64 +546,23 @@ int main(int argc, char* argv[]) {
 	//*********************************************/
 	//Sock_c.initConnection();
 
-	// Initialize shutdown state for graceful cleanup
-	g_shutdown.traffic = &Traffic_c;
-	g_shutdown.socket = &Sock_c;
-	g_shutdown.msgClient = &MsgClient_c;
-	g_shutdown.clientSockets = &actualClientSock;
-	g_shutdown.simTime = &simTime;
-	g_shutdown.enableVissim = ENABLE_VISSIM;
-	g_shutdown.enableClient = ENABLE_CLIENT;
-	g_shutdown.enableRealSim = ENABLE_REALSIM;
-	g_shutdown.initialized = true;
-
-	PERF_INIT("TrafficLayerPerf.log");
-
 	bool isVeryFirstStep = true; 
 
 	bool isEgoExist = false;
 	bool isInitialTimeFinished = false;
 
+	bool egoInsertedByTrafficLayer = false;
+	double EgoInsertTime = Config_c.SumoSetup.EgoInsertTime;
 	//while (simTime <= tSimuEnd && ii < nT) {
-	while (!g_shutdown.shutdownRequested) {
-
-		PERF_TIC("main_loop");
+	while (1) {
 
 		///****************************************************
 		// RUN one-step simulation
 		///****************************************************
+		
+
+		Traffic_c.runOneStepSimulation();
 
-		PERF_TIC("traffic_step");
-		if (ENABLE_REALSIM && !g_shutdown.trafficSimulatorClosed) {
-			try {
-				Traffic_c.runOneStepSimulation();
-			}
-			catch (const std::exception& e) {
-				printf("ERROR: Traffic simulator step failed: %s\n", e.what());
-				printf("\tTraffic simulator may have been closed\n");
-				printf("\tInitiating graceful shutdown...\n");
-				g_shutdown.trafficSimulatorClosed = true;
-				g_shutdown.shutdownRequested = true;
-				PERF_TOC("traffic_step");
-				PERF_TOC("main_loop");
-				break;
-			}
-			catch (...) {
-				printf("ERROR: Traffic simulator step failed (unknown error)\n");
-				printf("\tTraffic simulator may have been closed\n");
-				printf("\tInitiating graceful shutdown...\n");
-				g_shutdown.trafficSimulatorClosed = true;
-				g_shutdown.shutdownRequested = true;
-				PERF_TOC("traffic_step");
-				PERF_TOC("main_loop");
-				break;
-			}
-		}
-		PERF_TOC("traffic_step");
-#ifdef ENABLE_PERF_TIMING
-		// Log number of vehicles received from traffic simulator
-		PERF_LOG("t=%.2f vehicles_in_network=%d\n", simTime, (int)MsgServer_c.VehDataRecv_um.size());
-#endif
 
 		if (ENABLE_VEH_SIMULATOR && isVeryFirstStep) {
 			if (Sock_c.initConnection(TrafficLayerErrorFile) > 0) {
@@ -973,13 +589,36 @@ int main(int argc, char* argv[]) {
 			printf("===========SimTime %f==============\n", simTime);
 		}
 
-		// run sumo unitial initial time finished
-		if ((Config_c.SimulationSetup.SimulationMode == 4 || Config_c.SimulationSetup.SimulationMode == 5) && !isInitialTimeFinished) {
-			Traffic_c.runSimulation(Config_c.SimulationSetup.SimulationModeParameter);
-			isInitialTimeFinished = true;
+
+		if (!egoInsertedByTrafficLayer && simTime >= EgoInsertTime) {
+			int ret = Traffic_c.addEgoVehicle(simTime);
+			egoInsertedByTrafficLayer = true;
+			printf("[main] tried to insert ego at simTime = %.3f, ret = %d\n", simTime, ret);
+			printf("Ego inserted at time %f\n", simTime);
+			fflush(stdout);
 		}
 
-		if ((Config_c.SimulationSetup.SimulationMode == 1 || Config_c.SimulationSetup.SimulationMode == 2) && !isEgoExist) {
+
+		
+
+		// run sumo unitial initial time finished
+		//if ((Config_c.SimulationSetup.SimulationMode == 4 || Config_c.SimulationSetup.SimulationMode == 5) && !isInitialTimeFinished) {
+			//Traffic_c.runSimulation(Config_c.SimulationSetup.SimulationModeParameter);
+			//isInitialTimeFinished = true;
+		//}
+
+		// if ((Config_c.SimulationSetup.SimulationMode == 1 || Config_c.SimulationSetup.SimulationMode == 2) && !isEgoExist) {
+		/*if (!isEgoExist) {
+
+			if (!egoInsertedByTrafficLayer && simTime >= 28985.0) {
+				Traffic_c.addEgoVehicle(simTime);
+				egoInsertedByTrafficLayer = true;
+
+				if (!ENABLE_VERBOSE) {
+					printf("[TrafficLayer] Insert ego at t=%.1f s\n", simTime);
+				}
+			}
+
 			if (Traffic_c.checkIfEgoExist(&simTime)) {
 				// continue the code to sync VISSIM/SUMO with clients
 				isEgoExist = true;
@@ -987,7 +626,7 @@ int main(int argc, char* argv[]) {
 				// otherwise just continue running the simulation
 				continue;
 			}
-		}
+		}*/
 
 		// if veryFirstStep and vehicle simulator, add the ego vehicle
 		if (isVeryFirstStep && ENABLE_VEH_SIMULATOR) {
@@ -1001,28 +640,28 @@ int main(int argc, char* argv[]) {
 		try {
 			MsgServer_c.clearRecvStorage();
 
-			if (ENABLE_REALSIM && !g_shutdown.trafficSimulatorClosed) {
+			if (ENABLE_REALSIM) {
 				if (Traffic_c.recvFromTrafficSimulator(&simTime, MsgServer_c) < 0) {
 					if (WSAGetLastError() != WSAEINTR) {
 
 						printf("WARNING: receive from traffic simulator fails\n");
 
 						printf("\tVISSIM or SUMO may already be closed\n");
-						printf("\tInitiating graceful shutdown...\n");
+						printf("\texit......\n");
 					}
-					g_shutdown.trafficSimulatorClosed = true; // Mark that simulator is already closed
-					g_shutdown.shutdownRequested = true;
-					break;
+					Sock_c.socketShutdown();
+					exit(-1);
 				};
 
 				// end simulation if simulation time is greater than simulation end time setup
 				if (simTime > Config_c.SimulationSetup.SimulationEndTime) {
-					printf("Simulation end time reached.\n");
-					g_shutdown.shutdownRequested = true;
-					break;
+					exit(1);
 				}
 			}
 
+			Traffic_c.fillTrafficLightStates(MsgServer_c);
+			Traffic_c.updateEgoLoopRoute();
+
 			if (ENABLE_VERBOSE) {
 				for (auto& it : MsgServer_c.VehDataRecv_um) {
 					MsgServer_c.printVehData(it.second);
@@ -1032,14 +671,14 @@ int main(int argc, char* argv[]) {
 			}
 		}
 		catch (const std::exception& e) {
-			printf("ERROR: Exception in traffic simulator receive: %s\n", e.what());
-			g_shutdown.shutdownRequested = true;
-			break;
+			Sock_c.socketShutdown();
+			std::cout << e.what();
+			exit(-1);
 		}
 		catch (...) {
+			Sock_c.socketShutdown();
 			printf("UNKNOWN ERROR: receive from traffic simulator fails\n");
-			g_shutdown.shutdownRequested = true;
-			break;
+			exit(-1);
 		}
 
 		///****************************************************
@@ -1064,7 +703,6 @@ int main(int argc, char* argv[]) {
 		}
 
 		if (ENABLE_CLIENT) {
-			PERF_TIC("prepare_send");
 			try {
 				MsgClient_c.clearSendStorage();
 
@@ -1092,7 +730,20 @@ int main(int argc, char* argv[]) {
 							}
 						}
 						else {
-							MsgClient_c.VehDataSend_um[actualClientSock[iC]].push_back(it.second);
+							const std::string& vehId = it.second.id;
+							const int SIMULINK_PORT = 420;
+							const int PYTHON_PORT = 430;
+
+
+							if (curPort == SIMULINK_PORT) {
+								if (vehId.compare(Config_c.SumoSetup.EgoID) == 0) {
+									MsgClient_c.VehDataSend_um[actualClientSock[iC]].push_back(it.second);
+								}
+							}
+							
+							else {
+								MsgClient_c.VehDataSend_um[actualClientSock[iC]].push_back(it.second);
+							}
 						}
 					}
 					for (auto it : MsgServer_c.TlsDataRecv_um) {
@@ -1139,35 +790,19 @@ int main(int argc, char* argv[]) {
 					float simTimeSend = simTime;
 					uint8_t simStateSend = 1;
 
-					PERF_TOC("prepare_send");
-					PERF_TIC("send_data");
-#ifdef ENABLE_PERF_TIMING
-					// Check send buffer status before send
-					int sndBufSize = 0;
-					int optLen = sizeof(sndBufSize);
-					getsockopt(actualClientSock[iC], SOL_SOCKET, SO_SNDBUF, (char*)&sndBufSize, &optLen);
-
-					// Calculate message size
-					int msgSize = MsgClient_c.VehDataSend_um[actualClientSock[iC]].size();
-					PERF_LOG("t=%.2f client=%d/%d sndBufSize=%d nVeh=%d nTls=%d nDet=%d\n",
-						simTime, iC, (int)actualClientSock.size(), sndBufSize, msgSize,
-						(int)MsgClient_c.TlsDataSend_um[actualClientSock[iC]].size(),
-						(int)MsgClient_c.DetDataSend_um[actualClientSock[iC]].size());
-#endif
-
 					if (ENABLE_EXT_DYN) {
 						if (isVeryFirstStep) {
 							if (Sock_c.sendData(actualClientSock[iC], iC, simTimeSend, simStateSend, MsgClient_c) < 0) {
 								printf("ERROR: send to client fails\n");
-								g_shutdown.shutdownRequested = true;
-								break;
+								Sock_c.socketShutdown();
+								exit(-1);
 							};
 						}
 						else {
 							if (Sock_c.sendData(actualClientSock[iC], iC, simTimeSend, simStateSend, MsgClient_c) < 0) {
 								printf("ERROR: send to client fails\n");
-								g_shutdown.shutdownRequested = true;
-								break;
+								Sock_c.socketShutdown();
+								exit(-1);
 							};
 						}
 					}
@@ -1178,21 +813,19 @@ int main(int argc, char* argv[]) {
 						//else {
 							if (Sock_c.sendData(actualClientSock[iC], iC, simTimeSend, simStateSend, MsgClient_c) < 0) {
 								printf("ERROR: send to client fails\n");
-								g_shutdown.shutdownRequested = true;
-								break;
+								Sock_c.socketShutdown();
+								exit(-1);
 							};
 						//}
 					}
 					else {
 						if (Sock_c.sendData(actualClientSock[iC], iC, simTimeSend, simStateSend, MsgClient_c) < 0) {
 							printf("ERROR: send to client fails\n");
-							g_shutdown.shutdownRequested = true;
-							break;
+							Sock_c.socketShutdown();
+							exit(-1);
 						};
 					}
 
-					PERF_TOC("send_data");
-
 					if (ENABLE_VERBOSE) {
 						printf("send complete\n");
 
@@ -1204,22 +837,17 @@ int main(int argc, char* argv[]) {
 				}
 			}
 			catch (const std::exception& e) {
-				printf("ERROR: Exception in send to client: %s\n", e.what());
-				g_shutdown.shutdownRequested = true;
-				break;
+				Sock_c.socketShutdown();
+				std::cout << e.what();
+				exit(-1);
 			}
 			catch (...) {
+				Sock_c.socketShutdown();
 				printf("UNKNOWN ERROR: send to client fails\n");
-				g_shutdown.shutdownRequested = true;
-				break;
+				exit(-1);
 			}
 		}
 
-		// Check if shutdown was requested during client send
-		if (g_shutdown.shutdownRequested) {
-			break;
-		}
-
 		///****************************************************
 		// Traffic Layer <<<<<===== Clients (Controller, Vehicle Simulator, Models)
 		// RECV message from client (CAV CONTROLLER or VEHICLE)
@@ -1245,10 +873,16 @@ int main(int argc, char* argv[]) {
 					// save received message into Msg_c recv storages
 					if (Sock_c.recvData(actualClientSock[iC], &simStateRecv, &simTimeRecv, MsgClient_c) < 0) {
 						if (WSAGetLastError() != WSAEINTR && WSAGetLastError() != WSAEFAULT) {
-							printf("ERROR: receive from client fails\n");
+							//printf("ERROR: receive from client fails\n");
+							int err = WSAGetLastError();
+							printf("ERROR: receive from client fails, WSA=%d\n", err);
+
 						}
-						g_shutdown.shutdownRequested = true;
-						break;
+						printf("Paused. Press any key...\n");
+						fflush(stdout);
+						_getch();
+						Sock_c.socketShutdown();
+						exit(-1);
 					};
 
 					if (ENABLE_VERBOSE) {
@@ -1305,25 +939,20 @@ int main(int argc, char* argv[]) {
 				}
 			}
 			catch (const std::exception& e) {
-				printf("ERROR: Exception in receive from client: %s\n", e.what());
-				g_shutdown.shutdownRequested = true;
-				break;
+				Sock_c.socketShutdown();
+				std::cout << e.what();
+				exit(-1);
 			}
 			catch (...) {
+				Sock_c.socketShutdown();
 				printf("UNKNOWN ERROR: receive from client fails\n");
-				g_shutdown.shutdownRequested = true;
-				break;
+				exit(-1);
 			}
 		}
 		else {
 			//Sleep(100);
 		}
 
-		// Check if shutdown was requested during client recv
-		if (g_shutdown.shutdownRequested) {
-			break;
-		}
-
 		if (ENABLE_TIMING) {
 			QueryPerformanceCounter(&StartingTime);
 		}
@@ -1342,12 +971,11 @@ int main(int argc, char* argv[]) {
 				Traffic_c.parseSendMsg(MsgServer_c, MsgServer_c);
 			}
 
-			if (ENABLE_REALSIM && !g_shutdown.trafficSimulatorClosed) {
+			if (ENABLE_REALSIM) {
 				if (Traffic_c.sendToTrafficSimulator(simTime, MsgServer_c) < 0) {
 					printf("ERROR: send to traffic simulator fails\n");
-					g_shutdown.trafficSimulatorClosed = true;
-					g_shutdown.shutdownRequested = true;
-					break;
+					Sock_c.socketShutdown();
+					exit(-1);
 				};
 			}
 
@@ -1364,14 +992,14 @@ int main(int argc, char* argv[]) {
 			}
 		}
 		catch (const std::exception& e) {
-			printf("ERROR: Exception in send to traffic simulator: %s\n", e.what());
-			g_shutdown.shutdownRequested = true;
-			break;
+			Sock_c.socketShutdown();
+			std::cout << e.what();
+			exit(-1);
 		}
 		catch (...) {
+			Sock_c.socketShutdown();
 			printf("UNKNOWN ERROR: send to traffic simulator fails\n");
-			g_shutdown.shutdownRequested = true;
-			break;
+			exit(-1);
 		}
 
 		///****************************************************
@@ -1383,14 +1011,8 @@ int main(int argc, char* argv[]) {
 
 		ii = ii + 1;
 
-		PERF_TOC("main_loop");
-
 	}
 
-	PERF_SHUTDOWN();
+	Traffic_c.close();
 
-	// Perform graceful cleanup after main loop exits
-	performCleanup(false);
-
-	return 0;
-}
+}
\ No newline at end of file
diff --git a/VirCarlaEnv/VirCarlaEnv/mainVirCarla.cpp b/VirCarlaEnv/VirCarlaEnv/mainVirCarla.cpp
index 696a335f..49158f43 100644
--- a/VirCarlaEnv/VirCarlaEnv/mainVirCarla.cpp
+++ b/VirCarlaEnv/VirCarlaEnv/mainVirCarla.cpp
@@ -36,6 +36,7 @@
 #include "DebugHelper.h"
 
 //preset delay durations
+carla::time_duration timeout_10s = std::chrono::seconds(10);
 carla::time_duration timeout_1s = std::chrono::seconds(1);
 carla::time_duration timeout_50ms = std::chrono::milliseconds(50);
 carla::time_duration timeout_100ms = std::chrono::duration<double>(0.1);  // 0.1 second
@@ -46,7 +47,7 @@ using namespace std::string_literals;
 #define EXPECT_TRUE(pred) if (!(pred)) { throw std::runtime_error(#pred); }
 #define SET_CONTAINS_ID(set, value) ((set).find(value) != (set).end())
 #define MAP_CONTAINS_KEY(map, key) ((map).find(key) != (map).end())
-#define SPAWN_OFFSET_Z 0.5f // Offset for spawning actors above the ground
+#define SPAWN_OFFSET_Z 0.1f // Offset for spawning actors above the ground
 
 static void show_usage(std::string name)
 {
@@ -146,7 +147,9 @@ int main(int argc, const char* argv[]) {
     bool enableExternalControl = carlaSetup.EnableExternalControl;
     // centeredViewId is the id of the camera that is used to render the view in Carla      
     std::string centeredViewId = configHelper.CarlaSetup.CenteredViewId;
-
+    // Enable traffic light synchronization
+    /*bool enableTlsSync = configHelper.CarlaSetup.EnableTrafficLightSync;*/
+    bool enableTlsSync = true;
     std::unordered_set<std::string> setInterestedIds;
     for (const std::string& id : carlaSetup.InterestedIds) {
         setInterestedIds.insert(id);
@@ -181,7 +184,7 @@ int main(int argc, const char* argv[]) {
 		// initialize carla client and world
         std::cout << "Client API version : " << carlaClient.GetClientVersion() << '\n';
         std::cout << "Server API version : " << carlaClient.GetServerVersion() << '\n';
-		
+        carlaClient.SetTimeout(timeout_10s);
 		carla::client::World carlaWorld = carlaClient.GetWorld();
         carla::SharedPtr<carla::client::Actor> carlaSpectator = carlaWorld.GetSpectator();
 
@@ -206,37 +209,38 @@ int main(int argc, const char* argv[]) {
 		// command batch to set the transform of the actors (non-interested actors)
         std::vector<carla::rpc::Command> transformCommandBatch;
 
-        
+        if (enableTlsSync) {
+            carla::SharedPtr <carla::client::ActorList> carlaTrafficLightActors = carlaWorld.GetActors()->Filter("traffic.traffic_light");
+
+            for (const carla::SharedPtr<carla::client::Actor>& carlaActor : *carlaTrafficLightActors) {
+                carla::SharedPtr<carla::client::TrafficLight> carlaTrafficLightActorPtr = boost::static_pointer_cast<carla::client::TrafficLight>(carlaActor);
+                carlaTrafficLightActorPtr->Freeze(true);
+                std::string carlaTrafficLightActorId = std::to_string(carlaActor->GetId());
+                // Convert Carla location to SUMO coordinate
+                carla::geom::Location carlaLocation = carlaActor->GetLocation();
+                carla::geom::Location sumoLocation = BridgeHelper::map_location_Carla_to_Sumo(carlaLocation);
+
+                // Find closest TLS from map
+                std::pair<std::string, int> trafficLightId = BridgeHelper::find_closest_trafficLight_id(
+                    trafficLightMap,
+                    sumoLocation.x,
+                    sumoLocation.y
+                );
+                std::string junctionId = trafficLightId.first;
+                int linkId = trafficLightId.second;
+
+                if (junctionId == "") {
+                    std::cerr << "No matching traffic light found for actor " << carlaTrafficLightActorId << "\n";
+                    continue;
+                }
 
-        carla::SharedPtr <carla::client::ActorList> carlaTrafficLightActors = carlaWorld.GetActors()->Filter("traffic.traffic_light");
-
-        for (const carla::SharedPtr<carla::client::Actor>& carlaActor : *carlaTrafficLightActors) {
-            carla::SharedPtr<carla::client::TrafficLight> carlaTrafficLightActorPtr = boost::static_pointer_cast<carla::client::TrafficLight>(carlaActor);
-            carlaTrafficLightActorPtr->Freeze(true);
-            std::string carlaTrafficLightActorId = std::to_string(carlaActor->GetId());
-            // Convert Carla location to SUMO coordinate
-            carla::geom::Location carlaLocation = carlaActor->GetLocation();
-            carla::geom::Location sumoLocation = BridgeHelper::map_location_Carla_to_Sumo(carlaLocation);
-
-            // Find closest TLS from map
-            std::pair<std::string, int> trafficLightId = BridgeHelper::find_closest_trafficLight_id(
-                trafficLightMap,
-                sumoLocation.x,
-                sumoLocation.y
-            );
-            std::string junctionId = trafficLightId.first;
-            int linkId = trafficLightId.second;
-
-            if (junctionId == "") {
-                std::cerr << "No matching traffic light found for actor " << carlaTrafficLightActorId << "\n";
-                continue;
+                // Assign traffic light attributes
+                TrafficLight& trafficLight = trafficLightMap[junctionId][linkId];
+                trafficLight.carlaTrafficLightActorId = carlaTrafficLightActorId; // Store the Carla actor ID in the traffic light data
+                trafficLight.carlaTrafficLightActorPtr = carlaTrafficLightActorPtr; // Store the Carla actor pointer in the traffic light data
             }
-
-            // Assign traffic light attributes
-            TrafficLight& trafficLight = trafficLightMap[junctionId][linkId];
-            trafficLight.carlaTrafficLightActorId = carlaTrafficLightActorId; // Store the Carla actor ID in the traffic light data
-            trafficLight.carlaTrafficLightActorPtr = carlaTrafficLightActorPtr; // Store the Carla actor pointer in the traffic light data
         }
+        
         while (simTime < simEndTime) {
             std::chrono::steady_clock::time_point loop_start_time = std::chrono::steady_clock::now();
             ///***********************
@@ -278,8 +282,10 @@ int main(int argc, const char* argv[]) {
                 const VehFullData_t& tmpVehData = pair.second;
 				carla::geom::Location tmpLocation(tmpVehData.positionX, tmpVehData.positionY, tmpVehData.positionZ);
 				// The grade received from FIXS is in radians, convert to degrees
-                carla::geom::Rotation tmpRotation(tmpVehData.grade * 180/M_PI, tmpVehData.heading, 0.0f);
-                carla::geom::Vector3D tmpExtent(tmpVehData.length / 2, tmpVehData.width / 2, tmpVehData.height / 2);
+                //carla::geom::Rotation tmpRotation(tmpVehData.grade * 180/M_PI, tmpVehData.heading, 0.0f);
+                carla::geom::Rotation tmpRotation(0.0f, tmpVehData.heading, 0.0f);
+                //carla::geom::Vector3D tmpExtent(tmpVehData.length / 2, tmpVehData.width / 2, tmpVehData.height / 2);
+                carla::geom::Vector3D tmpExtent(4.8f, 2.0f, 1.8f); // Default extent, can be modified later
                 carla::geom::Transform tmpTransform(tmpLocation, tmpRotation);
 
                 if (!MAP_CONTAINS_KEY(mapSumoActor, tmpVehData.id)) {
@@ -293,33 +299,35 @@ int main(int argc, const char* argv[]) {
 				}
 				setCurrentSumoIds.insert(tmpVehData.id);
             }
-
-            for (const std::pair<const std::string, TrafficLightData_t>& pair : msgHelper.TlsDataRecv_um) {
-                const std::string& junctionID = pair.first;
-                const TrafficLightData_t& tmpTrafficLightData = pair.second;
-                const std::string& tafficLightStateStr = tmpTrafficLightData.state;
-                if (enableVerboseLog) {
-                    std::cout << "Received Traffic Light Data for junction: " << junctionID << std::endl;
-                    std::cout << "ID: " << tmpTrafficLightData.id << std::endl;
-                    std::cout << "Name: " << tmpTrafficLightData.name << std::endl;
-                    std::cout << "State: " << tmpTrafficLightData.state << std::endl;
-                }
-                // traverse each char in the state str
-                for (size_t linkId = 0; linkId < tafficLightStateStr.size(); ++linkId) {
-                    char stateChar = tafficLightStateStr[linkId];
-                    SumoTrafficLightState sumoTrafficLightState = BridgeHelper::get_Sumo_traffic_light_state_from_char(stateChar);
-                    carla::rpc::TrafficLightState carlaTrafficLightState = BridgeHelper::map_Sumo_traffic_light_state_to_Carla(sumoTrafficLightState);
-                    // set the traffic light state in Carla, if the traffic light exists in the map
-                    TrafficLight& trafficLight = trafficLightMap[junctionID][linkId];
-                    
-                    if (trafficLight.carlaTrafficLightActorPtr) {
-                        trafficLight.carlaTrafficLightActorPtr->SetState(carlaTrafficLightState);
+            if (enableTlsSync) {
+                for (const std::pair<const std::string, TrafficLightData_t>& pair : msgHelper.TlsDataRecv_um) {
+                    const std::string& junctionID = pair.first;
+                    const TrafficLightData_t& tmpTrafficLightData = pair.second;
+                    const std::string& tafficLightStateStr = tmpTrafficLightData.state;
+                    if (enableVerboseLog) {
+                        std::cout << "Received Traffic Light Data for junction: " << junctionID << std::endl;
+                        std::cout << "ID: " << tmpTrafficLightData.id << std::endl;
+                        std::cout << "Name: " << tmpTrafficLightData.name << std::endl;
+                        std::cout << "State: " << tmpTrafficLightData.state << std::endl;
                     }
-                    else {
-                        std::cerr << "Carla actor for traffic light " << tmpTrafficLightData.id << "Junction Id:" << junctionID << "Link Id: "<< linkId << " not found." << std::endl;
+                    // traverse each char in the state str
+                    for (size_t linkId = 0; linkId < tafficLightStateStr.size(); ++linkId) {
+                        char stateChar = tafficLightStateStr[linkId];
+                        SumoTrafficLightState sumoTrafficLightState = BridgeHelper::get_Sumo_traffic_light_state_from_char(stateChar);
+                        carla::rpc::TrafficLightState carlaTrafficLightState = BridgeHelper::map_Sumo_traffic_light_state_to_Carla(sumoTrafficLightState);
+                        // set the traffic light state in Carla, if the traffic light exists in the map
+                        TrafficLight& trafficLight = trafficLightMap[junctionID][linkId];
+
+                        if (trafficLight.carlaTrafficLightActorPtr) {
+                            trafficLight.carlaTrafficLightActorPtr->SetState(carlaTrafficLightState);
+                        }
+                        else {
+                            if (enableVerboseLog) std::cerr << "Carla actor for traffic light " << tmpTrafficLightData.id << "Junction Id:" << junctionID << "Link Id: " << linkId << " not found." << std::endl;
+                        }
                     }
                 }
             }
+            
 			// Check if the mapSumoActor contains vehicles that are not in the current step (vehicles have left the simulation)
 			// If so, remove them from the mapSumoActor and mapCarlaActor
             for (std::unordered_map<std::string, SumoActor>::iterator it = mapSumoActor.begin(); it != mapSumoActor.end(); ) {
@@ -377,9 +385,6 @@ int main(int argc, const char* argv[]) {
                 // This is to ensure that the carla transform is on the road
                 // =======================================================
                 //carlaTransform.location = waypointTransform.location;
-				
-			
-                
                 //Use the waypoint's rotation to ensure the vehicle is aligned with the road
 				//carlaTransform.rotation = waypointTransform.rotation; 
                 //print the sumo and carla transform
@@ -390,7 +395,7 @@ int main(int argc, const char* argv[]) {
                 // =======================================================
                 if (!MAP_CONTAINS_KEY(mapSumoToCarla , sumoActorId) || !sumoActor.spawnedInCarla || sumoActor.carlaVehicleActorPtr ==nullptr) {
                     // Add a small offset to the z coordinate to avoid collision with the ground
-                    carlaTransform.location.z = SPAWN_OFFSET_Z;
+                    carlaTransform.location.z = carlaTransform.location.z + SPAWN_OFFSET_Z;
 					
 
                     std::string carlaActorTypeId;
@@ -423,47 +428,46 @@ int main(int argc, const char* argv[]) {
                         if (enableVerboseLog) std::cout << "Found Intertested Vehicle with Carla Type ID: " << carlaActorTypeId << " SUMO ID:" << sumoActorId << std::endl;
                     }
                     else {
-                        carlaVehicleActorPtr = boost::static_pointer_cast<carla::client::Vehicle>(carlaWorld.TrySpawnActor(vehicle_blueprint_local, carlaTransform));
                         if (enableVerboseLog) std::cout << "Spawning actor with Carla Type ID: " << carlaActorTypeId << " SUMO ID:" << sumoActorId << std::endl;
-                        // set the simulate physics to false, so that the actor does not fall down
-                        //carlaActor->SetSimulatePhysics(false);
-                    }
-                    
-                     
-					
-                    if (carlaVehicleActorPtr != nullptr) {
-						sumoActor.spawnedInCarla = true; // Mark the Sumo actor as spawned in Carla
-						sumoActor.carlaVehicleActorPtr = carlaVehicleActorPtr; // Store the Carla actor in the SumoActor
-
-                        if (enableVerboseLog) std::cout << "Spawned actor with Carla ID: " << carlaVehicleActorPtr->GetId() << " SUMO ID:" << sumoActorId << std::endl;
-                        if (enableVerboseLog && sumoActorId == "ego") {
-                            std::cout << "Sumo Transform:" << std::endl;
-                            std::cout << "  Location -> x: " << sumoActor.sumoTransform.location.x
-                                << ", y: " << sumoActor.sumoTransform.location.y
-                                << ", z: " << sumoActor.sumoTransform.location.z << std::endl;
-
-                            std::cout << "  Rotation -> pitch: " << sumoActor.sumoTransform.rotation.pitch
-                                << ", yaw: " << sumoActor.sumoTransform.rotation.yaw
-                                << ", roll: " << sumoActor.sumoTransform.rotation.roll << std::endl;
-
-                            std::cout << "Carla Transform:" << std::endl;
-                            std::cout << "  Location -> x: " << carlaTransform.location.x
-                                << ", y: " << carlaTransform.location.y
-                                << ", z: " << carlaTransform.location.z << std::endl;
-
-                            std::cout << "  Rotation -> pitch: " << carlaTransform.rotation.pitch
-                                << ", yaw: " << carlaTransform.rotation.yaw
-                                << ", roll: " << carlaTransform.rotation.roll << std::endl;
+
+                        carlaVehicleActorPtr = boost::static_pointer_cast<carla::client::Vehicle>(carlaWorld.TrySpawnActor(vehicle_blueprint_local, carlaTransform));
+                        
+                        if (carlaVehicleActorPtr != nullptr) {
+                            
+                            carlaVehicleActorPtr->SetSimulatePhysics(false);
+                            sumoActor.spawnedInCarla = true; // Mark the Sumo actor as spawned in Carla
+                            sumoActor.carlaVehicleActorPtr = carlaVehicleActorPtr; // Store the Carla actor in the SumoActor
+
+                            if (enableVerboseLog) std::cout << "Spawned actor with Carla ID: " << carlaVehicleActorPtr->GetId() << " SUMO ID:" << sumoActorId << std::endl;
+                            if (enableVerboseLog && sumoActorId == "ego") {
+                                std::cout << "Sumo Transform:" << std::endl;
+                                std::cout << "  Location -> x: " << sumoActor.sumoTransform.location.x
+                                    << ", y: " << sumoActor.sumoTransform.location.y
+                                    << ", z: " << sumoActor.sumoTransform.location.z << std::endl;
+
+                                std::cout << "  Rotation -> pitch: " << sumoActor.sumoTransform.rotation.pitch
+                                    << ", yaw: " << sumoActor.sumoTransform.rotation.yaw
+                                    << ", roll: " << sumoActor.sumoTransform.rotation.roll << std::endl;
+
+                                std::cout << "Carla Transform:" << std::endl;
+                                std::cout << "  Location -> x: " << carlaTransform.location.x
+                                    << ", y: " << carlaTransform.location.y
+                                    << ", z: " << carlaTransform.location.z << std::endl;
+
+                                std::cout << "  Rotation -> pitch: " << carlaTransform.rotation.pitch
+                                    << ", yaw: " << carlaTransform.rotation.yaw
+                                    << ", roll: " << carlaTransform.rotation.roll << std::endl;
+                            }
+                            // convert the carla actor id (uint_32 to string)
+                            std::string carlaActorId = std::to_string(carlaVehicleActorPtr->GetId());
+                            mapCarlaToSumo[carlaActorId] = sumoActorId;
+                            mapSumoToCarla[sumoActorId] = carlaActorId;
+                            sumoActor.carlaTransform = carlaTransform; // Store the Carla transform in the SumoActor
                         }
-                        // convert the carla actor id (uint_32 to string)
-                        std::string carlaActorId = std::to_string(carlaVehicleActorPtr->GetId());
-                        mapCarlaToSumo[carlaActorId] = sumoActorId;
-                        mapSumoToCarla[sumoActorId] = carlaActorId;
-                        sumoActor.carlaTransform = carlaTransform; // Store the Carla transform in the SumoActor
-                    }
-                    else {
-                        std::cerr << "Failed to spawn actor. " << sumoActorId << std::endl;
-                        /*return 1;*/
+                        else {
+                            std::cout << "[Warning] Failed to spawn actor with SUMO ID:" << sumoActorId << std::endl;
+                        }
+                        
                     }
                     
                 }
@@ -586,20 +590,24 @@ int main(int argc, const char* argv[]) {
 					    tmpVehData.heading = sumoRotation.yaw;
 					    tmpVehData.grade = sumoRotation.pitch * M_PI / 180.0; // Convert to radians
 					    
-					    tmpVehData.length = carlaExtent.x * 2; // The extent is half the length, so multiply by 2
-					    tmpVehData.width = carlaExtent.y * 2; // The extent is half the width, so multiply by 2
-					    tmpVehData.height = carlaExtent.z * 2; // The extent is half the height, so multiply by 2
+					    //tmpVehData.length = carlaExtent.x * 2; // The extent is half the length, so multiply by 2
+					    //tmpVehData.width = carlaExtent.y * 2; // The extent is half the width, so multiply by 2
+					    //tmpVehData.height = carlaExtent.z * 2; // The extent is half the height, so multiply by 2
 					    msgHelper.VehDataSend_um[socketHelper.serverSock[sockId]].push_back(tmpVehData);
                     }
+                    
                     if (sumoId == centeredViewId) {
+
 						// Set the spectator to follow the centered view actor
 						// Note: The spectator is a special camera that follows the actor
 						if (enableVerboseLog) std::cout << "Setting spectator to follow actor with ID: " << sumoId << std::endl;
+                        static carla::geom::Location smoothedLocation;
                         carla::geom::Location tmpLocation = carlaTransform.location;
-                        tmpLocation.z = 100.0; // Set height to 100 meters above the ground
+                        tmpLocation.z += 100.0f;// Set height to 100 meters above the ground
+                        smoothedLocation = 0.9f * smoothedLocation + 0.1f * tmpLocation;
                         //pitch = -90.0, yaw = 0.0, roll = 0.0
                         carla::geom::Rotation tmpRotation(-90.0f, -90.0f, 0.0f);
-                        carla::geom::Transform tmpTransform(tmpLocation, tmpRotation);
+                        carla::geom::Transform tmpTransform(smoothedLocation, tmpRotation);
                         carlaSpectator->SetTransform(tmpTransform);
                         //drawCircle(carlaWorld.MakeDebugHelper(), tmpLocation, 2.5f, 32, 0.5f, 0.1f, 0.2f);
 					}
@@ -637,12 +645,12 @@ int main(int argc, const char* argv[]) {
             
 			msgHelper.clearRecvStorage();
             msgHelper.clearSendStorage();
-            const std::chrono::steady_clock::time_point loop_end_time = std::chrono::steady_clock::now();
-            std::chrono::milliseconds elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(loop_end_time - loop_start_time);
-            std::chrono::milliseconds sleep_duration = std::chrono::milliseconds(step_duration_ms) - elapsed;
-            if (sleep_duration.count() > 0) {
-                std::this_thread::sleep_for(sleep_duration);
-            }
+            //const std::chrono::steady_clock::time_point loop_end_time = std::chrono::steady_clock::now();
+            //std::chrono::milliseconds elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(loop_end_time - loop_start_time);
+            //std::chrono::milliseconds sleep_duration = std::chrono::milliseconds(step_duration_ms) - elapsed;
+            //if (sleep_duration.count() > 0) {
+            //    std::this_thread::sleep_for(sleep_duration);
+            //}
         }
         
         carlaWorldSettings = carlaWorld.GetSettings();
diff --git a/scripts/download_large_assets.ps1 b/scripts/download_large_assets.ps1
new file mode 100644
index 00000000..0f16323c
--- /dev/null
+++ b/scripts/download_large_assets.ps1
@@ -0,0 +1,49 @@
+#!/usr/bin/env pwsh
+# Download large simulation assets from OneDrive
+
+$ASSETS = @{
+    "tests/Applications/SUMO_CARLA_EcoDriving/MLK_Carla_Scenario/CARLAFiles/MLK_noped1002_final_debug.fbx" = "https://outlookuga-my.sharepoint.com/:u:/g/personal/ys04893_uga_edu/EU8vuZV8DvtOlJ6HhpL7Z8MBkhCtVcloma0qxJXXMsPP7A?e=0KgETK&download=1"
+}
+
+$repoRoot = Split-Path -Parent $PSScriptRoot
+Set-Location $repoRoot
+
+foreach ($file in $ASSETS.Keys) {
+    $dest = Join-Path $repoRoot $file
+
+    if (Test-Path $dest) {
+        $sizeMB = [math]::Round((Get-Item $dest).Length / 1MB, 2)
+        Write-Host "File $file already exists ($sizeMB MB)" -ForegroundColor Yellow
+        $response = Read-Host "Redownload? (y/N)"
+        if ($response -notmatch '^[yY]') {
+            Write-Host "Skipped $file" -ForegroundColor Green
+            continue
+        }
+    }
+
+    Write-Host "Downloading $file..." -ForegroundColor Yellow
+    $destDir = Split-Path -Parent $dest
+    New-Item -ItemType Directory -Path $destDir -Force | Out-Null
+
+    try {
+        $ProgressPreference = 'SilentlyContinue'
+        Invoke-WebRequest -Uri $ASSETS[$file] -OutFile $dest
+        $ProgressPreference = 'Continue'
+
+        $sizeMB = [math]::Round((Get-Item $dest).Length / 1MB, 2)
+        Write-Host "OK Downloaded $file ($sizeMB MB)" -ForegroundColor Green
+    }
+    catch {
+        Write-Host "`nERROR Failed to download $file" -ForegroundColor Red
+        Write-Host $_.Exception.Message -ForegroundColor Red
+        Write-Host "`nTroubleshooting:" -ForegroundColor Yellow
+        Write-Host "1. Make sure you have access to the OneDrive link" -ForegroundColor Yellow
+        Write-Host "2. Check your internet connection" -ForegroundColor Yellow
+        Write-Host "3. Try downloading manually from: $($ASSETS[$file])" -ForegroundColor Yellow
+        Read-Host "`nPress Enter to exit"
+        exit 1
+    }
+}
+
+Write-Host "`nAll assets ready!" -ForegroundColor Green
+Read-Host "`nPress Enter to exit"