#pragma once

#include "../../../core/event_color.hpp"
#include "../imu_support.hpp"
#include "parser.hpp"

#include <boost/endian.hpp>
#include <opencv2/imgproc.hpp>

namespace dv::io::camera::parser::DAVIS {

enum class SensorModel {
	DAVIS240A = 0,
	DAVIS240B = 1,
	DAVIS240C = 2,
	// DAVIS128 = 3,
	DAVIS346 = 5,
	DAVIS640 = 6,
	CDAVIS   = 7,
	// DAVIS208 = 8,
	// DAVIS346BSI = 9,
};

inline std::ostream &operator<<(std::ostream &os, const SensorModel &var) {
	switch (var) {
		case SensorModel::DAVIS240A:
			return os << "DAVIS240A";

		case SensorModel::DAVIS240B:
			return os << "DAVIS240B";

		case SensorModel::DAVIS240C:
			return os << "DAVIS240C";

			// case SensorModel::DAVIS128:
			//	return os << "DAVIS128";

		case SensorModel::DAVIS346:
			return os << "DAVIS346";

		case SensorModel::DAVIS640:
			return os << "DAVIS640";

		case SensorModel::CDAVIS:
			return os << "CDAVIS";

			// case SensorModel::DAVIS208:
			//	return os << "DAVIS208";

			// case SensorModel::DAVIS346BSI:
			//	return os << "DAVIS346BSI";
	}

	return os;
}

enum class ColorMode {
	// Default: color for color sensors (debayer), grayscale for mono sensors.
	DEFAULT = 0,
	// Force grayscale.
	GRAYSCALE = 1,
	// Original: unchanged pixels from sensor. No debayering.
	ORIGINAL = 2,
	// Original split: unchanged pixels from sensor, split into four quadrants
	// by their color. No debayering. Only works on color sensors!
	ORIGINAL_SPLIT = 3,
};

inline std::ostream &operator<<(std::ostream &os, const ColorMode &var) {
	switch (var) {
		case ColorMode::DEFAULT:
			return os << "default";

		case ColorMode::GRAYSCALE:
			return os << "grayscale";

		case ColorMode::ORIGINAL:
			return os << "original";

		case ColorMode::ORIGINAL_SPLIT:
			return os << "original (split quadrants)";
	}

	return os;
}

class Parser final : public ParserBase {
public:
	Parser(const SensorModel sensorModel, const cv::Size dvsResolutionDevice, const cv::Size apsResolutionDevice,
		const dv::io::camera::imu::ImuModel imuModel, const uint32_t dvsOrientation, const uint32_t apsOrientation,
		const uint32_t imuOrientation, const dv::PixelArrangement apsColorFilter) :
		mModel(sensorModel) {
		mDvs.sizeDevice = dvsResolutionDevice;
		mDvs.invertXY   = dvsOrientation & 0x04;

		mFrame.colorFilterControl = apsColorFilter;
		mFrame.invertXY           = apsOrientation & 0x04;
		mFrame.flipX              = apsOrientation & 0x02;
		mFrame.flipY              = apsOrientation & 0x01;
		mFrame.sizeDevice         = apsResolutionDevice;
		mFrame.sizeUser           = mFrame.sizeDevice;
		if (mFrame.invertXY) {
			std::swap(mFrame.sizeUser.width, mFrame.sizeUser.height);
		}

		mImu.model = imuModel;
		mImu.flipX = imuOrientation & 0x04;
		mImu.flipY = imuOrientation & 0x02;
		mImu.flipZ = imuOrientation & 0x01;
	}

	[[nodiscard]] bool getDVSFlipHorizontal() const {
		return (mDvs.invertXY) ? (mDvs.flipYControl) : (mDvs.flipXControl);
	}

	void setDVSFlipHorizontal(const bool flip_x) {
		if (mDvs.invertXY) {
			mDvs.flipYControl = flip_x;
		}
		else {
			mDvs.flipXControl = flip_x;
		}
	}

	[[nodiscard]] bool getDVSFlipVertical() const {
		return (mDvs.invertXY) ? (mDvs.flipXControl) : (mDvs.flipYControl);
	}

	void setDVSFlipVertical(const bool flip_y) {
		if (mDvs.invertXY) {
			mDvs.flipXControl = flip_y;
		}
		else {
			mDvs.flipYControl = flip_y;
		}
	}

	[[nodiscard]] bool getDVSConvertAllOn() const {
		return mDvs.convertAllOn;
	}

	void setDVSConvertAllOn(const bool convertAllOn) {
		mDvs.convertAllOn = convertAllOn;
	}

	[[nodiscard]] bool getFrameFlipHorizontal() const {
		return (mFrame.invertXY) ? (mFrame.flipYControl) : (mFrame.flipXControl);
	}

	void setFrameFlipHorizontal(const bool flip_x) {
		const auto prev = getFrameFlipHorizontal();

		if (mFrame.invertXY) {
			mFrame.flipYControl = flip_x;
		}
		else {
			mFrame.flipXControl = flip_x;
		}

		// Update color filter to take flips into account, as those mirror
		// the 2x2 pixel groups within the Bayer filter. This is in effect
		// equivalent to switching to another Bayer color arrangement.
		if (mFrame.colorFilterControl != dv::PixelArrangement::MONO) {
			if ((!prev && flip_x) || (prev && !flip_x)) {
				switch (mFrame.colorFilterControl) {
					case dv::PixelArrangement::RGBG:
						mFrame.colorFilterControl = dv::PixelArrangement::GRGB;
						break;

					case dv::PixelArrangement::GRGB:
						mFrame.colorFilterControl = dv::PixelArrangement::RGBG;
						break;

					case dv::PixelArrangement::GBGR:
						mFrame.colorFilterControl = dv::PixelArrangement::BGRG;
						break;

					case dv::PixelArrangement::BGRG:
						mFrame.colorFilterControl = dv::PixelArrangement::GBGR;
						break;

					default:
					case dv::PixelArrangement::MONO:
						// Impossible.
						break;
				}
			}
		}
	}

	[[nodiscard]] bool getFrameFlipVertical() const {
		return (mFrame.invertXY) ? (mFrame.flipXControl) : (mFrame.flipYControl);
	}

	void setFrameFlipVertical(const bool flip_y) {
		const auto prev = getFrameFlipVertical();

		if (mFrame.invertXY) {
			mFrame.flipXControl = flip_y;
		}
		else {
			mFrame.flipYControl = flip_y;
		}

		// Update color filter to take flips into account, as those mirror
		// the 2x2 pixel groups within the Bayer filter. This is in effect
		// equivalent to switching to another Bayer color arrangement.
		if (mFrame.colorFilterControl != dv::PixelArrangement::MONO) {
			if ((!prev && flip_y) || (prev && !flip_y)) {
				switch (mFrame.colorFilterControl) {
					case dv::PixelArrangement::RGBG:
						mFrame.colorFilterControl = dv::PixelArrangement::GBGR;
						break;

					case dv::PixelArrangement::GRGB:
						mFrame.colorFilterControl = dv::PixelArrangement::BGRG;
						break;

					case dv::PixelArrangement::GBGR:
						mFrame.colorFilterControl = dv::PixelArrangement::RGBG;
						break;

					case dv::PixelArrangement::BGRG:
						mFrame.colorFilterControl = dv::PixelArrangement::GRGB;
						break;

					default:
					case dv::PixelArrangement::MONO:
						// Impossible.
						break;
				}
			}
		}
	}

	[[nodiscard]] dv::PixelArrangement getFrameColorFilter() const {
		return mFrame.colorFilterControl;
	}

	[[nodiscard]] ColorMode getFrameColorMode() const {
		return mFrame.colorMode;
	}

	void setFrameColorMode(const ColorMode mode) {
		mFrame.colorMode = mode;
	}

	[[nodiscard]] bool getIMUFlipX() const {
		return mImu.flipXControl;
	}

	void setIMUFlipX(const bool flip_x) {
		mImu.flipXControl = flip_x;
	}

	[[nodiscard]] bool getIMUFlipY() const {
		return mImu.flipYControl;
	}

	void setIMUFlipY(const bool flip_y) {
		mImu.flipYControl = flip_y;
	}

	[[nodiscard]] bool getIMUFlipZ() const {
		return mImu.flipZControl;
	}

	void setIMUFlipZ(const bool flip_z) {
		mImu.flipZControl = flip_z;
	}

	void parseData(const std::span<const uint8_t> buffer, [[maybe_unused]] const bool dataLost) override {
		for (size_t bufferPos = 0; bufferPos < buffer.size(); bufferPos += 2) {
			const uint16_t event = boost::endian::load_little_u16(&buffer[bufferPos]);

			// Check if is a timestamp.
			if ((event & 0x8000) != 0) {
				// Is a timestamp! Expand to 32 bits. (Tick is 1µs already.)
				updateTimestamp(mTimestamps.wrapAdd + (event & 0x7FFF));
			}
			else {
				// Look at the code, to determine event and data type.
				const auto code = static_cast<uint8_t>((event & 0x7000) >> 12);
				const auto data = static_cast<uint16_t>(event & 0x0FFF);

				switch (code) {
					case 0: // Special event
						switch (data) {
							case 0: // Ignore this, but log it.
								mLogger(false, "Caught special reserved event!");
								break;

							case 1: { // Timetamp reset
								mLogger(true, "Timestamp reset event received.");

								mTimestamps.wrapAdd = 0;

								// Let's keep timestamping simple: wait for a TS_RESET to initialize.
								timestampInit(std::chrono::microseconds::zero());

								break;
							}

							case 2: { // External input (falling edge)
								mLogger(true, "External input (falling edge) event received.");

								mBuffers.triggers.elements.emplace_back(
									getCurrentTimestamp(), dv::TriggerType::EXTERNAL_SIGNAL_FALLING_EDGE);

								break;
							}

							case 3: { // External input (rising edge)
								mLogger(true, "External input (rising edge) event received.");

								mBuffers.triggers.elements.emplace_back(
									getCurrentTimestamp(), dv::TriggerType::EXTERNAL_SIGNAL_RISING_EDGE);

								break;
							}

							case 4: { // External input (pulse)
								mLogger(true, "External input (pulse) event received.");

								mBuffers.triggers.elements.emplace_back(
									getCurrentTimestamp(), dv::TriggerType::EXTERNAL_SIGNAL_PULSE);

								break;
							}

							case 5: { // IMU Start (6 axes)
								mLogger(true, "IMU6 Start event received.");

								mImu.count = 0;
								mImu.type  = 0;

								mImu.currentEvent = dv::IMU{};

								break;
							}

							case 7: { // IMU End
								mLogger(true, "IMU End event received.");

								if (mImu.count == IMU_TOTAL_COUNT) {
									// Timestamp at event-stream insertion point.
									mImu.currentEvent.timestamp = getCurrentTimestamp();

									mBuffers.imu.elements.push_back(mImu.currentEvent);
								}
								else {
									mLogger(false,
										fmt::format(
											"IMU End: failed to validate IMU sample count ({}), discarding samples.",
											mImu.count));
								}

								break;
							}

							case 8: { // APS Global Shutter Frame Start
								mLogger(true, "APS GS Frame Start event received.");
								mFrame.globalShutter = true;

								apsInitFrame();

								break;
							}

							case 9: { // APS Rolling Shutter Frame Start
								mLogger(true, "APS RS Frame Start event received.");
								mFrame.globalShutter = false;

								apsInitFrame();

								break;
							}

							case 10: { // APS Frame End
								mLogger(true, "APS Frame End event received.");

								// NOTE: IMU6 and APS operate on an internal event and copy that to the actual output
								// packet here, in the END state, for a reason: if a packetContainer, with all its
								// packets, is committed due to hitting any of the triggers that are not TS reset
								// or TS wrap-around related, like number of polarity events, the event in the packet
								// would be left incomplete, and the event in the new packet would be corrupted.
								// We could avoid this like for the TS reset/TS wrap-around case (see tsReset) by
								// just deleting that event, but these kinds of commits happen much more often and the
								// possible data loss would be too significant. So instead we keep a private event,
								// fill it, and then only copy it into the packet here in the END state, at which point
								// the whole event is ready and cannot be broken/corrupted in any way anymore.
								const bool validFrame = apsEndFrame();

								// Validate event and advance frame packet position.
								if (validFrame) {
									// Frame goes out!
									auto &frame = mBuffers.frames.emplace_back(mFrame.startOfExposureTimestamp,
										mFrame.exposure.currentFrameExposure, mFrame.roi.positionX,
										mFrame.roi.positionY, mFrame.pixels, dv::FrameSource::SENSOR);

									if (mModel == SensorModel::CDAVIS) {
										frameFixCDavis(frame, mFrame.colorFilterSync);
									}

									const auto colorMode = mFrame.colorMode.load();

									if ((mFrame.colorFilterSync != dv::PixelArrangement::MONO)
										&& (colorMode != ColorMode::ORIGINAL)) {
										if (colorMode == ColorMode::ORIGINAL_SPLIT) {
											frameSplitIntoQuadrants(frame);
										}
										else {
											frameDebayer(frame, colorMode, mFrame.colorFilterSync);
										}
									}

									// Unlink frame.
									mFrame.pixels = cv::Mat{};
								}

								// Always commit data on frame end.
								dataCommit();

								break;
							}

							case 11: { // APS Reset Column Start
								mLogger(true, "APS Reset Column Start event received.");

								mFrame.currentReadoutType                               = ApsReadoutType::RESET;
								mFrame.countY[dv::EnumAsInteger(ApsReadoutType::RESET)] = 0;

								mFrame.cDavisSupport.positionY = mFrame.cDavisSupport.startPositionOdd;
								mFrame.cDavisSupport.direction = CDavisOffset::INCREASING;

								break;
							}

							case 12: { // APS Signal Column Start
								mLogger(true, "APS Signal Column Start event received.");

								mFrame.currentReadoutType                                = ApsReadoutType::SIGNAL;
								mFrame.countY[dv::EnumAsInteger(ApsReadoutType::SIGNAL)] = 0;

								mFrame.cDavisSupport.positionY = mFrame.cDavisSupport.startPositionOdd;
								mFrame.cDavisSupport.direction = CDavisOffset::INCREASING;

								break;
							}

							case 13: { // APS Column End
								mLogger(true, "APS Column End event received.");

								if (mFrame.countY[dv::EnumAsInteger(mFrame.currentReadoutType)]
									!= mFrame.expectedCountY) {
									mLogger(false,
										fmt::format(
											"APS Column End - {} - {}: wrong row count {} detected, expected {}.",
											dv::EnumAsInteger(mFrame.currentReadoutType),
											mFrame.countX[dv::EnumAsInteger(mFrame.currentReadoutType)],
											mFrame.countY[dv::EnumAsInteger(mFrame.currentReadoutType)],
											mFrame.expectedCountY));
								}

								mFrame.countX[dv::EnumAsInteger(mFrame.currentReadoutType)]++;

								break;
							}

							case 14: { // APS Exposure Start
								mLogger(true, "APS Exposure Start event received.");

								mFrame.startOfExposureTimestamp = getCurrentTimestamp();

								// Send APS info event out (as special event).
								mBuffers.triggers.elements.emplace_back(
									mFrame.startOfExposureTimestamp, dv::TriggerType::APS_EXPOSURE_START);

								break;
							}

							case 15: { // APS Exposure End
								mLogger(true, "APS Exposure End event received.");

								// Send APS info event out (as special event).
								mBuffers.triggers.elements.emplace_back(
									getCurrentTimestamp(), dv::TriggerType::APS_EXPOSURE_END);

								break;
							}

							case 16: { // External generator (falling edge)
								mLogger(true, "External generator (falling edge) event received.");

								mBuffers.triggers.elements.emplace_back(
									getCurrentTimestamp(), dv::TriggerType::EXTERNAL_GENERATOR_FALLING_EDGE);

								break;
							}

							case 17: { // External generator (rising edge)
								mLogger(true, "External generator (rising edge) event received.");

								mBuffers.triggers.elements.emplace_back(
									getCurrentTimestamp(), dv::TriggerType::EXTERNAL_GENERATOR_RISING_EDGE);

								break;
							}

							default:
								mLogger(false, fmt::format("Caught special event that can't be handled: {}.", data));
								break;
						}
						break;

					case 1: {
						// Y address
						// Check range conformity.
						dv::runtime_assert(
							[this, data] {
								return data < mDvs.sizeDevice.height;
							},
							[this, data] {
								return fmt::format(
									"DVS: Y address out of range (0-{}): {}", (mDvs.sizeDevice.height - 1), data);
							});

						mDvs.lastY = (mDvs.flipYControl) ? (mDvs.sizeDevice.height - 1 - data) : (data);

						break;
					}

					case 2:   // X address, Polarity OFF
					case 3: { // X address, Polarity ON
						// Check range conformity.
						dv::runtime_assert(
							[this, data] {
								return data < mDvs.sizeDevice.width;
							},
							[this, data] {
								return fmt::format(
									"DVS: X address out of range (0-{}): {}", (mDvs.sizeDevice.width - 1), data);
							});

						const auto xAddr    = (mDvs.flipXControl) ? (mDvs.sizeDevice.width - 1 - data) : (data);
						const auto polarity = (mDvs.convertAllOn || static_cast<bool>(code & 0x01));

						if (mDvs.invertXY) {
							mBuffers.events.elements.emplace_back(getCurrentTimestamp(), mDvs.lastY, xAddr, polarity);
						}
						else {
							mBuffers.events.elements.emplace_back(getCurrentTimestamp(), xAddr, mDvs.lastY, polarity);
						}

						break;
					}

					case 4: {
						// Ignore too big X/Y counts, can happen if column start/end events are lost.
						if ((mFrame.countX[dv::EnumAsInteger(mFrame.currentReadoutType)] >= mFrame.expectedCountX)
							|| (mFrame.countY[dv::EnumAsInteger(mFrame.currentReadoutType)] >= mFrame.expectedCountY)) {
							break;
						}

						uint16_t pixelValue = data;

						// DAVIS240 has a reduced dynamic range due to external
						// ADC high/low ref resistors not having optimal values.
						// To fix this multiply by 1.95 to 2.15, so we choose to
						// just shift by one (multiply by 2.00) for efficiency.
						if ((mModel == SensorModel::DAVIS240A) || (mModel == SensorModel::DAVIS240B)
							|| (mModel == SensorModel::DAVIS240C)) {
							pixelValue = std::clamp<uint16_t>(pixelValue << 1, 0, 1023);
						}

						// Cut down to 8 bits from 10. CDAVIS is special.
						if (mModel == SensorModel::CDAVIS) {
							cdavisUpdateFrame(pixelValue >> 2);
						}
						else {
							apsUpdateFrame(pixelValue >> 2);
						}

						mFrame.countY[dv::EnumAsInteger(mFrame.currentReadoutType)]++;

						break;
					}

					case 5: {
						// Misc 8bit data.
						const auto misc8Code = static_cast<uint8_t>((data & 0x0F00) >> 8);
						const auto misc8Data = static_cast<uint8_t>(data & 0x00FF);

						switch (misc8Code) {
							case 0:
								mLogger(true, fmt::format("IMU Data event ({}) received.", misc8Data));

								// IMU data event.
								switch (mImu.count) {
									case 0:
									case 2:
									case 4:
									case 6:
									case 8:
									case 10:
									case 12:
										mImu.tmpData = misc8Data;
										break;

									case 1: {
										auto accelX = static_cast<int16_t>((mImu.tmpData << 8) | misc8Data);
										if (mImu.flipX != mImu.flipXControl) {
											accelX = imu::safeFlip16(accelX);
										}
										mImu.currentEvent.accelerometerX = static_cast<float>(accelX) / mImu.accelScale;
										break;
									}

									case 3: {
										auto accelY = static_cast<int16_t>((mImu.tmpData << 8) | misc8Data);
										if (mImu.flipY != mImu.flipYControl) {
											accelY = imu::safeFlip16(accelY);
										}
										mImu.currentEvent.accelerometerY = static_cast<float>(accelY) / mImu.accelScale;
										break;
									}

									case 5: {
										auto accelZ = static_cast<int16_t>((mImu.tmpData << 8) | misc8Data);
										if (mImu.flipZ != mImu.flipZControl) {
											accelZ = imu::safeFlip16(accelZ);
										}
										mImu.currentEvent.accelerometerZ = static_cast<float>(accelZ) / mImu.accelScale;

										// IMU parser count depends on which data is present.
										if ((mImu.type & IMU_TYPE_TEMP) == 0) {
											if (mImu.type & IMU_TYPE_GYRO) {
												// No temperature, but gyro.
												mImu.count = static_cast<uint8_t>(mImu.count + 2);
											}
											else {
												// No others enabled.
												mImu.count = static_cast<uint8_t>(mImu.count + 8);
											}
										}
										break;
									}

									case 7: {
										// Temperature is signed. Formula for converting to °C:
										// (SIGNED_VAL / 340) + 35 for InvenSense MPU-6050
										// (SIGNED_VAL / 333.87) + 21 for InvenSense MPU-9250
										const auto temp = static_cast<int16_t>((mImu.tmpData << 8) | misc8Data);

										if (mImu.model == dv::io::camera::imu::ImuModel::IMU_INVENSENSE_6500_9250) {
											mImu.currentEvent.temperature
												= (static_cast<float>(temp) / 333.87F) + 21.0F;
										}
										else {
											mImu.currentEvent.temperature = (static_cast<float>(temp) / 340.0F) + 35.0F;
										}

										// IMU parser count depends on which data is present.
										if ((mImu.type & IMU_TYPE_GYRO) == 0) {
											// No others enabled.
											mImu.count = static_cast<uint8_t>(mImu.count + 6);
										}
										break;
									}

									case 9: {
										auto gyroX = static_cast<int16_t>((mImu.tmpData << 8) | misc8Data);
										if (mImu.flipX != mImu.flipXControl) {
											gyroX = imu::safeFlip16(gyroX);
										}
										mImu.currentEvent.gyroscopeX = static_cast<float>(gyroX) / mImu.gyroScale;
										break;
									}

									case 11: {
										auto gyroY = static_cast<int16_t>((mImu.tmpData << 8) | misc8Data);
										if (mImu.flipY != mImu.flipYControl) {
											gyroY = imu::safeFlip16(gyroY);
										}
										mImu.currentEvent.gyroscopeY = static_cast<float>(gyroY) / mImu.gyroScale;
										break;
									}

									case 13: {
										auto gyroZ = static_cast<int16_t>((mImu.tmpData << 8) | misc8Data);
										if (mImu.flipZ != mImu.flipZControl) {
											gyroZ = imu::safeFlip16(gyroZ);
										}
										mImu.currentEvent.gyroscopeZ = static_cast<float>(gyroZ) / mImu.gyroScale;
										break;
									}

									default:
										mLogger(false, "Got invalid IMU update sequence.");
										break;
								}

								mImu.count++;

								break;

							case 1:
								// APS ROI Size Part 1 (bits 15-8).
								// Here we just store the temporary value, and use it again
								// in the next case statement.
								mFrame.roi.tmpData = static_cast<uint16_t>(misc8Data << 8);

								break;

							case 2: {
								// APS ROI Size Part 2 (bits 7-0).
								// Here we just store the values and re-use the four fields
								// sizeX/Y and positionX/Y to store endCol/Row and startCol/Row.
								// We then recalculate all the right values and set everything
								// up in START_FRAME.
								switch (mFrame.roi.update & 0x03) {
									case 0:
										// START COLUMN
										mFrame.roi.positionX = static_cast<uint16_t>(mFrame.roi.tmpData | misc8Data);
										break;

									case 1:
										// START ROW
										mFrame.roi.positionY = static_cast<uint16_t>(mFrame.roi.tmpData | misc8Data);
										break;

									case 2:
										// END COLUMN
										mFrame.roi.sizeX = static_cast<uint16_t>(mFrame.roi.tmpData | misc8Data);
										break;

									case 3:
										// END ROW
										mFrame.roi.sizeY = static_cast<uint16_t>(mFrame.roi.tmpData | misc8Data);

										// Got all sizes, now compute correct window.
										apsROIUpdateSizes();
										break;

									default:
										mLogger(false, "Got invalid ROI update sequence.");
										break;
								}

								// Jump to next type of APS info (col->row, start->end).
								mFrame.roi.update++;

								break;
							}

							case 3: {
								mLogger(true, fmt::format("IMU Scale Config event ({}) received.", data));

								// Set correct IMU accel and gyro scales, used to interpret subsequent
								// IMU samples from the device.
								mImu.accelScale = calculateIMUAccelScale(static_cast<uint16_t>(data >> 2) & 0x03);
								mImu.gyroScale  = calculateIMUGyroScale(data & 0x03);

								// Set expected type of data to come from IMU (accel, gyro, temp).
								mImu.type = static_cast<uint8_t>(data >> 5) & 0x07;

								// IMU parser start count depends on which data is present.
								if (mImu.type & IMU_TYPE_ACCEL) {
									// Accelerometer.
									mImu.count = 0;
								}
								else if (mImu.type & IMU_TYPE_TEMP) {
									// Temperature
									mImu.count = 6;
								}
								else if (mImu.type & IMU_TYPE_GYRO) {
									// Gyroscope.
									mImu.count = 8;
								}
								else {
									// Nothing, should never happen.
									mImu.count = 14;

									mLogger(false, "IMU Scale Config: no IMU sensors enabled.");
								}

								break;
							}

							default:
								mLogger(false, "Caught Misc8 event that can't be handled.");
								break;
						}

						break;
					}

					case 6: {
						// Misc 10bit data.
						const auto misc10Code = static_cast<uint8_t>((data & 0x0C00) >> 10);
						const auto misc10Data = static_cast<uint16_t>(data & 0x03FF);

						switch (misc10Code) {
							case 0:
								mFrame.exposure.currentFrameExposure
									|= (static_cast<uint32_t>(misc10Data)
										<< static_cast<uint32_t>(10 * mFrame.exposure.tmpData));
								mFrame.exposure.tmpData++;
								break;

							default:
								mLogger(false, "Caught Misc10 event that can't be handled.");
								break;
						}

						break;
					}

					case 7: { // Timestamp wrap
						mLogger(true, fmt::format("Timestamp wrap event received with multiplier of {}.", data));

						// Each wrap is 2^15 µs (~32ms), and we have
						// to multiply it with the wrap counter,
						// which is located in the data part of this
						// event.
						mTimestamps.wrapAdd += (TS_WRAP_ADD * data);

						updateTimestamp(mTimestamps.wrapAdd);

						break;
					}

					default:
						mLogger(false, "Caught event that can't be handled.");
						break;
				}
			}
		}
	}

private:
	enum class ApsReadoutType : bool {
		RESET  = false,
		SIGNAL = true,
	};

	enum class CDavisOffset : bool {
		INCREASING = false,
		DECREASING = true,
	};

	static constexpr uint16_t TS_WRAP_ADD{0x8000};

	static constexpr uint8_t IMU_TYPE_TEMP{0x01};
	static constexpr uint8_t IMU_TYPE_GYRO{0x02};
	static constexpr uint8_t IMU_TYPE_ACCEL{0x04};
	static constexpr uint8_t IMU_TOTAL_COUNT{14};

	static constexpr uint8_t APS_RESET_CUTOFF{96};

	[[nodiscard]] static float calculateIMUAccelScale(const uint8_t imuAccelScale) {
		// Accelerometer scale is:
		// 0 - +-2 g - 16384 LSB/g
		// 1 - +-4 g - 8192 LSB/g
		// 2 - +-8 g - 4096 LSB/g
		// 3 - +-16 g - 2048 LSB/g
		const float accelScale = 65536.0F / static_cast<float>(4 * (1 << imuAccelScale));

		return accelScale;
	}

	[[nodiscard]] static float calculateIMUGyroScale(const uint8_t imuGyroScale) {
		// Gyroscope scale is:
		// 0 - +-250 °/s - 131 LSB/°/s
		// 1 - +-500 °/s - 65.5 LSB/°/s
		// 2 - +-1000 °/s - 32.8 LSB/°/s
		// 3 - +-2000 °/s - 16.4 LSB/°/s
		const float gyroScale = 65536.0F / static_cast<float>(500 * (1 << imuGyroScale));

		return gyroScale;
	}

	void apsInitFrame() {
		// Sync once at start of frame to avoid having them change midway.
		mFrame.flipXSync       = (mFrame.flipX != mFrame.flipXControl);
		mFrame.flipYSync       = (mFrame.flipY != mFrame.flipYControl);
		mFrame.colorFilterSync = mFrame.colorFilterControl;

		mFrame.exposure.tmpData              = 0;
		mFrame.exposure.currentFrameExposure = 0;
		mFrame.roi.tmpData                   = 0;
		mFrame.roi.update                    = 0;

		mFrame.currentReadoutType = ApsReadoutType::RESET;
		mFrame.countX.fill(0);
		mFrame.countY.fill(0);

		// Write out start of frame timestamp.
		mBuffers.triggers.elements.emplace_back(getCurrentTimestamp(), dv::TriggerType::APS_FRAME_START);
	}

	void apsROIUpdateSizes() {
		// Calculate APS ROI sizes.
		const auto startColumn = mFrame.roi.positionX;
		const auto startRow    = mFrame.roi.positionY;
		const auto endColumn   = mFrame.roi.sizeX;
		const auto endRow      = mFrame.roi.sizeY;

		// Position is already set to startCol/Row, so we don't have to reset
		// it here. We only have to calculate size from start and end Col/Row.
		mFrame.roi.sizeX = endColumn + 1 - startColumn;
		mFrame.roi.sizeY = endRow + 1 - startRow;

		// ROI positions are not inverted on device, so reflect the resolution as the user would see it.
		// Sanity check.
		if ((mFrame.roi.sizeX == 0) || (mFrame.roi.sizeX > mFrame.sizeUser.width)) {
			mLogger(false, fmt::format("ROI X size incorrect - SizeX: {}.", mFrame.roi.sizeX));

			mFrame.roi.sizeX = mFrame.sizeUser.width;
		}

		if ((mFrame.roi.sizeY == 0) || (mFrame.roi.sizeY > mFrame.sizeUser.height)) {
			mLogger(false, fmt::format("ROI Y size incorrect - SizeY: {}.", mFrame.roi.sizeY));

			mFrame.roi.sizeY = mFrame.sizeUser.height;
		}

		if ((mFrame.roi.positionX + mFrame.roi.sizeX) > mFrame.sizeUser.width) {
			mLogger(false, fmt::format("ROI X position incorrect - PosX: {}.", mFrame.roi.positionX));

			mFrame.roi.positionX = 0;
		}

		if ((mFrame.roi.positionY + mFrame.roi.sizeY) > mFrame.sizeUser.height) {
			mLogger(false, fmt::format("ROI Y position incorrect - PosY: {}.", mFrame.roi.positionY));

			mFrame.roi.positionY = 0;
		}

		if (mFrame.invertXY) {
			mFrame.expectedCountX                 = mFrame.roi.sizeY;
			mFrame.expectedCountY                 = mFrame.roi.sizeX;
			mFrame.cDavisSupport.startPositionOdd = !static_cast<bool>(mFrame.roi.positionX & 0x01);

			// Revert flips (inverted).
			if (mFrame.flipYSync != mFrame.flipY) {
				mFrame.roi.positionX = mFrame.sizeUser.width - mFrame.roi.positionX - mFrame.roi.sizeX;
			}

			if (mFrame.flipXSync != mFrame.flipX) {
				mFrame.roi.positionY = mFrame.sizeUser.height - mFrame.roi.positionY - mFrame.roi.sizeY;
			}
		}
		else {
			mFrame.expectedCountX                 = mFrame.roi.sizeX;
			mFrame.expectedCountY                 = mFrame.roi.sizeY;
			mFrame.cDavisSupport.startPositionOdd = !static_cast<bool>(mFrame.roi.positionY & 0x01);

			// Revert flips.
			if (mFrame.flipXSync != mFrame.flipX) {
				mFrame.roi.positionX = mFrame.sizeUser.width - mFrame.roi.positionX - mFrame.roi.sizeX;
			}

			if (mFrame.flipYSync != mFrame.flipY) {
				mFrame.roi.positionY = mFrame.sizeUser.height - mFrame.roi.positionY - mFrame.roi.sizeY;
			}
		}

		// CDAVIS: if only one pixel is to be read, it will always go into the first position, regardless of even/odd.
		if (mFrame.expectedCountY == 1) {
			mFrame.cDavisSupport.startPositionOdd = false;
		}

		// Allocate pixel memory.
		mFrame.pixels = cv::Mat(cv::Size{mFrame.roi.sizeX, mFrame.roi.sizeY}, CV_8UC1, cv::Scalar{0});
	}

	void apsUpdateFrame(const uint8_t dataValue) {
		auto xPos = mFrame.countX[dv::EnumAsInteger(mFrame.currentReadoutType)];
		if (mFrame.flipXSync) {
			xPos = mFrame.expectedCountX - 1 - xPos;
		}

		auto yPos = mFrame.countY[dv::EnumAsInteger(mFrame.currentReadoutType)];
		if (mFrame.flipYSync) {
			yPos = mFrame.expectedCountY - 1 - yPos;
		}

		if (mFrame.invertXY) {
			std::swap(xPos, yPos);
		}

		// Standard CDS support.
		if (mFrame.currentReadoutType == ApsReadoutType::RESET) {
			mFrame.pixels.at<uint8_t>(yPos, xPos) = dataValue;
		}
		else {
			const uint8_t resetValue  = mFrame.pixels.at<uint8_t>(yPos, xPos);
			const uint8_t signalValue = dataValue;

			if ((resetValue < APS_RESET_CUTOFF) || (signalValue == 0)) {
				// If the signal value is 0, that is only possible if the camera
				// has seen tons of light. In that case, the photo-diode current
				// may be greater than the reset current, and the reset value
				// never goes back up fully, which results in black spots where
				// there is too much light. This confuses algorithms, so we filter
				// this out here by setting the pixel to white in that case.
				// Another effect of the same thing is the reset value not going
				// back up to a decent value, so we also filter that out here.
				mFrame.pixels.at<uint8_t>(yPos, xPos) = UINT8_MAX;
			}
			else {
				// Do CDS.
				const auto pixelValue = resetValue - signalValue;

				// Check for over/underflow.
				mFrame.pixels.at<uint8_t>(yPos, xPos) = cv::saturate_cast<uint8_t>(pixelValue);
			}
		}
	}

	void cdavisUpdateFrame(const uint8_t dataValue) {
		auto xPos = mFrame.countX[dv::EnumAsInteger(mFrame.currentReadoutType)];
		if (mFrame.flipXSync) {
			xPos = mFrame.expectedCountX - 1 - xPos;
		}

		auto yPos = mFrame.cDavisSupport.positionY;
		if (mFrame.flipYSync) {
			yPos = mFrame.expectedCountY - 1 - yPos;
		}

		if (mFrame.invertXY) {
			std::swap(xPos, yPos);
		}

		dv::runtime_assert(
			[this] {
				return mFrame.globalShutter;
			},
			[this] {
				return "CDAVIS: rolling shutter not supported.";
			});

		// Standard CDS support.
		if (mFrame.currentReadoutType == ApsReadoutType::SIGNAL) {
			mFrame.pixels.at<uint8_t>(yPos, xPos) = dataValue;
		}
		else {
			// CDAVIS Global-Shutter has inverted samples, signal read comes first
			// and was stored above inside mFrame.pixels.
			const uint8_t resetValue  = dataValue;
			const uint8_t signalValue = mFrame.pixels.at<uint8_t>(yPos, xPos);

			if ((resetValue < APS_RESET_CUTOFF) || (signalValue == 0)) {
				// If the signal value is 0, that is only possible if the camera
				// has seen tons of light. In that case, the photo-diode current
				// may be greater than the reset current, and the reset value
				// never goes back up fully, which results in black spots where
				// there is too much light. This confuses algorithms, so we filter
				// this out here by setting the pixel to white in that case.
				// Another effect of the same thing is the reset value not going
				// back up to a decent value, so we also filter that out here.
				mFrame.pixels.at<uint8_t>(yPos, xPos) = UINT8_MAX;
			}
			else {
				// Do CDS.
				const auto pixelValue = resetValue - signalValue;

				// Check for over/underflow.
				mFrame.pixels.at<uint8_t>(yPos, xPos) = cv::saturate_cast<uint8_t>(pixelValue);
			}
		}

		// CDAVIS support: first 320 pixels are even, then odd.
		// When and how we switch directions depends on the ROI start position and its length on the Y axis.
		// Specifically on if they are even or odd numbers. For even/odd start position we can use the already
		// computed startPositionOdd. For the length we compute it here.
		const bool lengthOdd = (mFrame.expectedCountY & 0x01);

		// If ROI start position AND length are both even, OR both odd.
		if ((!mFrame.cDavisSupport.startPositionOdd && !lengthOdd)
			|| (mFrame.cDavisSupport.startPositionOdd && lengthOdd)) {
			if (mFrame.cDavisSupport.positionY == (mFrame.expectedCountY - 2)) {
				mFrame.cDavisSupport.direction = CDavisOffset::DECREASING;
				mFrame.cDavisSupport.positionY++;
				return;
			}
		}
		else {
			if (mFrame.cDavisSupport.positionY == (mFrame.expectedCountY - 1)) {
				mFrame.cDavisSupport.direction = CDavisOffset::DECREASING;
				mFrame.cDavisSupport.positionY--;
				return;
			}
		}

		if (mFrame.cDavisSupport.direction == CDavisOffset::INCREASING) {
			mFrame.cDavisSupport.positionY += 2;
		}
		else {
			// CDavisOffset::DECREASING
			mFrame.cDavisSupport.positionY -= 2;
		}
	}

	[[nodiscard]] bool apsEndFrame() {
		bool validFrame = true;

		for (size_t i = 0; i < mFrame.countX.size(); i++) {
			mLogger(true, fmt::format("APS Frame End: CountX[{}] is {}.", i, mFrame.countX[i]));

			if (mFrame.countX[i] != mFrame.expectedCountX) {
				mLogger(false, fmt::format("APS Frame End - {}: wrong column count {} detected, expected {}.", i,
								   mFrame.countX[i], mFrame.expectedCountX));
				validFrame = false;
			}
		}

		// Write out end of frame timestamp.
		mBuffers.triggers.elements.emplace_back(getCurrentTimestamp(), dv::TriggerType::APS_FRAME_END);

		return validFrame;
	}

	static void frameFixCDavis(dv::Frame &frame, const dv::PixelArrangement colorFilter) {
		// Adjust color filter value to take ROI into account.
		const auto firstPixelColor = dv::colorForPoint({frame.positionX, frame.positionY}, colorFilter);

		// Overwrite fourth quadrant (DAVIS) from first (G pixel), as DAVIS readout is
		// broken due to design error and has unusable quality.
		switch (firstPixelColor) {
			case dv::EventColor::RED:
				for (int y = 0; y < frame.image.rows; y += 2) {
					for (int x = 0; x < frame.image.cols; x += 2) {
						frame.image.at<uint8_t>(y + 1, x) = frame.image.at<uint8_t>(y, x + 1);
					}
				}
				break;

			case dv::EventColor::GREEN1:
				for (int y = 0; y < frame.image.rows; y += 2) {
					for (int x = 0; x < frame.image.cols; x += 2) {
						frame.image.at<uint8_t>(y + 1, x + 1) = frame.image.at<uint8_t>(y, x);
					}
				}
				break;

			case dv::EventColor::GREEN2:
				for (int y = 0; y < frame.image.rows; y += 2) {
					for (int x = 0; x < frame.image.cols; x += 2) {
						frame.image.at<uint8_t>(y, x) = frame.image.at<uint8_t>(y + 1, x + 1);
					}
				}
				break;

			case dv::EventColor::BLUE:
				for (int y = 0; y < frame.image.rows; y += 2) {
					for (int x = 0; x < frame.image.cols; x += 2) {
						frame.image.at<uint8_t>(y, x + 1) = frame.image.at<uint8_t>(y + 1, x);
					}
				}
				break;

			default:
			case dv::EventColor::WHITE:
				// Impossible.
				break;
		}
	}

	static void frameDebayer(dv::Frame &frame, const ColorMode colorMode, const dv::PixelArrangement colorFilter) {
		// Adjust color filter value to take ROI into account.
		const auto firstPixelColor = dv::colorForPoint({frame.positionX, frame.positionY}, colorFilter);

		cv::ColorConversionCodes conversionCode{};

		switch (firstPixelColor) {
			case dv::EventColor::RED:
				// As if RGBG.
				conversionCode = (colorMode == dv::io::camera::parser::DAVIS::ColorMode::DEFAULT)
								   ? (cv::COLOR_BayerBG2BGR_EA)
								   : (cv::COLOR_BayerBG2GRAY);
				break;

			case dv::EventColor::GREEN1:
				// As if GRGB.
				conversionCode = (colorMode == dv::io::camera::parser::DAVIS::ColorMode::DEFAULT)
								   ? (cv::COLOR_BayerGB2BGR_EA)
								   : (cv::COLOR_BayerGB2GRAY);
				break;

			case dv::EventColor::GREEN2:
				// As if GBGR.
				conversionCode = (colorMode == dv::io::camera::parser::DAVIS::ColorMode::DEFAULT)
								   ? (cv::COLOR_BayerGR2BGR_EA)
								   : (cv::COLOR_BayerGR2GRAY);
				break;

			case dv::EventColor::BLUE:
				// As if BGRG.
				conversionCode = (colorMode == dv::io::camera::parser::DAVIS::ColorMode::DEFAULT)
								   ? (cv::COLOR_BayerRG2BGR_EA)
								   : (cv::COLOR_BayerRG2GRAY);
				break;

			case dv::EventColor::WHITE:
			default:
				// Impossible, other color filters have been filtered out.
				break;
		}

		// Convert Bayer pattern to RGB or GRAYSCALE image.
		cv::Mat converted{};
		cv::cvtColor(frame.image, converted, conversionCode);
		frame.image = converted;
	}

	static void frameSplitIntoQuadrants(dv::Frame &frame) {
		cv::Mat clone = frame.image.clone();
		int q0x = 0, q0y = 0;
		int q1x = (clone.cols / 2), q1y = 0;
		int q2x = 0, q2y = (clone.rows / 2);
		int q3x = (clone.cols / 2), q3y = (clone.rows / 2);

		for (int y = 0; y < clone.rows; y++) {
			// ROW EVEN.
			if ((y % 2) == 0) {
				for (int x = 0; x < clone.cols; x++) {
					// COLUMN EVEN.
					if ((x % 2) == 0) {
						frame.image.at<uint8_t>(q0y, q0x) = clone.at<uint8_t>(y, x);
						q0x++;
					}
					// COLUMN ODD.
					else {
						frame.image.at<uint8_t>(q1y, q1x) = clone.at<uint8_t>(y, x);
						q1x++;
					}
				}

				q0x = 0;
				q0y++;
				q1x = (clone.cols / 2);
				q1y++;
			}
			// ROW ODD.
			else {
				for (int x = 0; x < clone.cols; x++) {
					// COLUMN EVEN.
					if ((x % 2) == 0) {
						frame.image.at<uint8_t>(q2y, q2x) = clone.at<uint8_t>(y, x);
						q2x++;
					}
					// COLUMN ODD.
					else {
						frame.image.at<uint8_t>(q3y, q3x) = clone.at<uint8_t>(y, x);
						q3x++;
					}
				}

				q2x = 0;
				q2y++;
				q3x = (clone.cols / 2);
				q3y++;
			}
		}
	}

	struct {
		int64_t wrapAdd{0};
	} mTimestamps;

	// DVS specific fields
	struct {
		cv::Size sizeDevice;
		bool invertXY{false};
		std::atomic<bool> flipXControl{false};
		std::atomic<bool> flipYControl{false};
		std::atomic<bool> convertAllOn{false};
		uint16_t lastY{0};
	} mDvs;

	// APS specific fields
	struct {
		cv::Size sizeDevice;
		cv::Size sizeUser;
		std::atomic<dv::PixelArrangement> colorFilterControl{dv::PixelArrangement::MONO};
		std::atomic<ColorMode> colorMode{ColorMode::DEFAULT};
		std::atomic<bool> flipXControl{false};
		std::atomic<bool> flipYControl{false};
		bool flipXSync{false};
		bool flipYSync{false};
		bool flipX{false};
		bool flipY{false};
		bool invertXY{false};
		bool globalShutter{true};
		dv::PixelArrangement colorFilterSync{dv::PixelArrangement::MONO};
		ApsReadoutType currentReadoutType{ApsReadoutType::RESET};
		std::array<uint16_t, 2> countX{};
		std::array<uint16_t, 2> countY{};
		uint16_t expectedCountX{0};
		uint16_t expectedCountY{0};
		int64_t startOfExposureTimestamp{0};
		cv::Mat pixels;

		struct {
			// Temporary values from device.
			uint16_t tmpData{0};
			uint16_t update{0};
			// Parameters for frame parsing.
			uint16_t positionX{0};
			uint16_t positionY{0};
			uint16_t sizeX{0};
			uint16_t sizeY{0};
		} roi;

		struct {
			uint32_t currentFrameExposure{0};
			uint8_t tmpData{0};
		} exposure;

		struct {
			uint16_t positionY{0};
			bool startPositionOdd{false};
			CDavisOffset direction{CDavisOffset::INCREASING};
		} cDavisSupport;
	} mFrame;

	// IMU specific fields
	struct {
		dv::io::camera::imu::ImuModel model;
		bool flipX{false};
		bool flipY{false};
		bool flipZ{false};
		std::atomic<bool> flipXControl{false};
		std::atomic<bool> flipYControl{false};
		std::atomic<bool> flipZControl{false};
		uint8_t type{0};
		uint8_t count{0};
		uint8_t tmpData{0};
		float accelScale{};
		float gyroScale{};
		// Current composite events, for later copy, to not lose them on commits.
		dv::IMU currentEvent{};
	} mImu;

	SensorModel mModel;
};

} // namespace dv::io::camera::parser::DAVIS

// fmt compatibility for enum class printing.
template<>
struct fmt::formatter<dv::io::camera::parser::DAVIS::SensorModel> : fmt::ostream_formatter {};

template<>
struct fmt::formatter<dv::io::camera::parser::DAVIS::ColorMode> : fmt::ostream_formatter {};
