#pragma once

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

#include <boost/endian.hpp>

namespace dv::io::camera::parser::S5KRC1S {
static constexpr int16_t WIDTH{960};
static constexpr int16_t HEIGHT{720};

class Parser final : public ParserBase {
public:
	Parser() = default;

	void parseData(const std::span<const uint8_t> buffer, const bool dataLost) override {
		if (dataLost) [[unlikely]] {
			processEvent(eDataLost{});
		}

		for (size_t bufferPos = 0; bufferPos < buffer.size(); bufferPos += 4) {
			const uint32_t event = boost::endian::load_little_u32(&buffer[bufferPos]);

			if (event == 0) {
				// Padding event for MIPI, discard.
				continue;
			}

			if (event & 0x80'00'00'00) [[likely]] {
				// SGROUP/MGROUP.
				processEvent(eSMGroup{event});
			}
			else {
				switch (const uint32_t eventType = (event & 0x7C'00'00'00); eventType) {
					case 0b00000100'00000000'00000000'00000000:
						// COLUMN.
						processEvent(eColumn{event});
						break;

					case 0b00001100'00000000'00000000'00000000:
						// FRAME END.
						processEvent(eFrameEnd{event});
						break;

					case 0b00001000'00000000'00000000'00000000:
						// TIMESTAMP (reference or sub-unit).
						if (event & 0x00'80'00'00) {
							processEvent(eTimestampSubUnit{event});
						}
						else {
							processEvent(eTimestampReference{event});
						}
						break;

					case 0b00010000'00000000'00000000'00000000: // IMU data.
					case 0b00011000'00000000'00000000'00000000: // IMU timestamp (reference or sub-unit).
					default:
						mLogger(false, fmt::format("Invalid event received with code: {:#05b}.", (eventType >> 26)));
						break;
				}
			}
		}
	}

	void injectImu(dv::IMU imuData) {
		imuData.timestamp = getCurrentTimestamp();

		mBuffers.imu.elements.push_back(imuData);
	}

private:
	static_assert(std::endian::native == std::endian::little, "little-endian system required");
	static_assert(sizeof(void *) == sizeof(size_t), "sizes of pointers and size_t should be equal");

	static constexpr int16_t NUM_GROUPS{HEIGHT / 8};
	static constexpr size_t EVENT_GROUPS_FAST{96};
	static constexpr size_t EVENT_GROUPS_REPEAT{EVENT_GROUPS_FAST / sizeof(size_t)};

	struct {
		alignas(64) std::array<std::array<uint8_t, EVENT_GROUPS_FAST>, 2> columnEvents{};
		int16_t columnAddress{-1};
		uint8_t frameNumber{0};
		bool mirrorMode{false};
		std::array<bool, NUM_GROUPS> groupsUsed{};
		size_t resetIndex{0};
		size_t lastCommittedEvents{0};
	} mDvs;

	struct {
		int64_t reference{-1};
		int64_t referenceOverflow{0};
		int32_t lastReference{-1}; // -1 to ensure first ts packet cannot increase overflow.
	} mTimestamps;

	enum class State {
		WAIT_FOR_TIMESTAMP_REFERENCE,
		WAIT_FOR_TIMESTAMP_SUB_UNIT,
		WAIT_FOR_COLUMN_START,
		WAIT_FOR_SMGROUP,
		WAIT_FOR_COLUMN_OR_SMGROUP_OR_FRAME_END,
	};

	State mState{State::WAIT_FOR_TIMESTAMP_REFERENCE};

	// State Machine events are parsed event integers from sensor.
	struct eSMGroup {
		explicit eSMGroup(const uint32_t event) {
			group1Events = static_cast<uint8_t>(event >> 0);
			group2Events = static_cast<uint8_t>(event >> 8);

			group1Polarity = (((event >> 16) & 0x01) == 0); // ON polarity is 0 here.
			group2Polarity = (((event >> 17) & 0x01) == 0); // ON polarity is 0 here.

			group1Address = static_cast<int16_t>((event >> 18) & 0x00'7F);
			group2Offset  = static_cast<int16_t>((event >> 26) & 0x00'1F);

			dv::runtime_assert(
				[this] {
					return group1Address < NUM_GROUPS;
				},
				[this] {
					return fmt::format(
						"DVS: Group1 Y address out of range (0-{}): {}", (NUM_GROUPS - 1), group1Address);
				});
			dv::runtime_assert(
				[this] {
					return group2Offset < NUM_GROUPS;
				},
				[this] {
					return fmt::format("DVS: Group2 Y offset out of range (0-{}): {}", (NUM_GROUPS - 1), group2Offset);
				});
		}

		int16_t group1Address;
		int16_t group2Offset;
		uint8_t group1Events;
		uint8_t group2Events;
		bool group1Polarity;
		bool group2Polarity;
	};

	struct eColumn {
		explicit eColumn(const uint32_t event) {
			columnAddress = static_cast<int16_t>(event & 0x07'FF);
			frameNumber   = static_cast<uint8_t>((event >> 11) & 0x00'FF);
			startOfFrame  = (((event >> 21) & 0x01) == 1);
			mirrorMode    = (((event >> 23) & 0x01) == 1);

			dv::runtime_assert(
				[this] {
					return columnAddress < WIDTH;
				},
				[this] {
					return fmt::format("DVS: X address out of range (0-{}): {}", (WIDTH - 1), columnAddress);
				});
		}

		int16_t columnAddress;
		uint8_t frameNumber;
		bool startOfFrame;
		bool mirrorMode;
	};

	struct eFrameEnd {
		explicit eFrameEnd(const uint32_t event) {
			frameNumber = static_cast<uint8_t>((event >> 11) & 0x00'FF);
		}

		uint8_t frameNumber;
	};

	struct eTimestampReference {
		explicit eTimestampReference(const uint32_t event) {
			timestampReference = static_cast<int32_t>(event & 0x00'3F'FF'FF);
		}

		int32_t timestampReference;
	};

	struct eTimestampSubUnit {
		explicit eTimestampSubUnit(const uint32_t event) {
			timestampSubUnit = static_cast<int32_t>(event & 0x00'00'03'FF);
		}

		int32_t timestampSubUnit;
	};

	// State Machine Actions.
	void aUpdateTimestampReference(const eTimestampReference timestampRef) {
		// New reference timestamp is smaller, must have overflown its 22 bits.
		// Reference timestamps come in every millisecond; in theory if they all get lost in transit
		// and then we have wrap-around, we'd completely lose track of time with no way of detecting
		// it: this is extremely unlikely, as we'd have to lose a full four million consecutive such
		// messages which would take over one hour, where the general data loss would dominate.
		// UPDATE: Some cases have proven to repeat two timestampRef messages consecutively, the comparison
		// is strictly less, in some cases it can be the same value, which can be safely ignored.
		if (timestampRef.timestampReference < mTimestamps.lastReference) {
			mTimestamps.referenceOverflow++;

			// Produce information message for debug.
			mLogger(true, fmt::format("Reference timestamp overflow detected; new value is [{}], previous [{}]",
							  timestampRef.timestampReference, mTimestamps.lastReference));
		}

		// Generate full 64bit reference timestamp, with overflow added.
		mTimestamps.reference = ((mTimestamps.referenceOverflow << 22) + timestampRef.timestampReference);

		// In ms, convert to µs.
		mTimestamps.reference *= 1000;

		// Initialize timestamp tracking on startup.
		if (mTimestamps.lastReference == -1) [[unlikely]] {
			timestampInit(std::chrono::microseconds{mTimestamps.reference});
		}

		mTimestamps.lastReference = timestampRef.timestampReference;
	}

	void aUpdateTimestampSubUnit(const eTimestampSubUnit timestampSub) {
		mDvs.lastCommittedEvents = mBuffers.events.elements.size();

		// Implies a data commit.
		updateTimestamp(timestampSub.timestampSubUnit + mTimestamps.reference);

		mBuffers.events.elements.reserve(mDvs.lastCommittedEvents);
	}

	void aSetFrameNumber(const eColumn column) {
		mDvs.frameNumber = column.frameNumber;
		mDvs.resetIndex  = mBuffers.events.elements.size();
	}

	void aSetColumnData(const eColumn column) {
		mDvs.columnAddress = column.columnAddress;
		mDvs.mirrorMode    = column.mirrorMode;

		mDvs.groupsUsed.fill(false);

		mDvs.columnEvents[false].fill(0);
		mDvs.columnEvents[true].fill(0);
	}

	void bitIndexToOutput(std::vector<dv::Event> &dst, const size_t *src, const bool polarity) const noexcept {
		// Force unroll, works on supported GCC and clang.
		// GCC tends to not unroll without this, while clang does.
		// GCC also adds a useless popcount instruction, known bug: https://gcc.gnu.org/bugzilla/show_bug.cgi?id=90594
#pragma GCC unroll(EVENT_GROUPS_REPEAT)
		for (size_t i = 0; i < EVENT_GROUPS_REPEAT; ++i) {
			auto word        = src[i];
			const auto index = static_cast<int>(i * std::numeric_limits<size_t>::digits);

			while (word != 0) {
				dst.emplace_back(getCurrentTimestamp(), mDvs.columnAddress,
					static_cast<int16_t>(index + std::countr_zero(word)), polarity);

				word &= (word - 1);
			}
		}
	}

	void aTransformPreviousColumn() {
		{
			const auto *src = reinterpret_cast<const size_t *>(mDvs.columnEvents[false].data());

			bitIndexToOutput(mBuffers.events.elements, src, false);
		}

		{
			const auto *src = reinterpret_cast<const size_t *>(mDvs.columnEvents[true].data());

			bitIndexToOutput(mBuffers.events.elements, src, true);
		}
	}

	void aGenerateEventsFromSMGroup(const eSMGroup mGroup, const int16_t group2Address) {
		// MGROUP event.
		// Group addresses can happen out of order when MGROUP compression is enabled.
		mDvs.groupsUsed[mGroup.group1Address] = true;
		mDvs.groupsUsed[group2Address]        = true;

		if (mGroup.group1Events != 0) {
			mDvs.columnEvents[mGroup.group1Polarity][mGroup.group1Address] = mGroup.group1Events;
		}

		if (mGroup.group2Events != 0) {
			mDvs.columnEvents[mGroup.group2Polarity][group2Address] = mGroup.group2Events;
		}
	}

	void aResetEventFrame() {
		mBuffers.events.elements.resize(mDvs.resetIndex);
	}

	// State Machine Guards (return bool).
	[[nodiscard]] static bool gIsFrameStart(const eColumn column) {
		return column.startOfFrame;
	}

	[[nodiscard]] bool gIsSameFrameNumber(const eColumn column) const {
		return (column.frameNumber == mDvs.frameNumber);
	}

	[[nodiscard]] bool gVerifyFrameNumber(const eFrameEnd frameEnd) const {
		return (frameEnd.frameNumber == mDvs.frameNumber);
	}

	[[nodiscard]] bool gIsGroupParsedAlready(const eSMGroup mGroup, const int16_t group2Address) const {
		return (mDvs.groupsUsed[mGroup.group1Address] || mDvs.groupsUsed[group2Address]);
	}

	void processEvent([[maybe_unused]] const eDataLost event) {
		switch (mState) {
			default:
				mState = State::WAIT_FOR_TIMESTAMP_REFERENCE;
				break;

			case State::WAIT_FOR_SMGROUP:
			case State::WAIT_FOR_COLUMN_OR_SMGROUP_OR_FRAME_END:
				aResetEventFrame();
				mState = State::WAIT_FOR_TIMESTAMP_REFERENCE;
				break;
		}
	}

	void processEvent(const eTimestampReference event) {
		switch (mState) {
			default:
				mState = State::WAIT_FOR_TIMESTAMP_REFERENCE;
				break;

			case State::WAIT_FOR_TIMESTAMP_REFERENCE:
				aUpdateTimestampReference(event);
				mState = State::WAIT_FOR_TIMESTAMP_SUB_UNIT;
				break;

			case State::WAIT_FOR_SMGROUP:
			case State::WAIT_FOR_COLUMN_OR_SMGROUP_OR_FRAME_END:
				aResetEventFrame();
				mState = State::WAIT_FOR_TIMESTAMP_REFERENCE;
				break;
		}
	}

	void processEvent(const eTimestampSubUnit event) {
		switch (mState) {
			default:
				mState = State::WAIT_FOR_TIMESTAMP_REFERENCE;
				break;

			case State::WAIT_FOR_TIMESTAMP_SUB_UNIT:
				aUpdateTimestampSubUnit(event);
				mState = State::WAIT_FOR_COLUMN_START;
				break;

			case State::WAIT_FOR_SMGROUP:
			case State::WAIT_FOR_COLUMN_OR_SMGROUP_OR_FRAME_END:
				aResetEventFrame();
				mState = State::WAIT_FOR_TIMESTAMP_REFERENCE;
				break;
		}
	}

	void processEvent(const eColumn event) {
		switch (mState) {
			default:
				mState = State::WAIT_FOR_TIMESTAMP_REFERENCE;
				break;

			case State::WAIT_FOR_COLUMN_START:
				if (gIsFrameStart(event)) {
					aSetFrameNumber(event);
					aSetColumnData(event);
					mState = State::WAIT_FOR_SMGROUP;
					break;
				}

				mState = State::WAIT_FOR_TIMESTAMP_REFERENCE;
				break;

			case State::WAIT_FOR_SMGROUP:
				aResetEventFrame();
				mState = State::WAIT_FOR_TIMESTAMP_REFERENCE;
				break;

			case State::WAIT_FOR_COLUMN_OR_SMGROUP_OR_FRAME_END:
				if (gIsSameFrameNumber(event)) {
					aTransformPreviousColumn();
					aSetColumnData(event);
					mState = State::WAIT_FOR_SMGROUP;
					break;
				}

				aResetEventFrame();
				mState = State::WAIT_FOR_TIMESTAMP_REFERENCE;
				break;
		}
	}

	void processEvent(const eSMGroup event) {
		switch (mState) {
			default:
				mState = State::WAIT_FOR_TIMESTAMP_REFERENCE;
				break;

			case State::WAIT_FOR_SMGROUP:
			case State::WAIT_FOR_COLUMN_OR_SMGROUP_OR_FRAME_END:
				const int16_t group2Address = (mDvs.mirrorMode)
												? static_cast<int16_t>(event.group1Address - event.group2Offset)
												: static_cast<int16_t>(event.group1Address + event.group2Offset);

				dv::runtime_assert(
					[group2Address] {
						return (group2Address >= 0) && (group2Address < NUM_GROUPS);
					},
					[group2Address] {
						return fmt::format(
							"DVS: Group2 Y address out of range (0-{}): {}", (NUM_GROUPS - 1), group2Address);
					});

				if (!gIsGroupParsedAlready(event, group2Address)) {
					aGenerateEventsFromSMGroup(event, group2Address);
					mState = State::WAIT_FOR_COLUMN_OR_SMGROUP_OR_FRAME_END;
					break;
				}

				// DATA_LOST/ERROR: reset and fall back to start.
				aResetEventFrame();
				mState = State::WAIT_FOR_TIMESTAMP_REFERENCE;
				break;
		}
	}

	void processEvent(const eFrameEnd event) {
		switch (mState) {
			default:
				mState = State::WAIT_FOR_TIMESTAMP_REFERENCE;
				break;

			case State::WAIT_FOR_SMGROUP:
				aResetEventFrame();
				mState = State::WAIT_FOR_TIMESTAMP_REFERENCE;
				break;

			case State::WAIT_FOR_COLUMN_OR_SMGROUP_OR_FRAME_END:
				if (gVerifyFrameNumber(event)) {
					aTransformPreviousColumn();
					mState = State::WAIT_FOR_TIMESTAMP_REFERENCE;
					break;
				}

				aResetEventFrame();
				mState = State::WAIT_FOR_TIMESTAMP_REFERENCE;
				break;
		}
	}
};
} // namespace dv::io::camera::parser::S5KRC1S
