#include "knoise.hpp"

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

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

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

void Knoise::initConfigOptions(dv::RuntimeConfig &config) {
	config.add("deltaT", dv::ConfigOption::intOption("Time delta for filter.", 1000, 1, 1000000));
	config.add("supporters", dv::ConfigOption::intOption("Number of supporting pixels.", 1, 0, 10));

	config.setPriorityOptions({"deltaT", "supporters"});
}

Knoise::Knoise() {
	sizeX = inputs.getEventInput("events").sizeX();
	sizeY = inputs.getEventInput("events").sizeY();
	xCols = std::vector<k_mem_cell>(sizeX);
	yRows = std::vector<k_mem_cell>(sizeY);
	outputs.getEventOutput("events").setup(inputs.getEventInput("events"));
}

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

	if (!inEvent) {
		return;
	}

	for (auto &evt : inEvent) {
		uint16_t xAddr    = evt.x();
		uint16_t yAddr    = evt.y();
		bool polarity     = evt.polarity();
		int64_t timestamp = evt.timestamp();
		size_t support    = 0;

		bool xAddrMinusOne = (xAddr > 0);
		bool xAddrPlusOne  = (xAddr < (sizeX - 1));
		bool yAddrMinusOne = (yAddr > 0);
		bool yAddrPlusOne  = (yAddr < (sizeY - 1));

		if (xAddrMinusOne) {
			struct k_mem_cell &xPrevCell = xCols[xAddr - 1];
			if ((timestamp - xPrevCell.timestamp) <= deltaT && xPrevCell.polarity == polarity) {
				if ((yAddrMinusOne && (xPrevCell.otherAddr == (yAddr - 1))) || (xPrevCell.otherAddr == yAddr)
					|| (yAddrPlusOne && (xPrevCell.otherAddr == (yAddr + 1)))) {
					support++;
				}
			}
		}

		struct k_mem_cell &xCell = xCols[xAddr];
		if ((timestamp - xCell.timestamp) <= deltaT && xCell.polarity == polarity) {
			if ((yAddrMinusOne && (xCell.otherAddr == (yAddr - 1)))
				|| (yAddrPlusOne && (xCell.otherAddr == (yAddr + 1)))) {
				support++;
			}
		}

		if (xAddrPlusOne) {
			struct k_mem_cell &xNextCell = xCols[xAddr + 1];
			if ((timestamp - xNextCell.timestamp) <= deltaT && xNextCell.polarity == polarity) {
				if ((yAddrMinusOne && (xNextCell.otherAddr == (yAddr - 1))) || (xNextCell.otherAddr == yAddr)
					|| (yAddrPlusOne && (xNextCell.otherAddr == (yAddr + 1)))) {
					support++;
				}
			}
		}

		if (yAddrMinusOne) {
			struct k_mem_cell &yPrevCell = yRows[yAddr - 1];
			if ((timestamp - yPrevCell.timestamp) <= deltaT && yPrevCell.polarity == polarity) {
				if ((xAddrMinusOne && (yPrevCell.otherAddr == (xAddr - 1))) || (yPrevCell.otherAddr == xAddr)
					|| (xAddrPlusOne && (yPrevCell.otherAddr == (xAddr + 1)))) {
					support++;
				}
			}
		}

		struct k_mem_cell &yCell = yRows[yAddr];
		if ((timestamp - yCell.timestamp) <= deltaT && yCell.polarity == polarity) {
			if ((xAddrMinusOne && (yCell.otherAddr == (xAddr - 1)))
				|| (xAddrPlusOne && (yCell.otherAddr == (xAddr + 1)))) {
				support++;
			}
		}

		if (yAddrPlusOne) {
			struct k_mem_cell &yNextCell = yRows[yAddr + 1];
			if ((timestamp - yNextCell.timestamp) <= deltaT && yNextCell.polarity == polarity) {
				if ((xAddrMinusOne && (yNextCell.otherAddr == (xAddr - 1))) || (yNextCell.otherAddr == xAddr)
					|| (xAddrPlusOne && (yNextCell.otherAddr == (xAddr + 1)))) {
					support++;
				}
			}
		}

		if (support >= supporters) {
			outEvent << evt;
			xCell.passed = true;
			yCell.passed = true;
		}
		else {
			xCell.passed = false;
			yCell.passed = false;
		}

		// Update maps.
		xCell.timestamp = timestamp;
		xCell.polarity  = polarity;
		xCell.otherAddr = yAddr;

		yCell.timestamp = timestamp;
		yCell.polarity  = polarity;
		yCell.otherAddr = xAddr;
	}
	outEvent << dv::commit;
}

void Knoise::configUpdate() {
	deltaT     = config.getInt("deltaT");
	supporters = config.getInt("supporters");
}
