#include "../../include/dv-processing/core/core.hpp"
#include "../../include/dv-processing/io/camera/davis.hpp"
#include "../../include/dv-processing/io/camera/dvxplorer.hpp"
#include "../../include/dv-processing/io/camera/dvxplorer_m.hpp"

#include "boost/ut.hpp"

#include <chrono>

int main() {
	using namespace boost::ut;
	using namespace std::chrono_literals;

	"simple_open"_test = [] {
		expect(throws([] {
			// Impossible config: invalid serial number should always throw an exception.
			dv::io::camera::DVXplorerM capture{"00000000"};
		}));
	};

	"davis_tests"_test = [] {
		try {
			// This should open up any DAVIS camera connected to the system
			dv::io::camera::DAVIS capture{};

			// Beyond this point, no function call should throw any exceptions
			expect(nothrow([&capture] {
				// Force a short exposure duration so we can expect APS triggers to be received.
				capture.setAutoExposure(false);
				capture.setExposureDuration(1ms);

				capture.setEventsRunning(true);
				capture.setFramesRunning(true);

				expect(capture.isFrameStreamAvailable());
				expect(capture.isImuStreamAvailable());
				expect(capture.isTriggerStreamAvailable());

				std::this_thread::sleep_for(500ms);

				expect(capture.getNextFrame().has_value());

				expect(nothrow([&capture] {
					expect(!capture.getNextTriggerBatch().value().empty());
				}));
				expect(capture.getNextEventBatch().has_value());
				expect(capture.getNextImuBatch().has_value());

				const auto pitch = capture.getPixelPitch();
				expect(eq(pitch, 0.0000185_f));

				// Disable auto exposure to avoid any interference with that
				capture.setExposureDuration(10ms);
				expect(eq(capture.getExposureDuration().count(), 10'000));

				// Set a high frame interval
				capture.setFrameInterval(100ms);
				expect(eq(capture.getFrameInterval().count(), 100'000));

				// Let everything settle in a bit
				std::this_thread::sleep_for(200ms);

				// Read any frames from within the buffer to drop them
				while (const auto frame = capture.getNextFrame()) {
				}

				// Let's collect 3 frames, these must be guaranteed to be fresh
				std::vector<dv::Frame> frames;
				while (frames.size() < 3) {
					if (const auto nextFrame = capture.getNextFrame(); nextFrame.has_value()) {
						frames.push_back(*nextFrame);
					}
				}

				// Timestamp differences should be very close to the frame interval
				expect(eq(frames.size(), 3));
				expect(gt(frames[1].timestamp - frames[0].timestamp, 98'000));
				expect(lt(frames[1].timestamp - frames[0].timestamp, 102'000));
				expect(gt(frames[2].timestamp - frames[1].timestamp, 98'000));
				expect(lt(frames[2].timestamp - frames[1].timestamp, 102'000));
			}));
		}
		catch (const std::exception &) {
			// Camera is not connected, we skip any tests
		}
	};

	"davis_reopen_test"_test = [] {
		try {
			{
				// Open any DAVIS, then do it again.
				dv::io::camera::DAVIS capture{};

				// Beyond this point, no function call should throw any exceptions
				expect(nothrow([&capture] {
					capture.setAutoExposure(false);
					capture.setExposureDuration(1ms);
				}));
			}

			{
				// Open any DAVIS, then do it again.
				dv::io::camera::DAVIS capture{};

				// Beyond this point, no function call should throw any exceptions
				expect(nothrow([&capture] {
					capture.setAutoExposure(false);
					capture.setExposureDuration(1ms);
				}));
			}
		}
		catch (const std::exception &) {
			// Camera is not connected, we skip any tests
		}
	};

	"dvxplorer_tests"_test = [] {
		try {
			// This should open up any DVXplorer camera connected to the system
			dv::io::camera::DVXplorer capture{};

			// Beyond this point, no function call should throw any exceptions
			expect(nothrow([&capture] {
				// These should work fine
				capture.setGlobalHold(false);
				capture.setReadoutFPS(dv::io::camera::DVXplorer::ReadoutFPS::VARIABLE_2000);

				expect(!capture.isFrameStreamAvailable());
				expect(capture.isImuStreamAvailable());
				expect(capture.isTriggerStreamAvailable());

				std::this_thread::sleep_for(1s);

				expect(!capture.getNextFrame().has_value());
				expect(capture.getNextEventBatch().has_value());
				expect(capture.getNextImuBatch().has_value());
				const auto pitch = capture.getPixelPitch();
				expect(eq(pitch, 0.000009_f));
			}));
		}
		catch (const std::exception &) {
			// Camera is not connected, we skip any tests
		}
	};

	"dvxplorer_reopen_test"_test = [] {
		try {
			{
				// Open any DVXplorer, then do it again.
				dv::io::camera::DVXplorer capture{};

				// Beyond this point, no function call should throw any exceptions
				expect(nothrow([&capture] {
					capture.setGlobalHold(false);
					capture.setReadoutFPS(dv::io::camera::DVXplorer::ReadoutFPS::VARIABLE_2000);
				}));
			}

			{
				// Open any DVXplorer, then do it again.
				dv::io::camera::DVXplorer capture{};

				// Beyond this point, no function call should throw any exceptions
				expect(nothrow([&capture] {
					capture.setGlobalHold(false);
					capture.setReadoutFPS(dv::io::camera::DVXplorer::ReadoutFPS::VARIABLE_2000);
				}));
			}
		}
		catch (const std::exception &) {
			// Camera is not connected, we skip any tests
		}
	};

	"handler"_test = [] {
		try {
			dv::io::camera::DVXplorerM capture{};

			// Beyond this point, no function call should throw any exceptions
			expect(nothrow([&capture] {
				// These should safely fail
				dv::io::DataReadHandler handler;

				bool imuDataReceived   = false;
				bool eventDataReceived = false;
				bool frameDataReceived = false;

				handler.mImuHandler = [&imuDataReceived](const auto &) {
					imuDataReceived = true;
				};

				handler.mEventHandler = [&eventDataReceived](const auto &) {
					eventDataReceived = true;
				};

				if (capture.isFrameStreamAvailable()) {
					handler.mFrameHandler = [&frameDataReceived](const auto &) {
						frameDataReceived = true;
					};
				}
				else {
					// Cannot test this one
					frameDataReceived = true;
				}

				int64_t start = dv::now();

				// Read until imu was received, or it takes more than 100ms
				while (!imuDataReceived || !frameDataReceived || !eventDataReceived) {
					if (!capture.handleNext(handler)) {
						break;
					}
					// There should be at least one imu measurement over the period
					if (dv::Duration(dv::now() - start) > 5000ms) {
						break;
					}
				}
				expect(imuDataReceived);
				expect(frameDataReceived);
				expect(eventDataReceived);
			}));
		}
		catch (const std::exception &) {
			// Camera is not connected, we skip any tests
		}
	};

	return EXIT_SUCCESS;
}
