//
// Created by Thomas Debrunner on 2019-08-15.
//

#ifndef DV_RUNTIME_AEDAT2TOOLS_H
#define DV_RUNTIME_AEDAT2TOOLS_H

#include "../../include/dv-sdk/module_io.hpp"

#include "aedat2HeaderScraper.h"

#include <boost/nowide/fstream.hpp>
#include <opencv2/imgproc.hpp>

#include <thread>

class Aedat2Parser {
private:
	using EventOut   = dv::OutputVectorDataWrapper<dv::EventPacket, dv::Event>;
	using FrameOut   = dv::OutputDataWrapper<dv::Frame>;
	using IMUOut     = dv::OutputVectorDataWrapper<dv::IMUPacket, dv::IMU>;
	using TriggerOut = dv::OutputVectorDataWrapper<dv::TriggerPacket, dv::Trigger>;

	static constexpr int64_t MAX_HEADER_LINE_LENGTH   = 10000;
	static constexpr int64_t LEARN_EVENT_COUNT        = 10000;
	static constexpr int64_t PULSE_DURATION           = 10000;
	static constexpr uint32_t TIME_OVERFLOW_THRESHOLD = 900000000u;

	dv::Logger *log;

	boost::nowide::ifstream file;
	Aedat2HeaderScraper headerScraper;
	Aedat2Prefs parsePrefs;

	uint32_t lastTimestamp32   = 0;
	uint32_t timestampOverflow = 0;

	std::vector<int32_t> resetImageBuffer;
	cv::Mat mImageMat;
	int64_t mExposureStartTimestamp  = 0;
	uint64_t currentFrameResetCount  = 0;
	uint64_t currentFrameSignalCount = 0;

	dv::IMU currentIMUSample;
	int32_t imuDataCount        = 0;
	int64_t headerSize          = 0;
	bool headerSizeAccountedFor = false;

	int64_t invalidCoordinateCount = 0;
	int64_t invalidTimestampCount  = 0;

	/**
	 * Checks a line if it is a header line.
	 * @param line The line to check
	 * @return Returns true, if the given line is a header line. False otherwise
	 */
	bool isHeaderLine(const std::string &line) {
		// if first character is not a hashtag, it is definetely not a header line
		if (line[0] != '#') {
			return false;
		}
		// if it is a hashtag, it is very likely to be a header line when all characters are printable
		for (const char c : line) {
			if (!(isprint(c) || c == '\r' || c == '\n')) {
				return false;
			}
		}
		return true;
	}

	void parseHeader() {
		log->info("Parsing header");
		bool headerLine = false;
		int64_t lastLineStart;
		char lineBuffer[MAX_HEADER_LINE_LENGTH];
		do {
			lastLineStart = file.tellg();
			file.getline(lineBuffer, MAX_HEADER_LINE_LENGTH);
			std::string line{lineBuffer};

			headerLine = isHeaderLine(line);
			if (headerLine) {
				headerScraper.analyzeLine(line);
			}
		}
		while (headerLine);

		// clear the state if getline return -1,
		// when the readed line is longer than MAX_HEADER_LINE_LENGTH
		file.clear(file.rdstate() & ~std::istream::failbit);

		file.seekg(lastLineStart);

		parsePrefs = headerScraper.getPrefs();

		if (!parsePrefs.valid) {
			log->info("Could not obtain parse parameters from header. Trying to look at the data.");
			parsePrefs = learnDataFormat();
		}

		if (!parsePrefs.valid) {
			throw std::runtime_error("Could not determine parse parameters");
		}
	}

	/**
	 * Extracts four bytes from buff starting at offset and assembles an integer from it.
	 * Assumes buff is in big endian
	 * @param buff Big endian buffer
	 * @param offset The offset in buff to start decoding
	 * @return decoded integer
	 */
	static inline int32_t extractIntBigEndian(const char *buff, size_t offset) {
		return static_cast<int32_t>(((static_cast<uint32_t>(buff[offset + 0]) & 0xFF) << 24)
									| ((static_cast<uint32_t>(buff[offset + 1]) & 0xFF) << 16)
									| ((static_cast<uint32_t>(buff[offset + 2]) & 0xFF) << 8)
									| ((static_cast<uint32_t>(buff[offset + 3]) & 0xFF)));
	}

	/**
	 * Tries to establish the dimensions of the sensor that was used to record the data based
	 * on looking at some data samples and a list of known devices.
	 * @return The established configuration
	 */
	Aedat2Prefs learnDataFormat() {
		int64_t currentFilePos = file.tellg();
		std::array<char, 8 * LEARN_EVENT_COUNT> learnDataBuff{};
		file.read(learnDataBuff.data(), 8 * LEARN_EVENT_COUNT);
		size_t effectiveReadCount = file.gcount() / 8;
		std::vector<int32_t> addressData;
		addressData.resize(effectiveReadCount, 0);
		file.seekg(currentFilePos);

		// extract all the address data parts
		for (size_t i = 0; i < effectiveReadCount; i++) {
			addressData[i] = extractIntBigEndian(learnDataBuff.data(), 8 * i);
		}

		log->info("Testing for DVS128 aedat 2 format");
		// try to figure out parse format first
		// in case of DVS128 data, the middle 16 bit should always be zero.
		int32_t dvs128Aedat2ZeroTest = 0;
		for (int32_t address : addressData) {
			dvs128Aedat2ZeroTest |= (address & 0xFFFF0000);
		}
		// if all these bits were indeed zero, it is probably a dvs 128 in aedat2 format
		if (dvs128Aedat2ZeroTest == 0) {
			log->info("Data seems to be DVS128 data in aedat 2 format");
			return Aedat2Prefs::dvs128Pref();
		}

		// if not DVS128, it is probably a davis
		// try figuring out the maximum value for the X coordinate, to choose from a dimension of known devices
		log->info("Testing for DAVIS format");
		int16_t maxXValue = 0;
		int16_t minXValue = 0;
		int16_t maxYValue = 0;
		int16_t minYValue = 0;
		for (int32_t address : addressData) {
			if ((address >> 31) & 1) {
				// non-polarity data
				continue;
			}

			auto x = static_cast<int16_t>((address & 0x3FF000) >> 12);
			auto y = static_cast<int16_t>((address & 0x7F00) >> 8);

			maxXValue = std::max(x, maxXValue);
			maxYValue = std::max(y, maxYValue);
			minXValue = std::min(x, minXValue);
			minYValue = std::min(y, minYValue);
		}

		if (maxXValue < 0 || maxXValue >= 640 || minXValue < 0 || minXValue >= 640 || maxYValue < 0 || maxYValue >= 640
			|| minYValue < 0 || minYValue >= 640) {
			// Since there is no supported camera with more than 640 resolution,
			// this is either aedat1 data or an error
			// check for aedat 1
			bool isAedat1 = true;
			log->info("Testing for DVS128 aedat 1 format");
			for (size_t i = 0; i < LEARN_EVENT_COUNT; i++) {
				int32_t v = extractIntBigEndian(learnDataBuff.data(), 6 * i) & 0xFFFF;
				// check that first bit is always empty, except when trigger available, then coord empty
				isAedat1 = isAedat1 && (((v & 0xFFFE) == 0x8000) || ((v & 0x8000) == 0x00));
			}
			if (isAedat1) {
				log->info("Data seems to be DVS128 aedat 1 format");
				return Aedat2Prefs::dvs128Aedat1Pref();
			}
			// error
			log->error("Could not figure out data type. Maybe this is not aedat data?");
			return Aedat2Prefs::invalidPref();
		}
		if (maxXValue >= 346 || maxYValue >= 260) {
			log->info("Data seems to come from a DAVIS 640");
			return Aedat2Prefs::davisPref(640, 480, false, true, 0, 479);
		}
		if (maxXValue >= 240 || maxYValue >= 180) {
			log->info("Data seems to come from a DAVIS 346");
			return Aedat2Prefs::davisPref(346, 260, false, true, 0, 259);
		}
		if (maxXValue >= 128 || maxYValue >= 128) {
			log->info("Data seems to come from a DAVIS 240");
			return Aedat2Prefs::davisPref(240, 180, false, true, 0, 179);
		}
		log->info("Data seems to come from a DAVIS 128");
		return Aedat2Prefs::davisPref(128, 128, false, false, 0, 127);
	}

	inline void initOutFrame(FrameOut &frameOut) {
		resetImageBuffer.resize(parsePrefs.dataWidth * parsePrefs.dataHeight);
		mImageMat = cv::Mat{parsePrefs.dataHeight, parsePrefs.dataWidth, CV_8UC1};
	}

	inline void applyDVS128TriggerEvent(int64_t timestamp, int32_t address, TriggerOut &triggerOut) {
		bool polarity = (address & 0x1) > 0;
		dv::Trigger trigger;
		trigger.timestamp = timestamp;
		trigger.type      = polarity ? dv::TriggerType::EXTERNAL_SIGNAL_RISING_EDGE
									 : dv::TriggerType::EXTERNAL_GENERATOR_FALLING_EDGE;
		triggerOut << trigger;
	}

	inline void applyDVS128PolarityEvent(int64_t timestamp, int32_t address, EventOut &eventOut) {
		bool polarity   = (address & 0x1) > 0;
		auto x          = static_cast<int16_t>((address & 0xFE) >> 1);
		auto y_inverted = static_cast<int16_t>((address & 0x7F00) >> 8);
		auto y          = static_cast<int16_t>(parsePrefs.dataHeight - 1 - y_inverted);
		if (!(x < parsePrefs.dataWidth && y < parsePrefs.dataHeight && x >= 0 && y >= 0)) {
			invalidCoordinateCount += 1;
			log->warning.format("Invalid coordinates encountered: x = {:d}, y = {:d}. {:d} invalid coordinates so far.",
				x, y, invalidCoordinateCount);
			return;
		}

		eventOut << dv::Event(timestamp, x, y, polarity);
	}

	inline void applyDavisTriggerEvent(int64_t timestamp, int32_t address, TriggerOut &triggerOut) {
		bool polarity = (address & 0x800) > 0;
		dv::Trigger trigger;
		trigger.timestamp = timestamp;
		trigger.type      = polarity ? dv::TriggerType::EXTERNAL_SIGNAL_RISING_EDGE
									 : dv::TriggerType::EXTERNAL_GENERATOR_FALLING_EDGE;
		triggerOut << trigger;
	}

	inline void applyDavisPolarityEvent(int64_t timestamp, int32_t address, EventOut &eventOut) {
		bool polarity = (address & 0x800) > 0;
		auto y        = static_cast<int16_t>((address & 0x7FC00000) >> 22);
		auto x        = static_cast<int16_t>((address & 0x3FF000) >> 12);
		// always invert y
		y = static_cast<int16_t>(parsePrefs.dataHeight - 1 - y);
		x = parsePrefs.flipEventsX ? static_cast<int16_t>(parsePrefs.dataWidth - 1 - x) : x;
		if (!(x < parsePrefs.dataWidth && y < parsePrefs.dataHeight && x >= 0 && y >= 0)) {
			invalidCoordinateCount += 1;
			log->warning.format("Invalid coordinates encountered: x = {:d}, y = {:d}. {:d} invalid coordinates so far.",
				x, y, invalidCoordinateCount);
			return;
		}
		eventOut << dv::Event(timestamp, x, y, polarity);
	}

	inline void applyDavisAPSResetEvent(int64_t timestamp, int32_t address, FrameOut &frameOut) {
		auto y_inverted = static_cast<int16_t>((address & 0x7FC00000) >> 22);
		auto x          = static_cast<int16_t>((address & 0x3FF000) >> 12);
		auto y          = static_cast<int16_t>(parsePrefs.dataHeight - 1 - y_inverted);
		auto adc_value  = static_cast<int32_t>(address & 0x3FF);
		if (!(x < parsePrefs.dataWidth && y < parsePrefs.dataHeight && x >= 0 && y >= 0)) {
			invalidCoordinateCount += 1;
			log->warning.format("Invalid coordinates encountered: x = {:d}, y = {:d}. {:d} invalid coordinates so far.",
				x, y, invalidCoordinateCount);
			return;
		}

		mExposureStartTimestamp                            = timestamp;
		resetImageBuffer.at(y * parsePrefs.dataWidth + x)  = adc_value;
		currentFrameResetCount                            += 1;
	}

	inline void applyDavisAPSSignalEvent(int64_t timestamp, int32_t address, FrameOut &frameOut) {
		auto y_inverted  = static_cast<int16_t>((address & 0x7FC00000) >> 22);
		auto x           = static_cast<int16_t>((address & 0x3FF000) >> 12);
		auto y           = static_cast<int16_t>(parsePrefs.dataHeight - 1 - y_inverted);
		auto signalValue = static_cast<int32_t>(address & 0x3FF);
		if (!(x < parsePrefs.dataWidth && y < parsePrefs.dataHeight && x >= 0 && y >= 0)) {
			invalidCoordinateCount += 1;
			log->warning.format("Invalid coordinates encountered: x = {:d}, y = {:d}. {:d} invalid coordinates so far.",
				x, y, invalidCoordinateCount);
			return;
		}

		if (currentFrameSignalCount == 0) {
			frameOut.setTimestamp(mExposureStartTimestamp);
			frameOut.setExposure(dv::Duration{timestamp - mExposureStartTimestamp});
		}

		size_t index       = y * parsePrefs.dataWidth + x;
		int32_t resetValue = resetImageBuffer.at(index);

		int32_t pixelValue = 0;

		// 256 to better work with unchanged DAVIS240 values.
		if ((resetValue < 256) || (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.
			pixelValue = 1023;
		}
		else {
			// Do CDS.
			pixelValue = resetValue - signalValue;

			// Check for underflow.
			pixelValue = std::max(pixelValue, 0);

			// Check for overflow.
			pixelValue = std::min(pixelValue, 1023);
		}

		mImageMat.at<uint8_t>(y, x)  = static_cast<uint8_t>((pixelValue >> 2));
		currentFrameSignalCount     += 1;

		// if this signal event comes from the last pixel in readout, we assume frame complete
		if (y == parsePrefs.apsReadoutLastCoordY && x == parsePrefs.apsReadoutLastCoordX) {
			int64_t nPixels = static_cast<int64_t>(parsePrefs.dataHeight) * static_cast<int64_t>(parsePrefs.dataWidth);
			if (currentFrameSignalCount < nPixels || currentFrameResetCount < nPixels) {
				log->warning.format("Corrupted frame encountered. {:d} reset and {:d} signal events missing.",
					nPixels - currentFrameResetCount, nPixels - currentFrameSignalCount);
			}

			currentFrameSignalCount = 0;
			currentFrameResetCount  = 0;

			// only commit if all timestamps have been set and make sense. They could be corrupted in case
			// of corrupted frames
			if ((frameOut.timestampEndOfExposure() > parsePrefs.timeOffset)
				&& (frameOut.timestampStartOfExposure() > parsePrefs.timeOffset)) {
				if (parsePrefs.hasColor) {
					cv::Mat color;
					cv::cvtColor(mImageMat, color, cv::COLOR_BayerBG2BGR);
					frameOut.setMat(std::move(color));
				}
				else {
					frameOut.setMat(std::move(mImageMat));
				}

				frameOut.commit();

				initOutFrame(frameOut);
			}
			else {
				log->warning("Frame too corrupted to identify timestamps. Ignoring frame.");
			}
		}
	}

	inline void applyDavisIMUEvent(int64_t timestamp, int32_t address, IMUOut &imuOut) {
		int16_t sampleValue = ((address & 0xFFFF000) >> 12);

		int8_t imuSampleType        = ((address & 0x70000000) >> 28);
		currentIMUSample.timestamp  = timestamp;
		imuDataCount               += 1;
		switch (imuSampleType) {
			case 0:
				currentIMUSample.accelerometerX = static_cast<float>(sampleValue) / parsePrefs.accelScale;
				break;
			case 1:
				currentIMUSample.accelerometerY = static_cast<float>(sampleValue) / parsePrefs.accelScale;
				break;
			case 2:
				currentIMUSample.accelerometerZ = static_cast<float>(sampleValue) / parsePrefs.accelScale;
				break;
			case 3:
				currentIMUSample.temperature
					= (static_cast<float>(sampleValue) / static_cast<float>(340.0)) + static_cast<float>(35.0);
				break;
			case 4:
				currentIMUSample.gyroscopeX = static_cast<float>(sampleValue) / parsePrefs.gyroScale;
				break;
			case 5:
				currentIMUSample.gyroscopeY = static_cast<float>(sampleValue) / parsePrefs.gyroScale;
				break;
			case 6:
				currentIMUSample.gyroscopeZ = static_cast<float>(sampleValue) / parsePrefs.gyroScale;
				break;
		}

		if (imuDataCount >= 7) {
			imuOut << currentIMUSample;
			currentIMUSample = dv::IMU();
			imuDataCount     = 0;
		}
	}

	inline int64_t getTimestamp(uint32_t timestamp32, uint32_t timestampOverflow_) {
		return static_cast<int64_t>((static_cast<uint64_t>(timestampOverflow_) << 32u) | timestamp32)
			 + parsePrefs.timeOffset;
	}

	inline int64_t applyEvent(
		std::array<char, 8> &readBuff, EventOut &eventOut, FrameOut &frameOut, IMUOut &imuOut, TriggerOut &triggerOut) {
		// extract data and timestamp part
		int32_t address  = extractIntBigEndian(readBuff.data(), 0);
		auto timestamp32 = static_cast<uint32_t>(extractIntBigEndian(readBuff.data(), 4));

		// convert timestamp to 64bit
		if (timestamp32 < lastTimestamp32) {
			// timestamp went back in time, this is either wrong data (non-monotonic data) or an overflow
			// in time. If the differece between the times is big enough, we treat it as a wrap, otherwise as
			// wrong data
			uint32_t timeDiff32 = lastTimestamp32 - timestamp32;
			if (timeDiff32 > TIME_OVERFLOW_THRESHOLD) {
				timestampOverflow += 1;
			}
			else {
				// do not process event due to back in time.
				invalidTimestampCount += 1;
				log->warning.format("Non-monotonic timestamp encountered: -{:d} µs. {:d} invalid timestamps so far.",
					timeDiff32, invalidTimestampCount);
				return getTimestamp(lastTimestamp32, timestampOverflow);
			}
		}
		lastTimestamp32   = timestamp32;
		int64_t timestamp = getTimestamp(timestamp32, timestampOverflow);

		if (parsePrefs.aedat2Encoding == DAVIS) {
			// we are parsing DAVIS output

			if (((address >> 31) & 1)) {
				// davis imu or aps event
				int32_t typeInformation = (address & 0xC00) >> 10;
				if (typeInformation == 0) {
					// davis aps reset read event
					applyDavisAPSResetEvent(timestamp, address, frameOut);
				}
				else if (typeInformation == 1) {
					// davis aps signal read event
					applyDavisAPSSignalEvent(timestamp, address, frameOut);
				}
				else if (typeInformation == 3) {
					// davis imu event
					applyDavisIMUEvent(timestamp, address, imuOut);
				}
			}
			else {
				// davis trigger or polarity event
				if ((address & 0x400) > 0) {
					// davis trigger data
					applyDavisTriggerEvent(timestamp, address, triggerOut);
				}
				else {
					// davis event data
					applyDavisPolarityEvent(timestamp, address, eventOut);
				}
			}
		}
		else {
			// We are parsing DVS 128 output
			if ((address >> 15) & 1) {
				// dvs128 trigger event
				applyDVS128TriggerEvent(timestamp, address, triggerOut);
			}
			else {
				// dvs128 polarity event
				applyDVS128PolarityEvent(timestamp, address, eventOut);
			}
		}
		return timestamp;
	}

public:
	Aedat2Parser() = default;

	Aedat2Parser(const std::string &filePath, dv::Logger *log_) : log(log_), headerScraper(Aedat2HeaderScraper(log_)) {
		file = boost::nowide::ifstream(filePath, std::ios::binary);
		parseHeader();
		headerSize = file.tellg();
	}

	int64_t pulse(EventOut &eventOut, FrameOut &frameOut, IMUOut &imuOut, TriggerOut &triggerOut) {
		initOutFrame(frameOut);

		int64_t firstEventTime = -1;
		int64_t currentTime    = 0;
		// on first pulse, make sure we add the size of the header to the number of read bytes
		int64_t bytesRead      = headerSizeAccountedFor ? 0 : headerSize;
		headerSizeAccountedFor = true;
		while (currentTime < firstEventTime + PULSE_DURATION) {
			// read next event
			std::array<char, 8> readBuff{};
			if (parsePrefs.eventSize == Aedat2Prefs::AEDAT1_EVENT_SIZE) {
				file.read(readBuff.data() + 2, parsePrefs.eventSize);
			}
			else {
				file.read(readBuff.data(), parsePrefs.eventSize);
			}
			bytesRead += file.gcount();

			if (file.eof() || !file.good()) {
				// we've reached end of file. No point in trying to extract more data
				return bytesRead;
			}
			currentTime = applyEvent(readBuff, eventOut, frameOut, imuOut, triggerOut);

			if (firstEventTime < 0) {
				firstEventTime = currentTime;
			}
		}

		if (!eventOut.empty()) {
			eventOut.commit();
		}
		if (!imuOut.empty()) {
			imuOut.commit();
		}
		if (!triggerOut.empty()) {
			triggerOut.commit();
		}
		return bytesRead;
	}

	Aedat2Prefs getPrefs() {
		return parsePrefs;
	}
};

#endif // DV_RUNTIME_AEDAT2TOOLS_H
