#include "ynoise.hpp"

const char *Ynoise::initDescription() {
	return "Noise filter using the Yang algorithm.";
}

void Ynoise::initInputs(dv::InputDefinitionList &in) {
	in.addEventInput("events");
}

void Ynoise::initOutputs(dv::OutputDefinitionList &out) {
	out.addEventOutput("events");
}

void Ynoise::initConfigOptions(dv::RuntimeConfig &config) {
	config.add("deltaT", dv::ConfigOption::intOption("Time delta for filter.", 10000, 1, 1000000));
	config.add(
		"lParam", dv::ConfigOption::intOption("Value L of LxL matrix for density Matrix (odd number).", 3, 3, 15));
	config.add("threshold", dv::ConfigOption::intOption("Threshold value for density pixels.", 2, 0, 1000));

	config.setPriorityOptions({"deltaT", "lParam", "threshold"});
}

Ynoise::Ynoise() {
	auto input   = inputs.getEventInput("events");
	sizeX        = input.sizeX();
	sizeY        = input.sizeY();
	lParam       = static_cast<uint8_t>(config.getInt("lParam"));
	squareLParam = lParam * lParam;
	densityMatrix.resize(squareLParam);
	resetMatrix = densityMatrix;
	matrixMem.resize(sizeX * sizeY);
	regenerateDMLparam();
	outputs.getEventOutput("events").setup(inputs.getEventInput("events"));
}

void Ynoise::updateMatrix(const dv::Event &event) {
	auto address                 = event.x() * sizeY + event.y();
	matrixMem[address].polarity  = event.polarity();
	matrixMem[address].timestamp = static_cast<int32_t>(event.timestamp());
}

uint8_t Ynoise::calculateDensity(const dv::Event &event) {
	auto sub           = ((lParam - 1) / 2) - 1;
	auto addressX      = event.x() - sub;
	auto addressY      = event.y() - sub;
	auto timeToCompare = static_cast<uint32_t>(event.timestamp() - deltaT);
	auto polarity      = event.polarity();
	uint8_t lInfNorm{0}; // event density performed with l infinity norm instead of l1 norm
	uint8_t sum{0};

	if (addressX >= 0 && addressY >= 0) {
		for (uint32_t i = 0; i < squareLParam; i++) {
			uint32_t newAddressY = addressY + modLparam[i];
			uint32_t newAddressX = addressX + dividedLparam[i];
			if (newAddressX < sizeX && newAddressY < sizeY) {
				auto &matrixElem = matrixMem[newAddressX * sizeY + newAddressY];
				if (polarity == matrixElem.polarity) {
					if (timeToCompare < matrixElem.timestamp) {
						if (modLparam[i] == 0) {
							lInfNorm = std::max(lInfNorm, sum);
							sum      = 0;
						}
						sum++;
					}
				}
			}
		}
	}

	return lInfNorm;
}

void Ynoise::run() {
	auto inEvent  = inputs.getEventInput("events").events();
	auto outEvent = outputs.getEventOutput("events").events();

	if (!inEvent) {
		return;
	}

	for (auto &evt : inEvent) {
		// calculate density
		if (calculateDensity(evt) >= threshold) {
			outEvent << evt;
		}

		updateMatrix(evt);
	}
	outEvent << dv::commit;
}

// generation of array of i/lParam and i%lParam for performance optimisation
void Ynoise::regenerateDMLparam() {
	dividedLparam.resize(squareLParam);
	modLparam.resize(squareLParam);
	for (uint32_t i = 0; i < squareLParam; i++) {
		dividedLparam[i] = i / lParam;
		modLparam[i]     = i % lParam;
	}
}

void Ynoise::configUpdate() {
	deltaT    = config.getInt("deltaT");
	lParam    = static_cast<uint8_t>(config.getInt("lParam"));
	threshold = static_cast<uint8_t>(config.getInt("threshold"));
	if (lParam % 2 != 1) {
		lParam--;
		log.warning << "lParam must be odd, using value: " << lParam << std::endl;
	}
	squareLParam = lParam * lParam;
	densityMatrix.resize(squareLParam);
	resetMatrix.resize(squareLParam);
	regenerateDMLparam();
}
