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

#ifndef DV_RUNTIME_AEDAT2HEADERSCRAPE_H
#define DV_RUNTIME_AEDAT2HEADERSCRAPE_H

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

#include <boost/algorithm/string.hpp>

#include <regex>
#include <string>

enum Aedat2DataEncoding {
	DVS128,
	DAVIS
};

class Aedat2Prefs {
private:
	static constexpr float DAVIS_DEFAULT_ACCEL_SCALE = 4096;    // 8 G range
	static constexpr float DAVIS_DEFAULT_GYRO_SCALE  = 32.768F; // 1000 deg/s range

	Aedat2Prefs(bool valid_, Aedat2DataEncoding encoding_, int32_t dataWidth_, int32_t dataHeight_, bool hasColor_,
		float accelScale_, float gyroScale_, int64_t timeOffset_, const std::string &sourceInfo_, int32_t eventSize_,
		bool flipEventsX_, int32_t apsReadoutLastCoordX_, int32_t apsReadoutLastCoordY_) :
		valid(valid_),
		aedat2Encoding(encoding_),
		dataWidth(dataWidth_),
		dataHeight(dataHeight_),
		hasColor(hasColor_),
		accelScale(accelScale_),
		gyroScale(gyroScale_),
		timeOffset(timeOffset_),
		sourceInfo(sourceInfo_),
		eventSize(eventSize_),
		flipEventsX(flipEventsX_),
		apsReadoutLastCoordX(apsReadoutLastCoordX_),
		apsReadoutLastCoordY(apsReadoutLastCoordY_) {
	}

public:
	static constexpr int32_t AEDAT2_EVENT_SIZE = 8;
	static constexpr int32_t AEDAT1_EVENT_SIZE = 6;

	bool valid;
	Aedat2DataEncoding aedat2Encoding;
	int32_t dataWidth;
	int32_t dataHeight;
	bool hasColor;
	float accelScale;
	float gyroScale;
	int64_t timeOffset;
	std::string sourceInfo;
	int32_t eventSize;
	bool flipEventsX;
	int32_t apsReadoutLastCoordX;
	int32_t apsReadoutLastCoordY;

	Aedat2Prefs() = default;

	static Aedat2Prefs invalidPref() {
		return Aedat2Prefs{false, DAVIS, -1, -1, false, DAVIS_DEFAULT_ACCEL_SCALE, DAVIS_DEFAULT_GYRO_SCALE, -1, "",
			AEDAT2_EVENT_SIZE, false, -1, -1};
	}

	static Aedat2Prefs davisPref(int32_t dataWidth, int32_t dataHeight, bool color, bool flipEventsX,
		int32_t apsReadoutLastCoordX, int32_t apsReadoutLastCoordY) {
		return Aedat2Prefs{true, DAVIS, dataWidth, dataHeight, color, DAVIS_DEFAULT_ACCEL_SCALE,
			DAVIS_DEFAULT_GYRO_SCALE, 0, "DAVIS", AEDAT2_EVENT_SIZE, flipEventsX, apsReadoutLastCoordX,
			apsReadoutLastCoordY};
	}

	static Aedat2Prefs dvs128Pref() {
		return Aedat2Prefs{true, DVS128, 128, 128, false, -1, -1, 0, "DVS128", AEDAT2_EVENT_SIZE, false, -1, -1};
	}

	static Aedat2Prefs dvs128Aedat1Pref() {
		return Aedat2Prefs{true, DVS128, 128, 128, false, -1, -1, 0, "DVS128", AEDAT1_EVENT_SIZE, false, -1, -1};
	}
};

class Aedat2HeaderScraper {
private:
	enum class TimeOffsetEvidenceQuality {
		UNKNOWN               = 0,
		CREATION_TIME_FIELD   = 1,
		DATA_START_TIME_FIELD = 2
	};

	enum class IMUScaleEvidenceQuality {
		DEFAULT_VALUES      = 1,
		GLOBAL_VALUE        = 2,
		CHIP_SPECIFIC_VALUE = 3
	};

	std::regex regex_hardwareInterface;
	std::regex regex_aeChip;
	std::regex regex_creationTime;
	std::regex regex_dataStartTime;
	std::regex regex_imuAccelGlobal;
	std::regex regex_imuGyroGlobal;
	std::regex regex_imuAccelChip;
	std::regex regex_imuGyroChip;

	Aedat2Prefs currentPrefs;

	TimeOffsetEvidenceQuality timeOffsetQuality;
	IMUScaleEvidenceQuality accelScaleQuality;
	IMUScaleEvidenceQuality gyroScaleQuality;

	dv::Logger *log;

	std::string currentChipIdentifier;

	inline static bool contains(const std::string &haystack, const std::string &needle) {
		return haystack.find(needle) != std::string::npos;
	}

	void applySettingsForChip() {
		if (contains(currentChipIdentifier, "DAVIS640") || contains(currentChipIdentifier, "DAVISRGBW640")) {
			currentPrefs.dataWidth            = 640;
			currentPrefs.dataHeight           = 480;
			currentPrefs.aedat2Encoding       = DAVIS;
			currentPrefs.valid                = true;
			currentPrefs.flipEventsX          = true;
			currentPrefs.apsReadoutLastCoordX = 0;
			currentPrefs.apsReadoutLastCoordY = 479;
			log->info("Identified camera in header as a DAVIS 640");
		}
		if (contains(currentChipIdentifier, "DAVIS346")) {
			currentPrefs.dataWidth            = 346;
			currentPrefs.dataHeight           = 260;
			currentPrefs.aedat2Encoding       = DAVIS;
			currentPrefs.valid                = true;
			currentPrefs.flipEventsX          = true;
			currentPrefs.apsReadoutLastCoordX = 0;
			currentPrefs.apsReadoutLastCoordY = 259;
			log->info("Identified camera in header as a DAVIS 346");
		}
		if (contains(currentChipIdentifier, "DAVIS240")) {
			currentPrefs.dataWidth            = 240;
			currentPrefs.dataHeight           = 180;
			currentPrefs.aedat2Encoding       = DAVIS;
			currentPrefs.valid                = true;
			currentPrefs.flipEventsX          = true;
			currentPrefs.apsReadoutLastCoordX = 0;
			currentPrefs.apsReadoutLastCoordY = 179;
			log->info("Identified camera in header as a DAVIS 240");
		}
		if (contains(currentChipIdentifier, "DAVIS208")) {
			currentPrefs.dataWidth            = 208;
			currentPrefs.dataHeight           = 192;
			currentPrefs.aedat2Encoding       = DAVIS;
			currentPrefs.valid                = true;
			currentPrefs.flipEventsX          = false;
			currentPrefs.apsReadoutLastCoordX = 0;
			currentPrefs.apsReadoutLastCoordY = 191;
			log->info("Identified camera in header as a DAVIS 208");
		}
		if (contains(currentChipIdentifier, "DAVIS128")) {
			currentPrefs.dataWidth            = 128;
			currentPrefs.dataHeight           = 128;
			currentPrefs.aedat2Encoding       = DAVIS;
			currentPrefs.valid                = true;
			currentPrefs.flipEventsX          = false;
			currentPrefs.apsReadoutLastCoordX = 0;
			currentPrefs.apsReadoutLastCoordY = 127;
			log->info("Identified camera in header as a DAVIS 128");
		}
		if (contains(currentChipIdentifier, "DVS128")) {
			currentPrefs.dataWidth      = 128;
			currentPrefs.dataHeight     = 128;
			currentPrefs.aedat2Encoding = DVS128;
			currentPrefs.valid          = true;
			log->info("Identified camera in header as a DVS 128");
		}
		if (contains(currentChipIdentifier, "COLOR") || contains(currentChipIdentifier, "RGB")) {
			currentPrefs.hasColor = true;
			log->info("The camera is a color camera according to header");
		}
		if ((contains(currentChipIdentifier, "COCHLEA"))) {
			throw std::runtime_error("Cochlea chip is not supported");
		}
	}

	static inline float calculateIMUAccelScale(int32_t imuAccelScale) {
		// Accelerometer scale is:
		// +-2 g - 16384 LSB/g
		// +-4 g - 8192 LSB/g
		// +-8 g - 4096 LSB/g
		// +-16 g - 2048 LSB/g
		float accelScale = 32768.0F / static_cast<float>(imuAccelScale);
		return (accelScale);
	}

	static inline float calculateIMUGyroScale(int32_t imuGyroScale) {
		// Gyroscope scale is:
		// +-250 °/s - 131 LSB/°/s
		// +-500 °/s - 65.5 LSB/°/s
		// +-1000 °/s - 32.8 LSB/°/s
		// +-2000 °/s - 16.4 LSB/°/s
		float gyroScale = 32768.0F / static_cast<float>(imuGyroScale);
		return (gyroScale);
	}

public:
	Aedat2HeaderScraper(dv::Logger *log_) :
		regex_hardwareInterface(std::regex("^# HardwareInterface: (.*)$")),
		regex_aeChip(std::regex("^# AEChip: .*\\.([A-Za-z0-9]+)$")),
		regex_creationTime(std::regex("^# Creation time: System\\.currentTimeMillis\\(\\) ([0-9]+)$")),
		regex_dataStartTime(std::regex("^# DataStartTime: System\\.currentTimeMillis\\(\\) ([0-9]+)$")),
		regex_imuAccelGlobal(std::regex("^# *<entry key=\"ImuAccelScale\" value=\"ImuAccelScaleG([0-9]+)\"\\/>$")),
		regex_imuGyroGlobal(
			std::regex("^# *<entry key=\"ImuGyroScale\" value=\"GyroFullScaleDegPerSec([0-9]+)\"\\/>$")),
		regex_imuAccelChip(
			std::regex("^# *<entry key=\"([A-Za-z0-9]+)\\.IMU\\.AccelScale\" value=\"ImuAccelScaleG([0-9]+)\"\\/>$")),
		regex_imuGyroChip(std::regex(
			"^# *<entry key=\"([A-Za-z0-9]+)\\.IMU\\.GyroScale\" value=\"GyroFullScaleDegPerSec([0-9]+)\"\\/>$")),
		currentPrefs(Aedat2Prefs::invalidPref()),
		timeOffsetQuality(TimeOffsetEvidenceQuality::UNKNOWN),
		accelScaleQuality(IMUScaleEvidenceQuality::DEFAULT_VALUES),
		gyroScaleQuality(IMUScaleEvidenceQuality::DEFAULT_VALUES),
		log(log_) {
	}

	void analyzeLine(const std::string &line_) {
		std::string line = boost::trim_copy(line_);
		std::smatch matches;
		if (std::regex_search(line, matches, regex_hardwareInterface)) {
			if (matches.size() < 2) {
				return;
			}
			currentPrefs.sourceInfo = matches[1];
			log->debug.format("Hardware interface extracted as \"{:s}\"", currentPrefs.sourceInfo);
		}
		if (std::regex_search(line, matches, regex_aeChip)) {
			if (matches.size() < 2) {
				return;
			}
			std::string chipIdentifier = matches[1];
			currentChipIdentifier      = boost::to_upper_copy(chipIdentifier);
			applySettingsForChip();
			log->debug.format("Chip identifier extracted as \"{:s}\"", chipIdentifier);
		}
		if (std::regex_search(line, matches, regex_creationTime)) {
			if (timeOffsetQuality > TimeOffsetEvidenceQuality::CREATION_TIME_FIELD || matches.size() < 2) {
				// we already have better evidence for the time. No need to consider
				return;
			}
			currentPrefs.timeOffset = std::stoll(matches[1]) * 1000;
			timeOffsetQuality       = TimeOffsetEvidenceQuality::CREATION_TIME_FIELD;
			log->debug.format(
				"Time offset extracted as \"{:d}\", quality: CREATION_TIME_FIELD", currentPrefs.timeOffset);
		}
		if (std::regex_search(line, matches, regex_dataStartTime)) {
			if (timeOffsetQuality > TimeOffsetEvidenceQuality::DATA_START_TIME_FIELD || matches.size() < 2) {
				// we already have better evidence for the time. No need to consider
				return;
			}
			currentPrefs.timeOffset = std::stoll(matches[1]) * 1000;
			timeOffsetQuality       = TimeOffsetEvidenceQuality::DATA_START_TIME_FIELD;
			log->debug.format(
				"Time offset extracted as \"{:d}\", quality: DATA_START_TIME_FIELD", currentPrefs.timeOffset);
		}
		if (std::regex_search(line, matches, regex_imuAccelGlobal)) {
			if (accelScaleQuality > IMUScaleEvidenceQuality::GLOBAL_VALUE || matches.size() < 2) {
				// we already have better evidence for the time. No need to consider
				return;
			}
			currentPrefs.accelScale = calculateIMUAccelScale(std::stoi(matches[1]));
			accelScaleQuality       = IMUScaleEvidenceQuality::GLOBAL_VALUE;
			log->debug.format("Accel scale extracted as \"{:.10f}\", quality: GLOBAL_VALUE", currentPrefs.accelScale);
		}
		if (std::regex_search(line, matches, regex_imuGyroGlobal)) {
			if (gyroScaleQuality > IMUScaleEvidenceQuality::GLOBAL_VALUE || matches.size() < 2) {
				// we already have better evidence for the time. No need to consider
				return;
			}
			currentPrefs.gyroScale = calculateIMUGyroScale(std::stoi(matches[1]));
			gyroScaleQuality       = IMUScaleEvidenceQuality::GLOBAL_VALUE;
			log->debug.format("Gyro scale extracted as \"{:.10f}\", quality: GLOBAL_VALUE", currentPrefs.gyroScale);
		}
		if (std::regex_search(line, matches, regex_imuAccelChip)) {
			if (accelScaleQuality > IMUScaleEvidenceQuality::CHIP_SPECIFIC_VALUE || matches.size() < 3) {
				// we already have better evidence for the time. No need to consider
				return;
			}
			std::string chipIdentifier = matches[1];
			boost::to_upper(chipIdentifier);
			if (chipIdentifier != currentChipIdentifier) {
				//     wrong chip class. still better to use global or default.
				return;
			}

			currentPrefs.accelScale = calculateIMUAccelScale(std::stoi(matches[2]));
			accelScaleQuality       = IMUScaleEvidenceQuality::CHIP_SPECIFIC_VALUE;
			log->debug.format(
				"Accel scale extracted as \"{:.10f}\", quality: CHIP_SPECIFIC_VALUE", currentPrefs.accelScale);
		}
		if (std::regex_search(line, matches, regex_imuGyroChip)) {
			if (gyroScaleQuality > IMUScaleEvidenceQuality::CHIP_SPECIFIC_VALUE || matches.size() < 3) {
				// we already have better evidence for the time. No need to consider
				return;
			}
			std::string chipIdentifier = matches[1];
			boost::to_upper(chipIdentifier);
			if (chipIdentifier != currentChipIdentifier) {
				// wrong chip class. still better to use global or default.
				return;
			}
			currentPrefs.gyroScale = calculateIMUGyroScale(std::stoi(matches[2]));
			gyroScaleQuality       = IMUScaleEvidenceQuality::CHIP_SPECIFIC_VALUE;
			log->debug.format(
				"Gyro scale extracted as \"{:.10f}\", quality: CHIP_SPECIFIC_VALUE", currentPrefs.gyroScale);
		}
	}

	Aedat2Prefs getPrefs() {
		return currentPrefs;
	}
};

#endif // DV_RUNTIME_AEDAT2HEADERSCRAPE_H
