#include <asyncdevicepool.h>

using namespace std::chrono;

class FakeDevice : public CANDevice {
PIOBJECT_SUBCLASS(FakeDevice, CANDevice)
public:
	double duration_ms;

	explicit FakeDevice(int counter) : duration_ms(0.0), counter(counter) {}

	bool send(const CAN_Raw &m) override {
		counter--;
		if (counter <= 0) {
			if (counter == 0) duration_ms = (duration_cast<milliseconds>(high_resolution_clock::now().time_since_epoch()) - start_time).count();
			counter = -1;
			throw can::error(0, can_category);
		}
		return true;
	}

	bool open() override {
		start_time = duration_cast<milliseconds>(high_resolution_clock::now().time_since_epoch());
		return true;
	}

	void close() override {
		if (read_handle != can::event::invalid_handle) can::event::close(read_handle);
	}

	can::event::handle getReadEventHandle() override {
		if (read_handle == can::event::invalid_handle) {
			read_handle = can::event::make();
		}
		return read_handle;
	}

protected:
	bool nextMsg(CAN_Raw &rawMsg) override {
		return false;
	}

private:
	int counter = 0;
	milliseconds start_time;
};

int main() {
	int device_count = 63;
	PIBlockingQueue<double> wait_queue(device_count);

	AsyncDevicePool asyncDevicePool;
	asyncDevicePool.set_can_error_handler([&wait_queue](const can::error& error, CANDevice* device) {
//		can::stdout_error_handler(error, device);
		wait_queue.put(dynamic_cast<FakeDevice*>(device)->duration_ms);
		return false;
	});

	int it_count = 1'000'000;
	PIVector<CANDevice*> devices;
	for (int i = 0; i < device_count; ++i) {
		devices << new FakeDevice(it_count / device_count);
		asyncDevicePool.add(devices.back());
	}

	for (int i = 0; i < it_count; ++i) {
		CAN_Raw msg = { .Id = 0x11, .Size = 4, .Data = { 0x0, 0x1, 0x2, 0x3 } };
		asyncDevicePool.send(devices[i % device_count], msg);
		if (i % 1000 == 999) {
			std::this_thread::yield();
//			std::cout << i / 1000 << "/" << counter / 1000 << std::endl;
//			std::this_thread::sleep_for(milliseconds(1));
		}
	}

	double duration_ms;
	for (int i = 0; i < device_count; ++i) duration_ms = wait_queue.take();
	std::cout << duration_ms << " ms" << std::endl;
	return 0;
}