diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5bee39c..fb20032 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -17,7 +17,7 @@ set(PORT_SPEED "57600" CACHE STRING "Serial Port Speed")
 set(PROGRAMMER "stk500v1" CACHE STRING "Programmer Type")
 set(COMPILE_FLAGS "" CACHE STRING "Additional Compiler Flags")
 
-set(SRC_FILES  main.cpp serial.cpp  train.cpp item.cpp turnout.cpp signal.cpp)
+set(SRC_FILES  main.cpp serial.cpp  train.cpp item.cpp turnout.cpp signal.cpp softspim.cpp nfcbord.cpp mfrc522.cpp)
 
 # Compiler suite specification
 set(CMAKE_C_COMPILER /usr/bin/avr-gcc)
@@ -29,7 +29,7 @@ set(CMAKE_LINKER /usr/bin/avr-ld)
 
 # Compiler flags
 add_definitions(-mmcu=${MCU} -DF_CPU=${CPU_SPEED})
-add_definitions(-s -c -g -Os -Wall -std=c++17 )
+add_definitions(-s -c -g -O2 -Wall -std=c++17 )
 add_definitions(-fno-exceptions -ffunction-sections -fdata-sections)
 
 # Linker flags
diff --git a/inputshiftreg.h b/inputshiftreg.h
new file mode 100644
index 0000000..925b818
--- /dev/null
+++ b/inputshiftreg.h
@@ -0,0 +1,75 @@
+#pragma once
+
+template <int BITS> class InputShiftReg
+{
+private:
+	static constexpr int BYTES = (BITS % 8 == 0) ? (BITS/8) : (BITS/8+1);
+
+	static constexpr bool invert = true;
+	static constexpr bool invertInput = false;
+
+	volatile unsigned char *_port;
+	volatile unsigned char *_pin;
+	const unsigned char _pinSerOut;
+	const unsigned char _pinClk;
+	const unsigned char _pinParallelLoad;
+	
+	unsigned char data[BYTES];
+
+public:
+
+	InputShiftReg(volatile unsigned char* const portReg, volatile unsigned char* const pinReg,
+		const unsigned char clk, const unsigned char serOut, const unsigned char parallelLoad):
+		_port(portReg), _pin(pinReg), _pinSerOut(serOut), _pinClk(clk), _pinParallelLoad(parallelLoad)
+	{
+	}
+	bool getBit(unsigned char bit)
+	{
+		if(BITS <= bit)
+			return false;
+		bit = BITS - bit - 1;
+		return data[bit/8] & (1<<(bit%8));
+	}
+	unsigned char* read()
+	{
+		if constexpr(invert)
+		{
+			*_port &= ~(1<<_pinParallelLoad);
+			*_port |= (1<<_pinParallelLoad);
+			*_port &= ~(1<<_pinParallelLoad);
+
+			for(unsigned char i = 0; i < BITS; ++i)
+			{
+				*_port &= ~(1 << _pinClk);
+				bool value = *_pin & (1 << _pinSerOut);
+				if constexpr(invertInput)
+					value = !value;
+				if(value)
+					data[i/8] |= (1<<(i%8));
+				else
+					data[i/8] &= ~(1<<(i%8));
+				*_port |= (1 << _pinClk);
+			}
+		}
+		else
+		{
+			*_port |= (1<<_pinParallelLoad);
+			*_port &= ~(1<<_pinParallelLoad);
+			*_port |= (1<<_pinParallelLoad);
+
+			for(unsigned char i = 0; i < BITS; ++i)
+			{
+				*_port |= (1 << _pinClk);
+				bool value = *_pin & (1 << _pinSerOut);
+				if constexpr(invertInput)
+					value = !value;
+				if(value)
+					data[i/8] |= (1<<(i%8));
+				else
+					data[i/8] &= ~(1<<(i%8));
+				*_port &= ~(1 << _pinClk);
+			}
+		}
+		return data;
+	}
+};
diff --git a/main.cpp b/main.cpp
index dd1629c..17aa44f 100644
--- a/main.cpp
+++ b/main.cpp
@@ -12,19 +12,17 @@
 #include "staticvector.h"
 #include "turnout.h"
 #include "signal.h"
-
-#define COMMAND_BUFFER_SIZE 64
-#define SNPRINTF_BUFFER_SIZE 128
-#define EPPROM_SIZE 1024
+#include "shiftreg.h"
+#include "softspim.h"
+#include "nfcbord.h"
+#include "defines.h"
 
 char buffer[SNPRINTF_BUFFER_SIZE];
 
-static constexpr uint8_t EEPROM_RESERVE = 32;
-static constexpr uint8_t BLOCK = 4;
-
 SVector<Train, 32> trains;
 SVector<Turnout, 32> turnouts;
 SVector<Signal, 32> signals;
+NfcBoard nfcBoard;
 
 bool autoff = true;
 bool powerIsOn = true;
@@ -252,6 +250,12 @@ void serialDispatch(Serial* serial)
 				if(token != NULL)
 					ret = signalDispatch(token, serial);
 			}
+			else if(strcmp(token, "nfc") == 0)
+			{
+				token = strtok(NULL, " ");
+				if(token != NULL)
+					ret = nfcBoard.dispatch(token, serial);
+			}
 			else if(strncmp(token, "erase", 4) == 0)
 			{
 				for(uint16_t i = 0; i < EPPROM_SIZE; i++) EEPROM_write_char(i, 0);
@@ -327,7 +331,7 @@ int main()
 	TCNT0 = 0;
 	TCCR0B = (1<<CS02); // run timer0 with /256 scaler
 
-	DDRD =  (1 << PD2) | (1 << PD3) | (1 << PD4) | (1 << PD5);
+	DDRD = (1 << PD2) | (1 << PD3) | (1 << PD4) | (1 << PD5);
 
 	restore_state();
 
diff --git a/mfrc522.cpp b/mfrc522.cpp
new file mode 100644
index 0000000..ea46055
--- /dev/null
+++ b/mfrc522.cpp
@@ -0,0 +1,450 @@
+#include "mfrc522.h"
+
+
+
+uint8_t Mfrc522::read(uint8_t addr)
+{
+	_csReg->setBit(_csPin, false);
+	_spi->readWrite((addr << 1) | (1 << 7));
+	uint8_t res = _spi->readWrite();
+	_csReg->setBit(_csPin, true);
+	return res;
+}
+
+void Mfrc522::read(uint8_t addr, uint8_t* data, uint8_t datalen, uint8_t rxAlign)
+{
+	_csReg->setBit(_csPin, false);
+	_spi->readWrite(addr << 1 | (1 << 7));
+	for(uint8_t i = 0; i < datalen; ++i)
+	{
+		if(i == 0 && rxAlign)
+		{
+			uint8_t mask = 0;
+			for (uint8_t j = rxAlign; j <= 7; ++j)
+				mask |= (1 << j);
+			uint8_t value = _spi->readWrite();
+			data[0] = (data[0] & ~mask) | (value & mask);
+		}
+		else
+		{
+			data[i] = _spi->readWrite();
+		}
+	}
+	_csReg->setBit(_csPin, true);
+}
+
+void Mfrc522::write(uint8_t addr, uint8_t data)
+{
+	_csReg->setBit(_csPin, false);
+	_spi->readWrite(addr << 1);
+	_spi->readWrite(data);
+	_csReg->setBit(_csPin, true);
+}
+
+void Mfrc522::write(uint8_t addr, uint8_t* data, uint8_t datalen)
+{
+	_csReg->setBit(_csPin, false);
+	_spi->readWrite(addr << 1);
+	_spi->readWrite(datalen, nullptr, data);
+	_csReg->setBit(_csPin, true);
+}
+void Mfrc522::updateBit(uint8_t addr, uint8_t bit, bool value)
+{
+	uint8_t old = read(addr);
+	write(addr, value ? old | (1 << bit) : old & ~(1 << bit));
+}
+
+void (*_tagEnterdCb)(Mfrc522*, void*);
+void* _userData;
+
+Mfrc522::Mfrc522(SpiMaster* spi, ShiftReg<NFC_PORTS>*  csReg, uint8_t csPin,
+	void (*tagEnterdCb)(Mfrc522*, void*), void* userData):
+	_csReg(csReg), _spi(spi), _csPin(csPin), _tagEnterdCb(tagEnterdCb), _userData(userData)
+{
+	write(CommandReg, SOFTRESET);
+	_delay_ms(100);
+
+	write(TModeReg, 0x80);
+	write(TPrescalerReg, 0xA9);
+	write(TReloadRegH, 0x03);
+	write(TReloadRegL, 0xE8);
+	write(ModWidthReg, 0x26);
+
+	write(RFCfgReg, 0b111 << 4); //set gain to 48dB
+	
+	write(TxAutoReg, 0x40); // Default 0x00. Force a 100 % ASK modulation independent of the ModGsPReg register setting
+	write(ModeReg, 0x3D); // Default 0x3F. Set the preset value for the CRC coprocessor for the CalcCRC command to 0x6363 (ISO 14443-3 part 6.2.4)
+	//write(DivIEnReg, 0b10010000); // enable MfinActIrq as push-pull
+	//write(ComIEnReg, 0b00100000); // enable Rx irq
+
+	setRf(true);
+}
+
+uint8_t Mfrc522::calculateCrc(uint8_t *data, uint8_t length, uint16_t *result)
+{
+	write(CommandReg, IDLE);		// Stop any active command.
+	write(DivIrqReg, 0x04);				// Clear the CRCIRq interrupt request bit
+	updateBit(FIFOLevelReg, 7, true);		// FlushBuffer = 1, FIFO initialization
+	write(FIFODataReg, data, length);	// Write data to the FIFO
+	write(CommandReg, CALCCRC);		// Start the calculation
+	
+	// Wait for the CRC calculation to complete.
+	uint16_t i = 5000;
+	while(!(read(DivIrqReg) & 0x04) && i != 0)
+		--i;
+	
+	if(i == 0)
+		return ERR;
+	write(CommandReg, IDLE);
+	
+	*result = read(CRCResultRegL);
+	*result = read(CRCResultRegH) << 8;
+	return 0;
+}
+
+uint8_t Mfrc522::commuicateWithTag(uint8_t command, uint8_t waitIrq,
+	uint8_t *sendData, uint8_t sendLen,
+	uint8_t *recvData, uint8_t *recvLen,
+	uint8_t validBits, uint8_t rxAlign,
+	uint8_t *rxValidBits)
+{
+	write(CommandReg, IDLE);
+	write(ComIrqReg, 0b01111111); // clear irqs
+	write(FIFOLevelReg, 1 << 7);			// Flush fifo Buffer;
+	write(FIFODataReg, sendData, sendLen);	 // Fill fifo
+	write(BitFramingReg, (rxAlign << 4) + validBits);
+	write(CommandReg, command);				// Execute the command
+	if (command == TRANSCEIVE)
+		updateBit(BitFramingReg, 7, true);
+
+	uint16_t i = 2000;
+	uint8_t irq = read(ComIrqReg);
+	while(irq & waitIrq)
+	{
+		irq = read(ComIrqReg);
+		if(irq & 0x01 || --i == 0)
+		{
+			if(serial)
+				serial->write_p(PSTR("timeout\n"));
+			return TIMEOUT;
+		}
+	}
+
+	uint8_t errorRegValue = read(ErrorReg);
+	if (errorRegValue & 0b00010011) // BufferOvfl ParityErr ProtocolErr
+	{
+		if(serial)
+			serial->write_p(PSTR("BufferOvfl ParityErr ProtocolErr\n"));
+		return ERR;
+	}
+
+	if (recvData && recvLen)
+	{
+		uint8_t fifoBites = read(FIFOLevelReg);
+		
+		if(serial)
+		{
+			serial->write_p(PSTR("fifo has "));
+			serial->write((int)fifoBites);
+			serial->write_p(PSTR(" bytes\n"));
+		}
+		if(fifoBites > *recvLen)
+			return LEN;
+		*recvLen = fifoBites;
+		read(FIFODataReg, recvData, fifoBites, rxAlign);
+		if(rxValidBits)
+			*rxValidBits = read(ControlReg) & 0x07;
+	}
+	
+	if(errorRegValue & 0x08)
+	{
+		if(serial)
+			serial->write_p(PSTR("collision err\n"));
+		return COLLISION;
+	}
+	return 0;
+}
+
+uint8_t Mfrc522::transceive(uint8_t *sendData, uint8_t sendLen, uint8_t *recvData, uint8_t *recvLen,
+					uint8_t validBits, uint8_t rxAlign, uint8_t *rxValidBits)
+{
+	uint8_t waitIRq = 0x30; // RxIRq and IdleIRq
+	return commuicateWithTag(TRANSCEIVE, waitIRq, sendData, sendLen, recvData,
+								recvLen, validBits, rxAlign, rxValidBits);
+}
+
+uint8_t Mfrc522::wakeupTag(uint8_t* bufferATQA,	uint8_t *bufferLen)
+{
+	if(*bufferLen < 2)
+		return ERR;
+
+	updateBit(CollReg, 7, false);
+	uint8_t data = PICC_CMD_WUPA;
+	uint8_t ret = transceive(&data, 1, bufferATQA, bufferLen, 7);
+	
+	if(*bufferLen != 2)
+		return ERR;
+
+	return ret;
+}
+
+uint8_t Mfrc522::selectTag(Uid *uid)
+{
+	bool uidComplete;
+	bool selectDone;
+	uint8_t cascadeLevel = 0;
+	uint8_t result;
+	uint8_t count;
+	uint8_t checkBit;
+	uint8_t index;
+	uint8_t uidIndex;					// The first index in uid->uidByte[] that is used in the current Cascade Level.
+	int8_t currentLevelKnownBits;		// The number of known UID bits in the current Cascade Level.
+	uint8_t buffer[9];					// The SELECT/ANTICOLLISION commands uses a 7 uint8_t standard frame + 2 uint8_ts CRC_A
+	uint8_t bufferUsed;				// The number of uint8_ts used in the buffer, ie the number of uint8_ts to transfer to the FIFO
+	uint8_t txLastBits;				// Used in BitFramingReg. The number of valid bits in the last transmitted uint8_t. 
+	uint8_t *responseBuffer;
+	uint8_t responseLength;
+	
+	if(serial)
+			serial->write_p(PSTR("Select\n"));
+	
+	// Description of buffer structure:
+	//		Byte 0: SEL 				Indicates the Cascade Level: PICC_CMD_SEL_CL1, PICC_CMD_SEL_CL2 or PICC_CMD_SEL_CL3
+	//		Byte 1: NVB					Number of Valid Bits (in complete command, not just the UID): High nibble: complete bytes, Low nibble: Extra bits. 
+	//		Byte 2: UID-data or CT		See explanation below. CT means Cascade Tag.
+	//		Byte 3: UID-data
+	//		Byte 4: UID-data
+	//		Byte 5: UID-data
+	//		Byte 6: BCC					Block Check Character - XOR of bytes 2-5
+	//		Byte 7: CRC_A
+	//		Byte 8: CRC_A
+	// The BCC and CRC_A is only transmitted if we know all the UID bits of the current Cascade Level.
+	//
+	// Description of bytes 2-5: (Section 6.5.4 of the ISO/IEC 14443-3 draft: UID contents and cascade levels)
+	//		UID size	Cascade level	Byte2	Byte3	Byte4	Byte5
+	//		========	=============	=====	=====	=====	=====
+	//		 4 bytes		1			uid0	uid1	uid2	uid3
+	//		 7 bytes		1			CT		uid0	uid1	uid2
+	//						2			uid3	uid4	uid5	uid6
+	//		10 bytes		1			CT		uid0	uid1	uid2
+	//						2			CT		uid3	uid4	uid5
+	//						3			uid6	uid7	uid8	uid9
+
+	// Prepare MFRC522
+	updateBit(CollReg, 7, false);	// ValuesAfterColl=1 => Bits received after collision are cleared.
+
+	// Repeat Cascade Level loop until we have a complete UID.
+	uidComplete = false;
+	currentLevelKnownBits = 0;
+	while(!uidComplete)
+	{
+		// Set the Cascade Level in the SEL byte, find out if we need to use the Cascade Tag in byte 2.
+		switch(cascadeLevel)
+		{
+			case 0:
+				buffer[0] = PICC_CMD_SEL_CL1;
+				uidIndex = 0;
+				break;
+			
+			case 1:
+				buffer[0] = PICC_CMD_SEL_CL2;
+				uidIndex = 3;
+				break;
+			
+			case 2:
+				buffer[0] = PICC_CMD_SEL_CL3;
+				uidIndex = 6;
+				break;
+			
+			default:
+				if(serial)
+					serial->write_p(PSTR("err cascadeLevel\n"));
+				return ERR;
+				break;
+		}
+
+		// Copy the known bits from uid->uidByte[] to buffer[]
+		index = 2; // destination index in buffer[]
+		// The number of bytes needed to represent the known bits for this level.
+		uint8_t bytesToCopy = currentLevelKnownBits / 8 + (currentLevelKnownBits % 8 ? 1 : 0); 
+		if(bytesToCopy)
+		{
+			// Max 4 bytes in each Cascade Level. Only 3 left if we use the Cascade Tag
+			uint8_t maxBytes = 4; 
+			if (bytesToCopy > maxBytes)
+				bytesToCopy = maxBytes;
+			for (count = 0; count < bytesToCopy; count++)
+				buffer[index++] = uid->uidByte[uidIndex + count];
+		}
+		
+		// Repeat anti collision loop until we can transmit all UID bits + BCC and receive a SAK - max 32 iterations.
+		selectDone = false;
+		while (!selectDone)
+		{
+			// Find out how many bits and bytes to send and receive.
+			if (currentLevelKnownBits >= 32) // All UID bits in this Cascade Level are known. This is a SELECT.
+			{ 
+				buffer[1] = 0x70; // NVB - Number of Valid Bits: Seven whole bytes
+				// Calculate BCC - Block Check Character
+				buffer[6] = buffer[2] ^ buffer[3] ^ buffer[4] ^ buffer[5];
+				// Calculate CRC_A
+				result = calculateCrc(buffer, 7, reinterpret_cast<uint16_t*>(&buffer[7]));
+				if (result != 0)
+				{
+					if(serial)
+						serial->write_p(PSTR("err calculateCrc\n"));
+					return result;
+				}
+				txLastBits		= 0; // 0 => All 8 bits are valid.
+				bufferUsed		= 9;
+				// Store response in the last 3 bytes of buffer (BCC and CRC_A - not needed after tx)
+				responseBuffer	= &buffer[6];
+				responseLength	= 3;
+			}
+			else // This is an ANTICOLLISION.
+			{ 
+				txLastBits		= currentLevelKnownBits % 8;
+				count			= currentLevelKnownBits / 8;	// Number of whole bytes in the UID part.
+				index			= 2 + count;					// Number of whole bytes: SEL + NVB + UIDs
+				buffer[1]		= (index << 4) + txLastBits;	// NVB - Number of Valid Bits
+				bufferUsed		= index + (txLastBits ? 1 : 0);
+				// Store response in the unused part of buffer
+				responseBuffer	= &buffer[index];
+				responseLength	= sizeof(buffer) - index;
+			}
+
+			// Set bit adjustments
+			uint8_t rxAlign = txLastBits;
+			// RxAlign = BitFramingReg[6..4]. TxLastBits = BitFramingReg[2..0]
+			write(BitFramingReg, (rxAlign << 4) + txLastBits);
+			
+			if(serial)
+			{
+				serial->write_p(PSTR("entering transceive "));
+				serial->write((int)responseLength);
+				serial->putChar(' ');
+				serial->write((int)txLastBits);
+				serial->putChar(' ');
+				serial->write((int)currentLevelKnownBits);
+				serial->putChar('\n');
+			}
+
+			// Transmit the buffer and receive the response.
+			result = transceive(buffer, bufferUsed, responseBuffer, &responseLength, txLastBits, rxAlign, &txLastBits);			
+			if (result == COLLISION) // More than one PICC in the field => collision.
+			{
+				result = read(CollReg); // CollReg[7..0] bits are: ValuesAfterColl reserved CollPosNotValid CollPos[4:0]
+				if (result & 0x20)
+				{
+					if(serial)
+						serial->write_p(PSTR("err collision\n"));
+					return COLLISION; // Without a valid collision position we cannot continue
+				}
+				uint8_t collisionPos = result & 0x1F; // Values 0-31, 0 means bit 32.
+				if (collisionPos == 0) {
+					collisionPos = 32;
+				}
+				if (collisionPos <= currentLevelKnownBits) // No progress - should not happen 
+				{ 
+					if(serial)
+						serial->write_p(PSTR("err No progress\n"));
+					return ERR;
+				}
+				// Choose the PICC with the bit set.
+				currentLevelKnownBits	= collisionPos;
+				count			= currentLevelKnownBits % 8; // The bit to modify
+				checkBit		= (currentLevelKnownBits - 1) % 8;
+				index			= 1 + (currentLevelKnownBits / 8) + (count ? 1 : 0); // First byte is index 0.
+				buffer[index]	|= (1 << checkBit);
+			}
+			else if (result != 0)
+			{
+				if(serial)
+					serial->write_p(PSTR("err transceive\n"));
+				return result;
+			}
+			else 
+			{
+				if (currentLevelKnownBits >= 32) // This was a SELECT.
+					selectDone = true;
+				else // This was an ANTICOLLISION.  Run loop again to do the SELECT.
+					currentLevelKnownBits = 32;
+			}
+		}
+
+		// We do not check the CBB - it was constructed by us above.
+		
+		// Copy the found UID bytes from buffer[] to uid->uidByte[]
+		index			= (buffer[2] == PICC_CMD_CT) ? 3 : 2; // source index in buffer[]
+		bytesToCopy		= (buffer[2] == PICC_CMD_CT) ? 3 : 4;
+		for (count = 0; count < bytesToCopy; count++)
+		{
+			uid->uidByte[uidIndex + count] = buffer[index++];
+		}
+		
+		// Check response SAK (Select Acknowledge)
+		if (responseLength != 3 || txLastBits != 0) // SAK must be exactly 24 bits (1 byte + CRC_A).
+		{	
+			if(serial)
+			{
+				serial->write_p(PSTR("err SAK "));
+				serial->write((int)responseLength);
+				serial->putChar(' ');
+				serial->write((int)txLastBits);
+				serial->putChar('\n');
+			}
+			return ERR;
+		}
+		// Verify CRC_A - do our own calculation and store the control in buffer[2..3] - those bytes are not needed anymore.
+		result = calculateCrc(responseBuffer, 1, reinterpret_cast<uint16_t*>(&buffer[2]));
+		if (result != 0)
+			return result;
+		if ((buffer[2] != responseBuffer[1]) || (buffer[3] != responseBuffer[2]))
+			return CRC;
+		if (responseBuffer[0] & 0x04) // Cascade bit set - UID not complete yes
+		{ 
+			cascadeLevel++;
+		}
+		else
+		{
+			uidComplete = true;
+			uid->sak = responseBuffer[0];
+		}
+	} // End of while ( ! uidComplete)
+	
+	// Set correct uid->size
+	uid->size = 3 * cascadeLevel + 2;
+
+	return 0;
+}
+
+void Mfrc522::irq()
+{
+
+}
+
+void Mfrc522::setRf(bool on)
+{
+	uint8_t value = read(TxControlReg);
+	if(on && (value & 0x03) != 0x03)
+		write(TxControlReg, value | 0x03);
+	else if(!on && (value & 0x03) != 0x00)
+		write(TxControlReg, value & ~0x03);
+}
+
+bool Mfrc522::cardPresent()
+{
+	uint8_t bufferATQA[2];
+	uint8_t bufferLen = sizeof(bufferATQA);
+	uint8_t ret = wakeupTag(bufferATQA, &bufferLen);
+	return ret == 0 || ret == COLLISION;
+}
+
+bool Mfrc522::probe(SpiMaster* spi, ShiftReg<NFC_PORTS>*  csReg, uint8_t csPin)
+{
+	csReg->setBit(csPin, false);
+	spi->readWrite((VersionReg << 1) | (1 << 7));
+	uint8_t version = spi->readWrite();
+	csReg->setBit(csPin, true);
+	return version == 0x91 || version == 0x92;
+}
diff --git a/mfrc522.h b/mfrc522.h
new file mode 100644
index 0000000..43737ee
--- /dev/null
+++ b/mfrc522.h
@@ -0,0 +1,187 @@
+#pragma once
+#include "shiftreg.h"
+#include "softspim.h"
+#include "defines.h"
+#include "serial.h"
+
+class Mfrc522
+{
+public:
+	// Error codes.
+	static constexpr uint8_t OK = 0;        // Everything A-OK.
+	static constexpr uint8_t NOTAGERR = 1;  // No tag error
+	static constexpr uint8_t TIMEOUT = 2;   // Timeout error
+	static constexpr uint8_t LEN = 3;       // Buffer length error
+	static constexpr uint8_t COLLISION = 4; // Chip collision
+	static constexpr uint8_t CRC = 5;       // CRC incorrect collision
+	static constexpr uint8_t ERR = 6;       // General error
+
+	// Command words
+	static constexpr uint8_t IDLE       = 0x00; // NO action; Cancel the current command
+	static constexpr uint8_t MEM        = 0x01; // Store 25 byte into the internal buffer.
+	static constexpr uint8_t GENID      = 0x02; // Generates a 10 byte random ID number.
+	static constexpr uint8_t CALCCRC    = 0x03; // CRC Calculate or selftest.
+	static constexpr uint8_t TRANSMIT   = 0x04; // Transmit data
+	static constexpr uint8_t NOCMDCH    = 0x07; // No command change.
+	static constexpr uint8_t RECEIVE    = 0x08; // Receive Data
+	static constexpr uint8_t TRANSCEIVE = 0x0C; // Transmit and receive data
+	static constexpr uint8_t AUTHENT    = 0x0E; // Authentication Key
+	static constexpr uint8_t SOFTRESET  = 0x0F; // Reset
+
+	// Tag command words
+	static constexpr uint8_t MF1_REQIDL    = 0x26; // find the antenna area does not enter hibernation
+	static constexpr uint8_t MF1_REQALL    = 0x52; // find all the tags antenna area
+	static constexpr uint8_t MF1_ANTICOLL  = 0x93; // anti-collision
+	static constexpr uint8_t MF1_SELECTTAG = 0x93; // ??? election tag
+	static constexpr uint8_t MF1_AUTHENT1A = 0x60; // authentication key A
+	static constexpr uint8_t MF1_AUTHENT1B = 0x61; // authentication key B
+	static constexpr uint8_t MF1_READ      = 0x30; // Read Block
+	static constexpr uint8_t MF1_WRITE     = 0xA0; // write block
+	static constexpr uint8_t MF1_DECREMENT = 0xC0; // debit
+	static constexpr uint8_t MF1_INCREMENT = 0xC1; // recharge
+	static constexpr uint8_t MF1_RESTORE   = 0xC2; // transfer block data to the buffer
+	static constexpr uint8_t MF1_TRANSFER  = 0xB0; // save the data in the buffer
+	static constexpr uint8_t MF1_HALT      = 0x50; // Sleep
+
+	//Page 0:Command and Status
+	static constexpr uint8_t CommandReg    = 0x01;
+	static constexpr uint8_t ComIEnReg    = 0x02;
+	static constexpr uint8_t DivIEnReg     = 0x03;
+	static constexpr uint8_t ComIrqReg    = 0x04;
+	static constexpr uint8_t DivIrqReg     = 0x05;
+	static constexpr uint8_t ErrorReg      = 0x06;
+	static constexpr uint8_t Status1Reg    = 0x07;
+	static constexpr uint8_t Status2Reg    = 0x08;
+	static constexpr uint8_t FIFODataReg   = 0x09;
+	static constexpr uint8_t FIFOLevelReg  = 0x0A;
+	static constexpr uint8_t WaterLevelReg = 0x0B;
+	static constexpr uint8_t ControlReg    = 0x0C;
+	static constexpr uint8_t BitFramingReg = 0x0D;
+	static constexpr uint8_t CollReg       = 0x0E;
+	//Page 1:Command
+	static constexpr uint8_t ModeReg       = 0x11;
+	static constexpr uint8_t TxModeReg     = 0x12;
+	static constexpr uint8_t RxModeReg     = 0x13;
+	static constexpr uint8_t TxControlReg  = 0x14;
+	static constexpr uint8_t TxAutoReg     = 0x15;
+	static constexpr uint8_t TxSelReg      = 0x16;
+	static constexpr uint8_t RxSelReg      = 0x17;
+	static constexpr uint8_t RxThresholdReg= 0x18;
+	static constexpr uint8_t DemodReg      = 0x19;
+	static constexpr uint8_t MifareReg     = 0x1C;
+	static constexpr uint8_t SerialSpeedReg= 0x1F;
+	//Page 2:CFG
+	static constexpr uint8_t CRCResultRegH = 0x21;
+	static constexpr uint8_t CRCResultRegL = 0x22;
+	static constexpr uint8_t Reserved21    = 0x23;
+	static constexpr uint8_t ModWidthReg   = 0x24;
+	static constexpr uint8_t RFCfgReg      = 0x26;
+	static constexpr uint8_t GsNReg        = 0x27;
+	static constexpr uint8_t CWGsPReg      = 0x28;
+	static constexpr uint8_t ModGsPReg     = 0x29;
+	static constexpr uint8_t TModeReg      = 0x2A;
+	static constexpr uint8_t TPrescalerReg = 0x2B;
+	static constexpr uint8_t TReloadRegH   = 0x2C;
+	static constexpr uint8_t TReloadRegL   = 0x2D;
+	static constexpr uint8_t TCounterValueRegH = 0x2E;
+	static constexpr uint8_t TCounterValueRegL = 0x2F;
+	//Page 3:TestRegister
+	static constexpr uint8_t TestSel1Reg   = 0x31;
+	static constexpr uint8_t TestSel2Reg   = 0x32;
+	static constexpr uint8_t TestPinEnReg  = 0x33;
+	static constexpr uint8_t TestPinValueReg = 0x34;
+	static constexpr uint8_t TestBusReg    = 0x35;
+	static constexpr uint8_t AutoTestReg   = 0x36;
+	static constexpr uint8_t VersionReg    = 0x37;
+	static constexpr uint8_t AnalogTestReg = 0x38;
+	static constexpr uint8_t TestDAC1Reg   = 0x39;
+	static constexpr uint8_t TestDAC2Reg   = 0x3A;
+	static constexpr uint8_t TestADCReg    = 0x3B;
+
+	// PICC (tag) commands
+	// The commands used by the PCD to manage communication with several PICCs (ISO 14443-3, Type A, section 6.4)
+
+	// REQuest command, Type A. Invites PICCs in state IDLE to go to READY and prepare for anticollision or selection. 7 bit frame.
+	static constexpr uint8_t PICC_CMD_REQA = 0x26;
+	// Wake-UP command, Type A. Invites PICCs in state IDLE and HALT to go to READY(*) and prepare for anticollision or selection. 7 bit frame.
+	static constexpr uint8_t PICC_CMD_WUPA    = 0x52; // Cascade Tag. Not really a command, but used during anti collision.
+	static constexpr uint8_t PICC_CMD_CT      = 0x88;
+	static constexpr uint8_t PICC_CMD_SEL_CL1 = 0x93; // Anti collision/Select, Cascade Level 1
+	static constexpr uint8_t PICC_CMD_SEL_CL2 = 0x95; // Anti collision/Select, Cascade Level 1
+	static constexpr uint8_t PICC_CMD_SEL_CL3 = 0x97; // Anti collision/Select, Cascade Level 1
+	// HaLT command, Type A. Instructs an ACTIVE PICC to go to state HALT.
+	// The commands used for MIFARE Classic (from http://www.nxp.com/documents/data_sheet/MF1S503x.pdf, Section 9)
+	// Use PCD_MFAuthent to authenticate access to a sector, then use these commands to read/write/modify the blocks on the sector.
+	// The read/write commands can also be used for MIFARE Ultralight.
+	static constexpr uint8_t PICC_CMD_HLTA    = 0x50;
+	static constexpr uint8_t PICC_CMD_MF_AUTH_KEY_A = 0x60; // Perform authentication with Key A
+	static constexpr uint8_t PICC_CMD_MF_AUTH_KEY_B = 0x61; // Perform authentication with Key B
+	// Reads one 16 byte block from the authenticated sector of the PICC. Also used for MIFARE Ultralight.
+	static constexpr uint8_t PICC_CMD_MF_READ       = 0x30; 
+	// Writes one 16 byte block to the authenticated sector of the PICC. Called "COMPATIBILITY WRITE" for MIFARE Ultralight.
+	static constexpr uint8_t PICC_CMD_MF_WRITE      = 0xA0; 
+	// Decrements the contents of a block and stores the result in the internal data register.
+	static constexpr uint8_t PICC_CMD_MF_DECREMENT	= 0xC0; 
+	// Increments the contents of a block and stores the result in the internal data register.
+	static constexpr uint8_t PICC_CMD_MF_INCREMENT	= 0xC1; 
+	// Reads the contents of a block into the internal data register.
+	static constexpr uint8_t PICC_CMD_MF_RESTORE		= 0xC2;
+	// Writes the contents of the internal data register to a block.
+	static constexpr uint8_t PICC_CMD_MF_TRANSFER	= 0xB0;
+	// The commands used for MIFARE Ultralight (from http://www.nxp.com/documents/data_sheet/MF0ICU1.pdf, Section 8.6)
+	// The PICC_CMD_MF_READ and PICC_CMD_MF_WRITE can also be used for MIFARE Ultralight.
+	static constexpr uint8_t PICC_CMD_UL_WRITE		= 0xA2;		// Writes one 4 byte page to the PICC
+	
+	typedef struct
+	{
+		uint8_t size; // Number of bytes in the UID. 4, 7 or 10.
+		uint8_t uidByte[10];
+		uint8_t sak;  // The SAK (Select acknowledge) byte returned from the PICC after successful selection.
+	} Uid;
+
+
+private:
+	ShiftReg<NFC_PORTS>*  _csReg;
+	SpiMaster* _spi;
+
+	uint8_t _csPin;
+
+	uint8_t read(uint8_t addr);
+	void read(uint8_t addr, uint8_t* data, uint8_t datalen, uint8_t rxAlign = 0);
+	void write(uint8_t addr, uint8_t data);
+	void write(uint8_t addr, uint8_t* data, uint8_t datalen);
+	void updateBit(uint8_t addr, uint8_t bit, bool value);
+
+	void (*_tagEnterdCb)(Mfrc522*, void*);
+	void* _userData;
+
+public:
+	
+	inline static Serial* serial = nullptr;
+	
+	Mfrc522(SpiMaster* spi, ShiftReg<NFC_PORTS>*  csReg, uint8_t csPin,
+	        void (*tagEnterdCb)(Mfrc522*, void*) = nullptr, void* userData = nullptr);
+	
+	uint8_t calculateCrc(uint8_t *data, uint8_t length, uint16_t *result);
+
+	uint8_t commuicateWithTag(uint8_t command, uint8_t waitIrq,
+	                          uint8_t *sendData, uint8_t sendLen,
+	                          uint8_t *recvData, uint8_t *recvLen,
+	                          uint8_t validBits = 0, uint8_t rxAlign = 0,
+	                          uint8_t *rxValidBits = nullptr);
+
+	uint8_t transceive(uint8_t *sendData, uint8_t sendLen, uint8_t *recvData, uint8_t *recvLen,
+	                   uint8_t validBits = 0, uint8_t rxAlign = 0, uint8_t *rxValidBits = nullptr);
+
+	uint8_t wakeupTag(uint8_t* bufferATQA,	uint8_t *bufferLen);
+
+	uint8_t selectTag(Uid *uid);
+
+	void irq();
+
+	void setRf(bool on);
+	
+	bool cardPresent();
+
+	static bool probe(SpiMaster* spi, ShiftReg<NFC_PORTS>*  csReg, uint8_t csPin);
+};
diff --git a/nfcbord.cpp b/nfcbord.cpp
new file mode 100644
index 0000000..6b562e8
--- /dev/null
+++ b/nfcbord.cpp
@@ -0,0 +1,150 @@
+#include "nfcbord.h"
+
+#include <avr/io.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <string.h>
+#include <stdio.h>
+#include "writepin.h"
+
+extern char buffer[SNPRINTF_BUFFER_SIZE];
+
+NfcBoard::NfcBoard():
+csReg(&PORTC, PC1, PC2, PC0),
+irqReg(&PORTC, &PINB, PC5, PB1, PC4)
+{
+	DDRC = (1 << PC0) | (1 << PC1) | (1 << PC2) | (1 << PC4) | (1 << PC5);
+	DDRB = (1 << PB4) | (1 << PB3);
+	csReg.clear(true);
+	probe();
+}
+
+void NfcBoard::probe()
+{
+	readers.clear();
+	irqPins.clear();
+	for(uint8_t i = 0; i < NFC_PORTS; ++i)
+	{
+		if(Mfrc522::probe(&spim, &csReg, i))
+		{
+			irqPins.push_back(i);
+		}
+	}
+	for(uint8_t i = 0; i < irqPins.count(); ++i)
+	{
+		readers.push_back(Mfrc522(&spim, &csReg, irqPins[i]));
+	}
+}
+
+void NfcBoard::printNfcDevices(Serial* serial)
+{
+	serial->write_p(PSTR("NFC DEVICES:\n"));
+	for(uint8_t i = 0; i < readers.count(); ++i)
+	{
+		snprintf(buffer, SNPRINTF_BUFFER_SIZE, "NFC NUMBER: %u IRQ: %x\n", i, irqPins[i]);
+		serial->write(buffer, SNPRINTF_BUFFER_SIZE);
+	}
+}
+
+int NfcBoard::dispatch(char* inBuffer, Serial* serial)
+{
+	if(strcmp(inBuffer, "tsta") == 0 )
+	{
+		serial->write_p(PSTR("Runing shift test\n"));
+		while(!serial->dataIsWaiting())
+		{
+			csReg.clear(false);
+			_delay_us(100);
+		}
+		serial->write_p(PSTR("Finished\n"));
+		csReg.clear(true);
+		return 0;
+	}
+	else if(strcmp(inBuffer, "tstb") == 0 )
+	{
+		serial->write_p(PSTR("Runing input shift test b\n"));
+		while(!serial->dataIsWaiting())
+		{
+			serial->write((int)(*irqReg.read()));
+			serial->putChar('\n');
+			_delay_us(100);
+		}
+		serial->write_p(PSTR("Finished\n"));
+		return 0;
+	}
+	else if(strcmp(inBuffer, "tstc") == 0 )
+	{
+		serial->write_p(PSTR("Runing spi test\n"));
+		while(!serial->dataIsWaiting())
+		{
+			csReg.setBit(1, false);
+			spim.readWrite((0x37 << 1) | (1 << 7));
+			serial->write((int)spim.readWrite());
+			serial->putChar('\n');
+			csReg.setBit(1, true);
+			_delay_us(100);
+		}
+		serial->write_p(PSTR("Finished\n"));
+		return 0;
+	}
+	else if(strcmp(inBuffer, "tstd") == 0 )
+	{
+		serial->write_p(PSTR("Runing tag detection test\n"));
+		bool oldPresent = false;
+		Mfrc522::serial = serial;
+		while(!serial->dataIsWaiting())
+		{
+			bool present = readers[0].cardPresent();
+			if(present && !oldPresent)
+			{
+				oldPresent = present;
+
+				serial->write_p(PSTR("Tag found, selecting\n"));
+
+				Mfrc522::Uid uid;
+				uint8_t res = readers[0].selectTag(&uid);
+				if(res != 0)
+				{
+					serial->write_p(PSTR("Select Failed with "));
+					serial->write((int)res);
+					serial->putChar('\n');
+					continue;
+				}
+				snprintf(buffer, SNPRINTF_BUFFER_SIZE, "Uid: %x %x %x %x %x %x %x %x %x %x\n",
+						 uid.uidByte[0], uid.uidByte[1], uid.uidByte[2], uid.uidByte[3], uid.uidByte[4],
+						 uid.uidByte[5], uid.uidByte[6], uid.uidByte[7], uid.uidByte[8], uid.uidByte[9]);
+			}
+			else if(!present && oldPresent)
+			{
+				serial->write_p(PSTR("Tag lost\n"));
+				oldPresent = present;
+			}
+			_delay_ms(100);
+		}
+		serial->write_p(PSTR("Finished\n"));
+		return 0;
+	}
+	else if(strcmp(inBuffer, "status") == 0 )
+	{
+		printNfcDevices(serial);
+		return 0;
+	}
+	else if(strcmp(inBuffer, "wake") == 0 )
+	{
+		uint8_t bufferATQA[2];
+		uint8_t len = sizeof(bufferATQA);
+
+		uint8_t res = readers[0].wakeupTag(bufferATQA, &len);
+		snprintf(buffer, SNPRINTF_BUFFER_SIZE, "wakeupTag returned: %u Buffer: 0x%x 0x%x len %u\n",
+				 res, bufferATQA[0], bufferATQA[1], len);
+		serial->write(buffer, SNPRINTF_BUFFER_SIZE);
+		return 0;
+	}
+	else if(strcmp(inBuffer, "probe") == 0 )
+	{
+		probe();
+		printNfcDevices(serial);
+		return 0;
+	}
+	return -3;
+}
diff --git a/nfcbord.h b/nfcbord.h
new file mode 100644
index 0000000..eb18558
--- /dev/null
+++ b/nfcbord.h
@@ -0,0 +1,26 @@
+#pragma once
+#include "serial.h"
+#include "shiftreg.h"
+#include "inputshiftreg.h"
+#include "mfrc522.h"
+#include "softspim.h"
+#include "staticvector.h"
+#include "defines.h"
+
+class NfcBoard
+{
+public:
+	ShiftReg<NFC_PORTS> csReg;
+	InputShiftReg<NFC_PORTS> irqReg;
+	SpiMaster spim;
+	SVector<Mfrc522, NFC_PORTS> readers;
+	SVector<uint8_t, NFC_PORTS> irqPins;
+	
+	NfcBoard();
+	
+	void probe();
+	
+	void printNfcDevices(Serial* serial);
+	
+	int dispatch(char* inBuffer, Serial* serial);
+};
diff --git a/serial.h b/serial.h
index 46cab6f..80f9522 100644
--- a/serial.h
+++ b/serial.h
@@ -2,7 +2,7 @@
 #define SERIAL_H
 
 #define BAUD 38400
-#define SERIAL_BUFFER_SIZE 512
+#define SERIAL_BUFFER_SIZE 384
 
 #include <util/setbaud.h>
 #include <avr/io.h>
diff --git a/shiftreg.h b/shiftreg.h
new file mode 100644
index 0000000..2e31b99
--- /dev/null
+++ b/shiftreg.h
@@ -0,0 +1,81 @@
+#pragma once
+#include <util/delay.h>
+
+template <int BITS> class ShiftReg
+{
+private:
+	static constexpr int BYTES = (BITS % 8 == 0) ? (BITS/8) : (BITS/8+1);
+
+	static constexpr bool invert = true;
+
+	volatile unsigned char *_port;
+	const unsigned char _pinSer;
+	const unsigned char _pinSerClk;
+	const unsigned char _pinRClk;
+
+	unsigned char _lastData[BYTES] = {};
+
+public:
+
+	ShiftReg(volatile unsigned char* const port, const unsigned char pinSer, const unsigned char pinSerClk,
+	         const unsigned char pinRClk):
+		_port(port),  _pinSer(pinSer), _pinSerClk(pinSerClk), _pinRClk(pinRClk)
+	{
+		clear();
+	}
+	void setBit(unsigned char bit, bool value)
+	{
+		if(BITS <= bit)
+			return;
+		bit = BITS - bit - 1;
+		if(value)
+			_lastData[bit/8] |= (1<<(bit%8));
+		else
+			_lastData[bit/8] &= ~(1<<(bit%8));
+		write(_lastData);
+	}
+	bool getBit(unsigned char bit)
+	{
+		if(BITS <= bit)
+			return false;
+		bit = BITS - bit - 1;
+		return _lastData[bit/8] & (1<<(bit%8));
+	}
+	void write(const unsigned char * const in)
+	{
+		for(unsigned char i = 0 ; i < BYTES; ++i)
+			_lastData[i] = in[i];
+
+		if constexpr(invert)
+		{
+			*_port |= ((1<<_pinSer) | (1<<_pinSerClk));
+			*_port |= (1<<_pinRClk);
+
+			for(unsigned char i = 0; i < BITS; ++i)
+			{
+				*_port |= (1 << _pinSerClk);
+				in[i/8] & (1<<(i%8)) ? (*_port &= ~(1 << _pinSer)) : (*_port |= (1 << _pinSer));
+				*_port &= ~(1 << _pinSerClk);
+			}
+			*_port &= ~(1<<_pinRClk);
+		}
+		else
+		{
+			*_port &= ~((1<<_pinSer) | (1<<_pinSerClk));
+			*_port &= ~(1<<_pinRClk);
+			for(unsigned char i = 0; i < BITS; ++i)
+			{
+				*_port &= ~(1 << _pinSerClk);
+				in[i/8] & (1<<(i%8)) ? (*_port |= (1 << _pinSer)) : (*_port &= ~(1 << _pinSer));
+				*_port |= (1 << _pinSerClk);
+			}
+			*_port |= 1<<_pinRClk;
+		}
+	}
+	void clear(bool value = false)
+	{
+		for(unsigned char i = 0 ; i < BYTES; ++i)
+			_lastData[i] = value ? 0xff : 0x00;
+		write(_lastData);
+	}
+};
diff --git a/softspim.cpp b/softspim.cpp
new file mode 100644
index 0000000..a81fa4a
--- /dev/null
+++ b/softspim.cpp
@@ -0,0 +1,64 @@
+#include "softspim.h"
+#include "writepin.h"
+#include <util/delay.h>
+
+SpiMaster::SpiMaster()
+{
+
+}
+
+uint8_t SpiMaster::readWrite(uint8_t in)
+{
+	_delay_us(DELAY_TIME_US);
+	uint8_t recByte = 0;
+	for(uint8_t i = 0; i < 8; ++i)
+	{
+		if constexpr (BIT_ORDER == 0)
+			writePin(_port, _pinOut, in & (1 << i));
+		else
+			writePin(_port, _pinOut, in & (1 << (7-i)));
+		if constexpr (CLOCK_PHASE == 0)
+			_delay_us(DELAY_TIME_US);
+		writePin(_port, _pinClock, !CLOCK_POLARITY);
+		if constexpr (CLOCK_PHASE == 1)
+			_delay_us(DELAY_TIME_US);
+		if constexpr (BIT_ORDER == 0)
+			recByte |= readPin(_pinReg, _pinIn) << i;
+		else
+			recByte |= readPin(_pinReg, _pinIn) << (7-i);
+		if constexpr (CLOCK_PHASE == 0)
+			_delay_us(DELAY_TIME_US);
+		writePin(_port, _pinClock, CLOCK_POLARITY);
+		if constexpr (CLOCK_PHASE == 1)
+			_delay_us(DELAY_TIME_US);
+	}
+	return recByte;
+}
+
+void SpiMaster::readWrite(uint8_t length, uint8_t* bufferIn, uint8_t* bufferOut)
+{
+	for(uint8_t i = 0; i < length; ++i)
+	{
+		uint8_t outByte = 0;
+		if(bufferOut) outByte = bufferOut[i];
+		uint8_t inByte = readWrite(outByte);
+		if(bufferIn) bufferIn[i] = inByte;
+	}
+}
+
+
+void SpiMaster::prepare()
+{
+	writePin(_port, _pinClock, CLOCK_POLARITY);
+}
+
+
+void SpiMaster::write(uint8_t in)
+{
+	readWrite(in);
+}
+
+uint8_t SpiMaster::read()
+{
+	return readWrite();
+}
diff --git a/softspim.h b/softspim.h
new file mode 100644
index 0000000..a0fbf28
--- /dev/null
+++ b/softspim.h
@@ -0,0 +1,30 @@
+#pragma once
+#include <avr/io.h>
+#include <stdint.h>
+
+class SpiMaster
+{
+private:
+
+	static constexpr uint8_t CLOCK_POLARITY = 1;
+	static constexpr uint8_t CLOCK_PHASE    = 1;
+	static constexpr uint8_t BIT_ORDER      = 1;
+
+	volatile uint8_t * const _port        = &PORTB;
+	volatile uint8_t * const _pinReg      = &PINB;
+	static constexpr uint8_t _pinIn       = PB2;
+	static constexpr uint8_t _pinOut      = PB4;
+	static constexpr uint8_t _pinClock    = PB3;
+
+	static constexpr uint8_t DELAY_TIME_US  = 10;
+
+
+public:
+
+	SpiMaster();
+	void    readWrite(uint8_t length, uint8_t* bufferIn, uint8_t* bufferOut);
+	uint8_t readWrite(uint8_t in = 0);
+	void prepare();
+	uint8_t read();
+	void write(uint8_t in);
+};
diff --git a/staticvector.h b/staticvector.h
index 217817d..bfdbdae 100644
--- a/staticvector.h
+++ b/staticvector.h
@@ -59,7 +59,7 @@ public:
 	
 	bool push_back(const T in)
 	{
-		if( remainingCapacity() != 0)
+		if(remainingCapacity() != 0)
 		{
 			array[stored] = in;
 			++stored;
@@ -70,13 +70,13 @@ public:
 	
 	bool erase(size_t position)
 	{
-	if(position > stored)
-		return false;
-	array[position].~T();
-	--stored;
-	for( size_t i = position; i < stored; i++ )
-		memcpy(&array[i], &array[i+1], sizeof(T));
-	return true;
+		if(position > stored)
+			return false;
+		array[position].~T();
+		--stored;
+		for( size_t i = position; i < stored; i++ )
+			memcpy(&array[i], &array[i+1], sizeof(T));
+		return true;
 	}
 	
 	void clear()
diff --git a/traindispatch.h b/traindispatch.h
index 580a665..9740c65 100644
--- a/traindispatch.h
+++ b/traindispatch.h
@@ -1,4 +1,9 @@
 #pragma once
+#include <stdlib.h>
+#include <stdint.h>
+#include <string.h>
+#include "serial.h"
+#include "writepin.h"
 
 void printTrainState(int id, Serial* serial)
 {