Adding requests to diagnostic manager implemented.

Needed overload of operator and constructor set for
active_diagnostic_message_t class. Without we can't make
the basic operation needed by the manager.
Get rid of original raw pointer on "entry" which avoid some
tests.

It miss the acceptance filters implementations for now.

Change-Id: I1ca61ff843c13255af6d9a60ce72a8b8bc9d1c18
Signed-off-by: Romain Forlot <romain.forlot@iot.bzh>

4 files changed, 313 insertions, 40 deletions

diff --git a/src/diagnostic/active-diagnostic-request.cpp b/src/diagnostic/active-diagnostic-request.cpp
index 251fddbb..430e4f68 100644
--- a/src/diagnostic/active-diagnostic-request.cpp
+++ b/src/diagnostic/active-diagnostic-request.cpp
@@ -17,35 +17,88 @@
 
 #include "active-diagnostic-request.hpp"
 
+bool& operator==(const active_diagnostic_request_t& adr) const
+{
+	return (bus_ == adr.bus_ && id_ == adr.id_ && handle_ == adr.handle_) ? true : false;
+}
+
+active_diagnostic_request_t& operator=(const active_diagnostic_request_t& adr)
+	: can_bus_dev_{adr.can_bus_dev_}, id_{adr.id_}, handle_{adr.handle_}, name_{adr.name_},
+	  decoder_{adr.decoder_}, callback_{adr.callback_}, reccuring_{adr.reccuring_}, wait_for_multiple_responses_{adr.wait_for_multiple_responses_},
+	  in_flight_{adr.in_flight_}, frequency_clock_{adr.frequency_clock_}, timeout_clock_{adr.timeout_clock_}
+{}
+
 active_diagnostic_request_t::active_diagnostic_request_t()
-	: can_bus_dev_{nullptr}, uint32_t id_{0}, DiagnosticRequestHandle{nullptr}, name_{""},
+	: can_bus_dev_{nullptr}, id_{0}, handle_{nullptr}, name_{""},
 	  decoder_{nullptr}, callback_{nullptr}, reccuring_{false}, wait_for_multiple_responses_{false},
 	  in_flight_{false}, frequency_clock_{frequency_clock_t()}, timeout_clock_{frequency_clock_t()}
 {}
 
-void updateDiagnosticRequestEntry(CanBus* bus, DiagnosticRequest* request,
-		const char* name, bool waitForMultipleResponses,
+active_diagnostic_request_t(can_bus_dev_t* bus, DiagnosticRequest* request,
+		const std::string& name, bool wait_for_multiple_responses,
 		const DiagnosticResponseDecoder decoder,
 		const DiagnosticResponseCallback callback, float frequencyHz)
+	: bus_{bus}, id_{request->arbitration_id}, handle_{nullptr}, name_{name},
+	  decoder_{decoder}, callback_{callback}, reccuring_{frequencyHz ? true : false}, wait_for_multiple_responses_{wait_for_multiple_responses},
+	  in_flight_{false}, frequency_clock_{frequency_clock_t(frequencyHz)}, timeout_clock_{frequency_clock_t(10)}
 {
-	entry->bus = bus;
-	entry->arbitration_id = request->arbitration_id;
 	entry->handle = generate_diagnostic_request(
-			&manager->shims[bus->address - 1], request, NULL);
-	if(name != NULL) {
-		strncpy(entry->name, name, MAX_GENERIC_NAME_LENGTH);
-	} else {
-		entry->name[0] = '\0';
-	}
-	entry->waitForMultipleResponses = waitForMultipleResponses;
-
-	entry->decoder = decoder;
-	entry->callback = callback;
-	entry->recurring = frequencyHz != 0;
-	entry->frequencyClock = {0};
-	entry->frequencyClock.frequency = entry->recurring ? frequencyHz : 0;
-	// time out after 100ms
-	entry->timeoutClock = {0};
-	entry->timeoutClock.frequency = 10;
-	entry->inFlight = false;
-	}
\ No newline at end of file
+	&manager->shims[bus->address - 1], request, NULL);
+}
+
+can_bus_dev_t* active_diagnostic_request_t::get_can_bus_dev()
+{
+	return can_bus_dev_;
+}
+
+DiagnosticRequestHandle& active_diagnostic_request_t::get_handle()
+{
+	return handle_;
+}
+
+bool active_diagnostic_request_t::get_recurring() const
+{
+	return recurring_;
+}
+
+bool active_diagnostic_request_t::get_in_flight() const
+{
+	return in_flight_;
+}
+
+void active_diagnostic_request_t::set_handle(DiagnosticShims& shims, DiagnosticRequest& request)
+{
+	handle_ = generate_diagnostic_request(shims_, request, nullptr)
+}
+
+void active_diagnostic_request_t::set_in_flight(bool val)
+{
+	in_flight_ = val;
+}
+
+bool active_diagnostic_request_t::timed_out() const
+{
+	// don't use staggered start with the timeout clock
+	return timeout_clock_.elapsed(false);
+}
+
+/// @brief Returns true if a sufficient response has been received for a
+/// diagnostic request.
+///
+/// This is true when at least one response has been received and the request is
+/// configured to not wait for multiple responses. Functional broadcast requests
+/// may often wish to wait the full 100ms for modules to respond.
+bool active_diagnostic_request_t::response_received() const
+{
+	return !wait_for_multiple_responses_ &&
+				handle_.completed;
+}
+
+/// @brief Returns true if the request has timed out waiting for a response,
+/// or a sufficient number of responses has been received.
+///
+bool active_diagnostic_request_t::request_completed() const
+{
+	return response_received() || 
+		(timed_out() && diagnostic_request_sent(handle_));
+}
\ No newline at end of file
diff --git a/src/diagnostic/active-diagnostic-request.hpp b/src/diagnostic/active-diagnostic-request.hpp
index f1128c5e..1e996c03 100644
--- a/src/diagnostic/active-diagnostic-request.hpp
+++ b/src/diagnostic/active-diagnostic-request.hpp
@@ -79,9 +79,25 @@ private:
 	frequency_clock_t timeout_clock_; /*!< timeout_clock_ - A frequency_clock_t object to monitor how long it's been since
 									* this request was sent.*/
 public:
+	bool& operator==(const active_diagnostic_request_t& adr) const;
+	active_diagnostic_request_t& operator=(const active_diagnostic_request_t& adr);
 	active_diagnostic_request_t();
-
-	void updateDiagnosticRequestEntry(diagnostic_manager_t* manager, can_bus_dev_t* bus, DiagnosticRequest* request,
-		const std::string name, bool wait_for_multiple_responses, const DiagnosticResponseDecoder decoder,
+	active_diagnostic_request_t(active_diagnostic_request_t&&) = default;
+	active_diagnostic_request_t(const active_diagnostic_request_t&) = default;
+	active_diagnostic_request_t(can_bus_dev_t* bus, DiagnosticRequest* request,
+		const std::string& name, bool waitForMultipleResponses,
+		const DiagnosticResponseDecoder decoder,
 		const DiagnosticResponseCallback callback, float frequencyHz);
+	
+	can_bus_dev_t* get_can_bus_dev();
+	DiagnosticRequestHandle& get_handle();
+	bool get_recurring() const;
+	bool get_in_flight() const;
+
+	void set_handle(DiagnosticShims& shims, DiagnosticRequest* request);
+	void set_in_flight(bool val);
+
+	bool timed_out() const;
+	bool response_received() const;
+	bool request_completed() const;
 };
diff --git a/src/diagnostic/diagnostic-manager.cpp b/src/diagnostic/diagnostic-manager.cpp
index 547b087e..0b59809e 100644
--- a/src/diagnostic/diagnostic-manager.cpp
+++ b/src/diagnostic/diagnostic-manager.cpp
@@ -22,17 +22,215 @@
 #include "uds/uds.h"
 #include "../configuration.hpp"
 
+#define MAX_RECURRING_DIAGNOSTIC_FREQUENCY_HZ 10
+#define MAX_SIMULTANEOUS_DIAG_REQUESTS 50
 #define MAX_REQUEST_ENTRIES 50
 
 diagnostic_manager_t::diagnostic_manager_t()
-	: request_list_entries_(MAX_REQUEST_ENTRIES, active_diagnostic_request_t())
-{}
+	: request_list_entries_(MAX_REQUEST_ENTRIES), initialized_{false}
+{
+	reset();
+}
 
 diagnostic_manager_t::diagnostic_manager_t(can_bus_dev_t& bus)
-	: bus_(&bus), request_list_entries_(MAX_REQUEST_ENTRIES, active_diagnostic_request_t())
-{}
+	: bus_(&bus), request_list_entries_(MAX_REQUEST_ENTRIES), initialized_{false}
+{
+	reset();
+}
+
+void diagnostic_manager_t::find_and_erase(active_diagnostic_request_t& entry, std::vector<active_diagnostic_request_t>& requests_list)
+{
+	auto i = std::find(requests_list.begin(), requests_list.end(), entry);
+	if ( i != requests_list.end())
+		requests_list.erase(i);
+}
+
+/// Move the entry to the free list and decrement the lock count for any
+/// CAN filters it used.
+void diagnostic_manager_t::cancel_request(active_diagnostic_request_t& entry)
+{
+	free_request_entries_.push_back(entry);
+	/* TODO: implement acceptance filters.
+	if(entry.arbitration_id_ == OBD2_FUNCTIONAL_BROADCAST_ID) {
+		for(uint32_t filter = OBD2_FUNCTIONAL_RESPONSE_START;
+				filter < OBD2_FUNCTIONAL_RESPONSE_START +
+					OBD2_FUNCTIONAL_RESPONSE_COUNT;
+				filter++) {
+			removeAcceptanceFilter(entry.bus_, filter,
+					CanMessageFormat::STANDARD, getCanBuses(),
+					getCanBusCount());
+		}
+	} else {
+		removeAcceptanceFilter(entry.bus_,
+				entry.arbitration_id_ +
+					DIAGNOSTIC_RESPONSE_ARBITRATION_ID_OFFSET,
+				CanMessageFormat::STANDARD, getCanBuses(), getCanBusCount());
+	}*/
+}
+
+void diagnostic_manager_t::cleanup_request(active_diagnostic_request_t& entry, bool force)
+{
+	if(force || (entry.get_in_flight() && entry.request_completed()))
+	{
+		entry.set_in_flight(false);
+
+		char request_string[128] = {0};
+		diagnostic_request_to_string(&entry.get_handle().request,
+			request_string, sizeof(request_string));
+		if(entry.get_recurring())
+		{
+			find_and_erase(entry, recurring_requests_);
+			if(force)
+				cancel_request(entry);
+			else
+			{
+				DEBUG(binder_interface, "Moving completed recurring request to the back of the queue: %s", request_string);
+				recurring_requests_.push_back(entry);
+			}
+		}
+		else
+		{
+			DEBUG(binder_interface, "Cancelling completed, non-recurring request: %s", request_string);
+			find_and_erase(entry, non_recurring_requests_);
+			cancel_request(entry);
+		}
+	}
+}
+
+/// @brief Clean up the request list, move as many to the free list as possible
+void diagnostic_manager_t::cleanup_active_requests(bool force)
+{
+	for(auto& entry : non_recurring_requests_)
+		cleanup_request(entry, force);
+
+	for(auto& entry : recurring_requests_)
+		cleanup_request(entry, force);
+}
+
+/// @brief Note that this pops it off of whichver list it was on and returns it, so make
+/// sure to add it to some other list or it'll be lost.
+bool diagnostic_manager_t::lookup_recurring_request(const DiagnosticRequest* request)
+{
+	active_diagnostic_request_t existingEntry;
+	for (auto& entry : recurring_requests_)
+	{
+		active_diagnostic_request_t& candidate = entry;
+		if(candidate.get_can_bus_dev()->get_device_name() == bus_->get_device_name() &&
+			diagnostic_request_equals(&candidate.get_handle().request, request))
+		{
+			find_and_erase(entry, recurring_requests_);
+			//existingEntry = entry;
+			return true;
+			break;
+		}
+	}
+	return false;
+}
+
+void diagnostic_manager_t::reset()
+{
+	if(initialized_)
+	{
+		DEBUG(binder_interface, "Clearing existing diagnostic requests");
+		cleanup_active_requests(true);
+	}
+
+	for(int i = 0; i < MAX_SIMULTANEOUS_DIAG_REQUESTS; i++)
+		free_request_entries_.push_back(request_list_entries_[i]);
+}
+
+can_bus_dev_t* diagnostic_manager_t::get_can_bus_dev()
+{
+	return bus_;
+}
+
+active_diagnostic_request_t& diagnostic_manager_t::get_free_entry()
+{
+	//FIXME: Test against empty vector
+	//if (request_list_entries_.empty())
+	//	return;
+
+	active_diagnostic_request_t& adr = request_list_entries_.back();
+	request_list_entries_.pop_back();
+	return adr;
+}
+
+bool diagnostic_manager_t::add_request(DiagnosticRequest* request, const std::string name,
+	bool wait_for_multiple_responses, const DiagnosticResponseDecoder decoder,
+	const DiagnosticResponseCallback callback)
+{
+	cleanup_active_requests(false);
+
+	bool added = true;
+	active_diagnostic_request_t& entry = get_free_entry();
+
+	// TODO: implement Acceptance Filter
+	//	if(updateRequiredAcceptanceFilters(bus, request)) {
+		entry = active_diagnostic_request_t(bus_, request, name,
+				wait_for_multiple_responses, decoder, callback, 0);
+		entry.set_handle(shims_, request);
+
+		char request_string[128] = {0};
+		diagnostic_request_to_string(&entry.get_handle().request, request_string,
+				sizeof(request_string));
+
+		find_and_erase(entry, non_recurring_requests_);
+		DEBUG(binder_interface, "Added one-time diagnostic request on bus %s: %s",
+				bus_->get_device_name(), request_string);
+
+		non_recurring_requests_.push_back(entry);
+
+	return added;
+}
 
-bool shims_send(const uint32_t arbitration_id, const uint8_t* data, const uint8_t size)
+bool diagnostic_manager_t::validate_optional_request_attributes(float frequencyHz)
+{
+	if(frequencyHz > MAX_RECURRING_DIAGNOSTIC_FREQUENCY_HZ) {
+		DEBUG(binder_interface, "Requested recurring diagnostic frequency %d is higher than maximum of %d",
+			frequencyHz, MAX_RECURRING_DIAGNOSTIC_FREQUENCY_HZ);
+		return false;
+	}
+	return true;
+}
+
+bool diagnostic_manager_t::add_recurring_request(DiagnosticRequest* request, const char* name,
+		bool wait_for_multiple_responses, const DiagnosticResponseDecoder decoder,
+		const DiagnosticResponseCallback callback, float frequencyHz)
+{
+	if(!validate_optional_request_attributes(frequencyHz))
+		return false;
+
+	cleanup_active_requests(false);
+
+	bool added = true;
+	if(lookup_recurring_request(request))
+	{
+		active_diagnostic_request_t& entry = get_free_entry();
+		// TODO: implement Acceptance Filter
+		//if(updateRequiredAcceptanceFilters(bus, request)) {
+			entry = active_diagnostic_request_t(bus_, request, name,
+					wait_for_multiple_responses, decoder, callback, frequencyHz);
+			entry.set_handle(shims_, request);
+
+			char request_string[128] = {0};
+			diagnostic_request_to_string(&entry.get_handle().request, request_string,
+					sizeof(request_string));
+
+			find_and_erase(entry, recurring_requests_);
+			DEBUG(binder_interface, "Added recurring diagnostic request (freq: %f) on bus %d: %s",
+					frequencyHz, bus_->get_device_name(), request_string);
+
+			recurring_requests_.push_back(entry);
+	}
+	else
+	{
+		DEBUG(binder_interface, "Can't add request, one already exists with same key");
+		added = false;
+	}
+	return added;
+}
+
+bool diagnostic_manager_t::shims_send(const uint32_t arbitration_id, const uint8_t* data, const uint8_t size)
 {
 	can_bus_dev_t *can_bus_dev = config->get_diagnostic_manager().get_can_bus_dev();
 	return can_bus_dev->shims_send(arbitration_id, data, size);
@@ -46,10 +244,6 @@ void diagnostic_manager_t::shims_logger(const char* m, ...)
 void diagnostic_manager_t::shims_timer()
 {}
 
-can_bus_dev_t* diagnostic_manager_t::get_can_bus_dev()
-{
-	return bus_;
-}
 /**
  * @brief initialize shims used by UDS lib and set initialized_ to true.
  *  It is needed before used the diagnostic manager fully because shims are
@@ -57,6 +251,6 @@ can_bus_dev_t* diagnostic_manager_t::get_can_bus_dev()
  */
 void diagnostic_manager_t::init_diagnostic_shims()
 {
-	DiagnosticShims shims_ = diagnostic_init_shims(shims_logger, shims_send, NULL);
+	shims_ = diagnostic_init_shims(shims_logger, shims_send, NULL);
 	initialized_ = true;
 }
\ No newline at end of file
diff --git a/src/diagnostic/diagnostic-manager.hpp b/src/diagnostic/diagnostic-manager.hpp
index 2b804a47..e4221dec 100644
--- a/src/diagnostic/diagnostic-manager.hpp
+++ b/src/diagnostic/diagnostic-manager.hpp
@@ -32,6 +32,7 @@
  */
 #define MAX_SHIM_COUNT can_bus_t.get_can_devices().size()
 
+
 /**
  * @brief The core structure for running the diagnostics module on the VI.
  *
@@ -49,7 +50,7 @@ private:
 							 * library (uds-c) into the VI's CAN peripheral.*/
 	can_bus_dev_t* bus_; /*!< bus_ - A pointer to the CAN bus that should be used for all standard OBD-II requests, if the bus is not
 						  * explicitly spcified in the request. If NULL, all requests require an explicit bus.*/
-	std::queue<active_diagnostic_request_t> recurring_requests_; /*!< recurringRequests - A queue of active, recurring diagnostic requests. When
+	std::vector<active_diagnostic_request_t> recurring_requests_; /*!< recurringRequests - A queue of active, recurring diagnostic requests. When
 																  * a response is received for a recurring request or it times out, it is
 																  * popped from the queue and pushed onto the back. */
 	std::vector<active_diagnostic_request_t> non_recurring_requests_; /*!< nonrecurringRequests - A list of active one-time diagnostic requests. When a
@@ -69,9 +70,20 @@ public:
 	void init_diagnostic_shims();
 
 	can_bus_dev_t* get_can_bus_dev();
+	active_diagnostic_request_t& get_free_entry();
+
+	void find_and_erase(active_diagnostic_request_t& entry, std::vector<active_diagnostic_request_t>& requests_list);
+	void cancel_request(active_diagnostic_request_t& entry);
+	void cleanup_request(active_diagnostic_request_t& entry, bool force);
+	void cleanup_active_requests(bool force);
+	bool lookup_recurring_request(const DiagnosticRequest* request);
+
+
+	bool validate_optional_request_attributes(float frequencyHz);
+	void reset();
 
 	void checkSupportedPids(const active_diagnostic_request_t& request,
-	const DiagnosticResponse& response, float parsedPayload);
+		const DiagnosticResponse& response, float parsedPayload);
 
 	bool add_request(DiagnosticRequest* request, const std::string name,
 		bool waitForMultipleResponses, const DiagnosticResponseDecoder decoder,
@@ -80,6 +92,4 @@ public:
 	bool add_recurring_request(DiagnosticRequest* request, const char* name,
 		bool waitForMultipleResponses, const DiagnosticResponseDecoder decoder,
 		const DiagnosticResponseCallback callback, float frequencyHz);
-
-	void reset();
 };