#include "aedat3Parser.hpp"

#include "../../include/dv-sdk/cross/portable_endian.h"

#include <boost/date_time/local_time/local_time.hpp>

#include <iomanip>
#include <regex>
#include <string>
#include <time.h>

int Aedat3Parser::parse_aedat3_header() {
	// We expect that the full header part is contained within
	// this one data buffer.
	// File headers are part of the AEDAT 3.X specification.
	// Start with #, go until '\r\n' (Windows EOL). First must be
	// version header !AER-DATx.y, last must be end-of-header
	// marker with !END-HEADER (AEDAT 3.1 only).

	bool endHeader    = false;
	bool formatHeader = false;
	bool sourceHeader = false;

	char cline[1024];
	file.getline(cline, 1024); // skip first line because we have paresed version already
	while (!endHeader) {
		auto pos = file.tellg();
		file.getline(cline, 1024);
		std::string line{cline};
		if (line.empty() || line[0] != '#') {
			file.seekg(pos); // reset to beginning of line
			endHeader = true;
		}
		else {
			if (!formatHeader) {
				// Then the format header. Only with AEDAT 3.X.
				char formatString[1024 + 1];

				if (sscanf(line.c_str(), "#Format: %1024s\r\n", formatString) == 1) {
					formatHeader = true;

					// Parse format string to format ID.
					// We support either only RAW, or a mixture of the various compression
					// modes.
					if (strcmp(formatString, "RAW") == 0) {
						formatID = 0x00;
					}
					else {
						formatID = 0x00;

						if (strstr(formatString, "SerializedTS") != NULL) {
							formatID |= 0x01;
						}

						if (strstr(formatString, "PNGFrames") != NULL) {
							formatID |= 0x02;
						}

						if (!formatID) {
							// No valid format found.
							log->error("No valid format ID found.");
							throw std::runtime_error("No valid format ID found.");
						}
					}
				}
				else {
					log->error.format("No valid format string found. Line is: {:s}", line);
					throw std::runtime_error("No valid format string found.");
				}
			}
			else if (!sourceHeader) {
				// Then the source header. Only with AEDAT 3.X. We only support one active
				// source.
				char sourceString[1024 + 1];

				if (sscanf(line.c_str(), "#Source %" SCNi16 ": %1024[^\r]s\n", &sourceID, sourceString) == 2) {
					sourceHeader = true;
					if (strcmp(sourceString, "DVS128") == 0) {
						log->info.format("Source is: {:s}", sourceString);
						eventSizeX = eventSizeY = 128;
					}
					else if (strcmp(sourceString, "DAVIS240A") == 0 || strcmp(sourceString, "DAVIS240B") == 0
							 || strcmp(sourceString, "DAVIS240C") == 0) {
						log->info.format("Source is: {:s}", sourceString);
						eventSizeX = apsSizeX = 240;
						eventSizeY = apsSizeY = 180;
					}
					else if (strcmp(sourceString, "DAVIS128") == 0) {
						log->info.format("Source is: {:s}", sourceString);
						eventSizeX = apsSizeX = eventSizeY = apsSizeY = 128;
					}
					else if (strcmp(sourceString, "DAVIS346A") == 0 || strcmp(sourceString, "DAVIS346B") == 0
							 || strcmp(sourceString, "DAVIS346Cbsi") == 0 || strcmp(sourceString, "DAVIS346") == 0
							 || strcmp(sourceString, "DAVIS346bsi") == 0) {
						log->info.format("Source is: {:s}", sourceString);
						eventSizeX = apsSizeX = 346;
						eventSizeY = apsSizeY = 260;
					}
					else if (strcmp(sourceString, "DAVIS640") == 0) {
						log->info.format("Source is: {:s}", sourceString);
						eventSizeX = apsSizeX = 640;
						eventSizeY = apsSizeY = 480;
					}
					else if (strcmp(sourceString, "DAVISHet640") == 0 || strcmp(sourceString, "DAVIS640het") == 0) {
						log->info.format("Source is: {:s}", sourceString);
						eventSizeX = 320;
						eventSizeY = 240;
						apsSizeX   = 640;
						apsSizeY   = 480;
					}
					else if (strcmp(sourceString, "DAVIS208") == 0) {
						log->info.format("Source is: {:s}", sourceString);
						eventSizeX = apsSizeX = 208;
						eventSizeY = apsSizeY = 192;
					}
					else if (strcmp(sourceString, "DYNAPSE") == 0) {
						log->info.format("Source is: {:s}", sourceString);
					}
					else if (strncmp(sourceString, "File,", 5) == 0) {
						log->info.format("Source is: {:s}", sourceString);
						sscanf(sourceString + 5, "dvsSizeX=%zu,dvsSizeY=%zu,apsSizeX=%zu,apsSizeY=%zu", &eventSizeX,
							&eventSizeY, &apsSizeX, &apsSizeY);
					}
					else if (strncmp(sourceString, "Network,", 8) == 0) {
						log->info.format("Source is: {:s}", sourceString);
						sscanf(sourceString + 8, "dvsSizeX=%zu,dvsSizeY=%zu,apsSizeX=%zu,apsSizeY=%zu", &eventSizeX,
							&eventSizeY, &apsSizeX, &apsSizeY);
					}
					else if (strncmp(sourceString, "Processor,", 10) == 0) {
						log->info.format("Source is: {:s}", sourceString);
						sscanf(sourceString + 10, "dvsSizeX=%zu,dvsSizeY=%zu,apsSizeX=%zu,apsSizeY=%zu", &eventSizeX,
							&eventSizeY, &apsSizeX, &apsSizeY);
					}
					else {
						log->error("No valid source found.");
						throw std::runtime_error("No valid source found.");
					}
				}
				else {
					log->error("No valid device name found. Format could not be identified");
					throw std::runtime_error("No valid format found.");
				}
			}
			else {
				// Now we either have other header lines with AEDAT 2.0/AEDAT 3.X, or
				// the END-HEADER with AEDAT 3.1. We check this before the other possible,
				// because it terminates the AEDAT 3.1 header, so we stop in that case.
				// Then other headers, like Start-Time.
				if (strncmp(line.c_str(), "#Start-Time: ", 13) == 0) {
					if (strncmp(line.c_str(), "#Start-Time: ", 13) == 0) {
						char startTimeString[1024 + 1];
						auto tmp = sscanf(line.c_str(), "#Start-Time: %1024[^\r]s\n", startTimeString);
						if (tmp == 1) {
							std::tm tm{}; // Initialize all to zero.

							// Use libc strptime(). tm_isdst is not set. tm_gmtoff is set
							// on glibc with %z, but that doesn't work on Windows.
							const auto remain = strptime(startTimeString, "%Y-%m-%d %H:%M:%S", &tm);

							// So we parse the time-zone part of the string manually.
							const auto gmtoffset = parseTimezonePortable(remain);

							// The struct tm represents the time from the string, which was
							// originally local time. Now if we want to convert back to an
							// offset from the UNIX UTC epoch, we'd use timelocal()/mktime(), but
							// those functions are influenced by the time-zone settings of the
							// host we are currently running on. We already parsed our correct
							// UTC offset in 'gmtoffset'. So we use timegm() to convert as if
							// this was an UTC time in the string, which is not adjusted in any
							// way for the local time-zone.
							int64_t tt = timegm(&tm);

							// Then we remove the 'gmtoffset' value (UTC offset), so we transform
							// the local time back into UTC.
							tt -= gmtoffset;

							// If time offset is valid (not negative), convert to microseconds and use.
							if (tt >= 0) {
								startingTS = tt * 1000000;
								log->info.format("Start time is: {:d}", startingTS);
							}
						}
					}
				}
				else if (strncmp(line.c_str(), "#-Source ", 9) == 0) {
				}
			}
		}
	}

	return file.tellg();
	// Parsed AEDAT 3.0 header successfully.
}

int Aedat3Parser::get_data(dv::OutputVectorDataWrapper<dv::EventPacket, dv::Event> &eventOut,
	dv::OutputDataWrapper<dv::Frame> &frameOut, dv::OutputVectorDataWrapper<dv::IMUPacket, dv::IMU> &imuOut,
	dv::OutputVectorDataWrapper<dv::TriggerPacket, dv::Trigger> &triggerOut) {
	char header[28];
	int bytesRead = 0;
	if (file.read(header, 28)) {
		bytesRead         += 28;
		int16_t eventType  = I16T(le16toh(U16T(Aedat3PacketHeader_(header)->eventType)));
		switch (eventType) {
			case 0: // special event
				bytesRead += parse_specialEvent_packet(header, triggerOut);
				break;
			case 1: // polarity_event
				bytesRead += parse_polarityEvent_packet(header, eventOut);
				break;
			case 2: // frame_event
				bytesRead += parse_frameEvent_packet(header, frameOut);
				break;
			case 3: // imu6_event
				bytesRead += parse_imu6Event_packet(header, imuOut);
				break;
			case 4: // imu9_event
				log->error("IMU 9 Event not supported.");
				break;
			case 12: // spike_event
				log->error("Event type Spike not supported.");
				break;
			default:
				log->error("Wrong event type in packet header, could not parse packet.");
				break;
		}
	}
	return bytesRead;
}

int Aedat3Parser::parse_specialEvent_packet(
	char *header, dv::OutputVectorDataWrapper<dv::TriggerPacket, dv::Trigger> &triggerOut) {
	auto header_ = reinterpret_cast<Aedat3PacketHeader_>(header); // cast header into specific eventPacket header

	// packet header specifics
	int32_t eventNumber     = I32T(le32toh(U32T(header_->eventNumber)));
	int32_t eventSize       = I32T(le32toh(U32T(header_->eventSize)));
	int32_t eventTSOverflow = I32T(le32toh(U32T(header_->eventTSOverflow)));
	char data[eventSize];
	dv::Trigger trig;

	for (size_t i = 0; i < eventNumber; i++) {
		file.read(data, eventSize);
		auto special_ = reinterpret_cast<SpecialEvent_>(data);

		trig.timestamp = (I64T(U64T(eventTSOverflow)) << 31) | U64T(I32T(le32toh(U32T(special_->timestamp))));
		switch (SpecialEventTypes(U8T((le32toh(special_->data) >> 1) & 0x0000007F))) {
			case SpecialEventTypes::TIMESTAMP_WRAP:
				break;
			case SpecialEventTypes::TIMESTAMP_RESET:
				trig.type = dv::TriggerType::TIMESTAMP_RESET;
				break;
			case SpecialEventTypes::EXTERNAL_INPUT_RISING_EDGE:
				trig.type = dv::TriggerType::EXTERNAL_SIGNAL_RISING_EDGE;
				break;
			case SpecialEventTypes::EXTERNAL_INPUT_FALLING_EDGE:
				trig.type = dv::TriggerType::EXTERNAL_SIGNAL_FALLING_EDGE;
				break;
			case SpecialEventTypes::EXTERNAL_INPUT_PULSE:
				trig.type = dv::TriggerType::EXTERNAL_SIGNAL_PULSE;
				break;
			case SpecialEventTypes::DVS_ROW_ONLY:
				break;
			case SpecialEventTypes::EXTERNAL_INPUT1_RISING_EDGE:
				break;
			case SpecialEventTypes::EXTERNAL_INPUT1_FALLING_EDGE:
				break;
			case SpecialEventTypes::EXTERNAL_INPUT1_PULSE:
				break;
			case SpecialEventTypes::EXTERNAL_INPUT2_RISING_EDGE:
				break;
			case SpecialEventTypes::EXTERNAL_INPUT2_FALLING_EDGE:
				break;
			case SpecialEventTypes::EXTERNAL_INPUT2_PULSE:
				break;
			case SpecialEventTypes::EXTERNAL_GENERATOR_RISING_EDGE:
				trig.type = dv::TriggerType::EXTERNAL_GENERATOR_RISING_EDGE;
				break;
			case SpecialEventTypes::EXTERNAL_GENERATOR_FALLING_EDGE:
				trig.type = dv::TriggerType::EXTERNAL_GENERATOR_FALLING_EDGE;
				break;
			case SpecialEventTypes::APS_FRAME_START:
				trig.type = dv::TriggerType::APS_FRAME_START;
				break;
			case SpecialEventTypes::APS_FRAME_END:
				trig.type = dv::TriggerType::APS_FRAME_END;
				break;
			case SpecialEventTypes::APS_EXPOSURE_START:
				trig.type = dv::TriggerType::APS_EXPOSURE_START;
				break;
			case SpecialEventTypes::APS_EXPOSURE_END:
				trig.type = dv::TriggerType::APS_EXPOSURE_END;
				break;
		}
		triggerOut.push_back(trig);
	}
	triggerOut.commit();
	return eventNumber * eventSize;
}

int Aedat3Parser::parse_polarityEvent_packet(
	char *header, dv::OutputVectorDataWrapper<dv::EventPacket, dv::Event> &eventOut) {
	auto header_ = reinterpret_cast<Aedat3PacketHeader_>(header); // cast header into specific eventPacket header
	// packet header specifics
	int32_t eventNumber     = I32T(le32toh(U32T(header_->eventNumber)));
	int32_t eventSize       = I32T(le32toh(U32T(header_->eventSize)));
	int32_t eventTSOverflow = I32T(le32toh(U32T(header_->eventTSOverflow)));
	char data[eventSize];
	// event variables
	int64_t timestamp;
	bool polarity;
	int16_t x, y;

	size_t sizeY = eventSizeY;

	for (size_t i = 0; i < eventNumber; i++) {
		file.read(data, eventSize);
		const PolarityEventConst_ event_ = reinterpret_cast<PolarityEventConst_>(data);
		if (*((const uint8_t *) data) & 0x00000001) {
			timestamp = I64T((U64T(eventTSOverflow) << 31) | U64T(I32T(le32toh(U32T(event_->timestamp)))));
			polarity  = (le32toh(event_->data) >> 1) & 0x00000001;
			x         = U16T((le32toh(event_->data) >> 17) & 0x00007FFF);
			y         = sizeY - 1 - U16T((le32toh(event_->data) >> 2) & 0x00007FFF);
			eventOut.push_back(dv::Event(timestamp + startingTS, x, y, polarity));
		}
	}
	eventOut.commit();
	return eventNumber * eventSize;
}

int Aedat3Parser::parse_frameEvent_packet(char *header, dv::OutputDataWrapper<dv::Frame> &frameOut) {
	auto header_ = Aedat3PacketHeader_(header); // cast header into specific eventPacket header
	// packet header specifics
	int32_t eventNumber     = I32T(le32toh(U32T(header_->eventNumber)));
	int32_t eventSize       = I32T(le32toh(U32T(header_->eventSize)));
	int32_t eventTSOverflow = I32T(le32toh(U32T(header_->eventTSOverflow)));
	char data[eventSize];

	for (size_t i = 0; i < eventNumber; i++) {
		file.read(data, eventSize);

		auto frame_ = reinterpret_cast<FrameEvent_>(data);

		int64_t TSStartOfExposure
			= (I64T((U64T(eventTSOverflow)) << 31) | U64T(I32T(le32toh(U32T(frame_->ts_startexposure)))));
		int64_t TSEndOfExposure
			= (I64T((U64T(eventTSOverflow)) << 31) | U64T(I32T(le32toh(U32T(frame_->ts_endexposure)))));

		int32_t sizeX = I32T(le32toh(U32T(frame_->lengthX)));
		int32_t sizeY = I32T(le32toh(U32T(frame_->lengthY)));

		int32_t posX = I32T(le32toh(U32T(frame_->positionX)));
		int32_t posY = I32T(le32toh(U32T(frame_->positionY)));

		frameOut.setTimestamp(TSStartOfExposure);
		frameOut.setExposure(dv::Duration{TSEndOfExposure - TSStartOfExposure});
		frameOut.setSource(dv::FrameSource::UNDEFINED);
		frameOut.setPosition(posX, posY);

		// Default: 16 bits, 1 channel, grayscale to 8 bits grayscale.
		cv::Mat oldFrame{sizeY, sizeX, CV_16UC1, const_cast<uint16_t *>(frame_->pixels)};
		cv::Mat newFrame{sizeY, sizeX, CV_8UC1};

		oldFrame.convertTo(newFrame, CV_8U, (1.0 / 256.0), 0);
		// No color format conversion needed here.

		// Flip the Y axis (AEDAT 3.0 difference).
		cv::Mat newFrameFlipped;
		cv::flip(newFrame, newFrameFlipped, 0);

		frameOut.setMat(std::move(newFrameFlipped));

		frameOut.commit();
	}
	return eventSize * eventNumber;
}

int Aedat3Parser::parse_imu6Event_packet(char *header, dv::OutputVectorDataWrapper<dv::IMUPacket, dv::IMU> &imuOut) {
	auto header_ = reinterpret_cast<Aedat3PacketHeader_>(header); // cast header into specific eventPacket header
	// packet header specifics
	int32_t eventNumber     = I32T(le32toh(U32T(header_->eventNumber)));
	int32_t eventSize       = I32T(le32toh(U32T(header_->eventSize)));
	int32_t eventTSOverflow = I32T(le32toh(U32T(header_->eventTSOverflow)));
	char data[eventSize];

	bool isValid;
	float accelX, accelY, accelZ, gyroX, gyroY, gyroZ, temp;
	int64_t timestamp;
	dv::IMU imu;

	for (size_t i = 0; i < eventNumber; i++) {
		file.read(data, eventSize);
		auto imu6_ = reinterpret_cast<IMU6Event_>(data);
		isValid    = (le32toh(imu6_->info) >> 0) & 0x00000001;
		if (isValid) {
			timestamp = (I64T(U64T(eventTSOverflow)) << 31) | U64T(I32T(le32toh(U32T(imu6_->timestamp))));

			accelX = leflttoh(imu6_->accel_x);
			accelY = leflttoh(imu6_->accel_y);
			accelZ = leflttoh(imu6_->accel_z);

			gyroX = leflttoh(imu6_->gyro_x);
			gyroY = leflttoh(imu6_->gyro_y);
			gyroZ = leflttoh(imu6_->gyro_z);

			temp = leflttoh(imu6_->temp);

			imu.timestamp      = timestamp;
			imu.accelerometerX = accelX;
			imu.accelerometerY = accelY;
			imu.accelerometerZ = accelZ;
			imu.gyroscopeX     = gyroX;
			imu.gyroscopeY     = gyroY;
			imu.gyroscopeZ     = gyroZ;
			imu.temperature    = temp;

			imuOut.push_back(imu);
		}
	}
	imuOut.commit();
	return eventNumber * eventSize;
}
