#pragma once

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

#include <boost/endian.hpp>

namespace dv::io::camera::parser::DVXplorer {

class Parser final : public ParserBase {
public:
	explicit Parser(const cv::Size eventsResolution, const uint32_t dvsOrientation, const uint32_t imuOrientation) {
		mDvs.size  = eventsResolution;
		mDvs.flipX = dvsOrientation & 0x02;
		mDvs.flipY = dvsOrientation & 0x01;

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

	[[nodiscard]] bool getDVSFlipHorizontal() const {
		return mDvs.flipXControl;
	}

	void setDVSFlipHorizontal(const bool flip_x) {
		mDvs.flipXControl = flip_x;
	}

	[[nodiscard]] bool getDVSFlipVertical() const {
		return mDvs.flipYControl;
	}

	void setDVSFlipVertical(const bool flip_y) {
		mDvs.flipYControl = flip_y;
	}

	[[nodiscard]] bool getDVSDualBinning() const {
		return mDvs.dualBinning;
	}

	void setDVSDualBinning(const bool dual_binning) {
		mDvs.dualBinning = dual_binning;
	}

	[[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();

									// 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, the event in the
									// packet would be left incomplete, and the event in the new packet would be
									// corrupted. We could avoid this by just deleting that event, but these 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.
									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 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: { // X column address. 10 bits (9 - 0) contain address, bit 11 Start of Frame marker.
						const uint16_t columnAddress = data & 0x03FF;
						// const bool startOfFrame      = data & 0x0800;

						// TODO: startup issue workaround, sometimes all bits are 1.
						if (columnAddress == 1023) {
							mDvs.lastX = 0;
							break;
						}

						// Check range conformity.
						dv::runtime_assert(
							[this, columnAddress] {
								return columnAddress < mDvs.size.width;
							},
							[this, columnAddress] {
								return fmt::format(
									"DVS: X address out of range (0-{}): {}", (mDvs.size.width - 1), columnAddress);
							});

						mDvs.lastX = (mDvs.flipXControl) ? (mDvs.size.width - 1 - columnAddress) : (columnAddress);
						break;
					}

					case 2:
					case 3: { // 8-pixel group event presence and polarity.
							  // Code 2 is MGROUP Group 2 (SGROUP OFF), Code 3 is MGROUP Group 1 (SGROUP ON).
						const bool polarity  = ((data & 0x0100) == 0);
						const uint16_t lastY = (code == 3) ? (mDvs.lastYG1) : (mDvs.lastYG2);

						for (uint16_t i = 0, mask = 0x0001; i < 8; i++, mask <<= 1) {
							// Check if event present first.
							if ((data & mask) == 0) {
								continue;
							}

							uint16_t xAddr = mDvs.lastX;
							uint16_t yAddr = (mDvs.flipYControl) ? (mDvs.size.height - 1 - (lastY + i)) : (lastY + i);

							// Move bottom-right quadrant back up to top-left, due to internal flips.
							if (mDvs.dualBinning) {
								if (mDvs.flipX && (xAddr >= static_cast<uint16_t>(mDvs.size.width / 2))) {
									xAddr -= static_cast<uint16_t>(mDvs.size.width / 2);
								}

								if (mDvs.flipY && (yAddr >= static_cast<uint16_t>(mDvs.size.height / 2))) {
									yAddr -= static_cast<uint16_t>(mDvs.size.height / 2);
								}
							}

							// Received event!
							// Timestamp at event-stream insertion point.
							mBuffers.events.elements.emplace_back(getCurrentTimestamp(), xAddr, yAddr, polarity);
						}

						break;
					}

					case 4: {
						// Decode address.
						uint16_t group1Address      = data & 0x003F;
						const uint16_t group2Offset = static_cast<uint16_t>(data >> 6) & 0x001F;
						uint16_t group2Address
							= (data & 0x0800) ? (group1Address - group2Offset) : (group1Address + group2Offset);

						// 8 pixels per group.
						group1Address *= 8;
						group2Address *= 8;

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

						mDvs.lastYG1 = group1Address;
						mDvs.lastYG2 = group2Address;

						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: {
										// X/Y axes are inverted due to IMU chip rotation.
										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 3: {
										// X/Y axes are inverted due to IMU chip rotation.
										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 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 / 512) + 23
										const auto temp = static_cast<int16_t>((mImu.tmpData << 8) | misc8Data);
										mImu.currentEvent.temperature = (static_cast<float>(temp) / 512.0F) + 23.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: {
										// X/Y axes are inverted due to IMU chip rotation.
										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 11: {
										// X/Y axes are inverted due to IMU chip rotation.
										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 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 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 >> 3) & 0x03);
								mImu.gyroScale  = calculateIMUGyroScale(data & 0x07);

								// 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 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:
	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};

	[[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) {
		// Invert for ascending scale:
		const auto imuGyroScaleAsc = static_cast<uint8_t>(4 - imuGyroScale);

		// Gyroscope ascending scale is:
		// 0 - +- 125 °/s  - 262.4 LSB/°/s
		// 1 - +- 250 °/s  - 131.2 LSB/°/s
		// 2 - +- 500 °/s  - 65.6 LSB/°/s
		// 3 - +- 1000 °/s - 32.8 LSB/°/s
		// 4 - +- 2000 °/s - 16.4 LSB/°/s
		const float gyroScale = 65536.0F / static_cast<float>(250 * (1 << imuGyroScaleAsc));

		return gyroScale;
	}

	struct {
		int64_t wrapAdd{0};
	} mTimestamps;

	struct {
		// DVS specific fields
		cv::Size size;
		bool flipX{false};
		bool flipY{false};
		std::atomic<bool> flipXControl{false};
		std::atomic<bool> flipYControl{false};
		std::atomic<bool> dualBinning{false};
		uint16_t lastX{0};
		uint16_t lastYG1{0};
		uint16_t lastYG2{0};
	} mDvs;

	struct {
		// IMU specific fields
		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 event, for later copy, to not lose them on commits.
		dv::IMU currentEvent{};
	} mImu;
};

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