#include "mfrc522.h"

Serial *Mfrc522::serial = nullptr;
uint8_t Mfrc522::reponseBuffer[Mfrc522::reponseBufferLen];

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);
	const uint8_t addrS = addr << 1 | (1 << 7);
	_spi->readWrite(addrS);
	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(addrS);
			data[0] = (data[0] & ~mask) | (value & mask);
		}
		else
		{
			data[i] = _spi->readWrite(addrS);
		}
	}
	_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):
	_csReg(csReg), _spi(spi), _csPin(csPin)
{
	write(CommandReg, SOFTRESET);
	_delay_ms(100);

	write(TModeReg, 0x80);
	write(TPrescalerReg, 0xA9);
	write(TReloadRegH, 0x0B);
	write(TReloadRegL, 0xB8);
	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
	updateBit(RxModeReg, 3, true);
	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(ComIrqReg, 0b01111111);
	write(ComIEnReg, 0); // invert irq pin (high is active)
	write(DivIEnReg, 1 << 7); // enable MfinActIrq as push-pull

	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);
	
	if(serial)
	{
		serial->write_p(PSTR("Calculated CRC "));
		serial->write((int)read(CRCResultRegL));
		serial->putChar(' ');
		serial->write((int)read(CRCResultRegH));
		serial->putChar('\n');
	}
	
	*result = read(CRCResultRegL);
	*result |= read(CRCResultRegH) << 8;
	return 0;
}

void Mfrc522::setupTransceive(uint8_t *sendData, uint8_t sendLen, uint8_t validBits, uint8_t rxAlign)
{
	write(FIFOLevelReg, 1 << 7);			// Flush fifo Buffer;
	write(FIFODataReg, sendData, sendLen);	 // Fill fifo
	write(BitFramingReg, (rxAlign << 4) + validBits);
	write(CommandReg, TRANSCEIVE);	// Execute the command
	
	if(serial)
	{
		serial->write_p(PSTR("Communicate params: CommandReg: "));
		serial->write((int)read(CommandReg));
		serial->putChar(' ');
		serial->write_p(PSTR("FIFO: "));
		for(uint8_t i = 0; i < sendLen; ++i)
		{
			serial->write((int)sendData[i]);
			serial->putChar(' ');
		}
		serial->write_p(PSTR("BitFramingReg: "));
		serial->write((int)read(BitFramingReg));
		serial->putChar('\n');
	}

	updateBit(BitFramingReg, 7, true);
}

uint8_t Mfrc522::transceiveAsync(void (*transceiveCb)(uint8_t, Mfrc522*, uint8_t*, uint8_t, void*), void* userData,
								 uint8_t *sendData, uint8_t sendLen, uint8_t validBits, uint8_t rxAlign)
{
	if(mode != MODE_IDLE)
		return BUSY;
	
	mode = MODE_TRANSCEIVE;
	
	_transceiveCb = transceiveCb;
	_transceiveUserData = userData;
	
	write(CommandReg, IDLE);

	write(ComIrqReg, 0b01111111); // clear irqs
	write(ComIEnReg, (1 << 5) | (1 << 0));

	setupTransceive(sendData, sendLen, validBits, rxAlign);

	return 0;
}

void Mfrc522::transceiveAsyncFinish(uint8_t irq)
{
	write(ComIEnReg, 0); // disable irqs

	if(!_transceiveCb)
	{
		mode = MODE_IDLE;
		updateBit(FIFOLevelReg, 7, true);
		return;
	}

	uint8_t errorRegValue = read(ErrorReg);
	if (errorRegValue & 0b00010011) // BufferOvfl ParityErr ProtocolErr
	{
		mode = MODE_IDLE;
		_transceiveCb(ERR, this, nullptr, 0, _transceiveUserData);
		return;
	}
	
	if(irq & 1)
	{
		mode = MODE_IDLE;
		_transceiveCb(TIMEOUT, this, nullptr, 0, _transceiveUserData);
		return;
	}

	uint8_t fifoBites = read(FIFOLevelReg);

	if(fifoBites > reponseBufferLen)
		_transceiveCb(LEN, this, nullptr, 0, _transceiveUserData);
	read(FIFODataReg, reponseBuffer, fifoBites);
	mode = MODE_IDLE;
	if(errorRegValue & 0x08)
		_transceiveCb(COLLISION, this, reponseBuffer, fifoBites, _transceiveUserData);
	else
		_transceiveCb(0, this, reponseBuffer, fifoBites, _transceiveUserData);
	
}

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)
{
	write(CommandReg, IDLE);
	write(ComIrqReg, 0b01111111); // clear irqs
	setupTransceive(sendData, sendLen, validBits, rxAlign);
	
	uint16_t i = 2000;
	uint8_t irq = read(ComIrqReg);
	while(!(irq & 0x30)) // RxIRq and IdleIRq
	{
		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(fifoBites > *recvLen)
			return LEN;
		*recvLen = fifoBites;
		read(FIFODataReg, recvData, fifoBites, rxAlign);
		if(rxValidBits)
			*rxValidBits = read(ControlReg) & 0x07;
		
		if(serial)
		{
			serial->write_p(PSTR("fifo has "));
			serial->write((int)fifoBites);
			serial->write_p(PSTR(" bytes: "));
			for(uint8_t i = 0; i < fifoBites; ++i)
			{
				serial->write((int)recvData[i]);
				serial->putChar(' ');
			}
			serial->putChar('\n');
		}
	}
	
	if(errorRegValue & 0x08)
	{
		if(serial)
			serial->write_p(PSTR("collision err\n"));
		return COLLISION;
	}
	return 0;
}


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 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 *responseBufferPtr;
	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:
				reponseBuffer[0] = PICC_CMD_SEL_CL1;
				uidIndex = 0;
				break;
			
			case 1:
				reponseBuffer[0] = PICC_CMD_SEL_CL2;
				uidIndex = 3;
				break;
			
			case 2:
				reponseBuffer[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++)
				reponseBuffer[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.
			{ 
				reponseBuffer[1] = 0x70; // NVB - Number of Valid Bits: Seven whole bytes
				// Calculate BCC - Block Check Character
				reponseBuffer[6] = reponseBuffer[2] ^ reponseBuffer[3] ^ reponseBuffer[4] ^ reponseBuffer[5];
				// Calculate CRC_A
				result = calculateCrc(reponseBuffer, 7, reinterpret_cast<uint16_t*>(&reponseBuffer[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)
				responseBufferPtr	= &reponseBuffer[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
				reponseBuffer[1]		= (index << 4) + txLastBits;	// NVB - Number of Valid Bits
				bufferUsed		= index + (txLastBits ? 1 : 0);
				// Store response in the unused part of buffer
				responseBufferPtr	= &reponseBuffer[index];
				responseLength	= sizeof(reponseBuffer) - 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(reponseBuffer, bufferUsed, responseBufferPtr, &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.
				reponseBuffer[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			= (reponseBuffer[2] == PICC_CMD_CT) ? 3 : 2; // source index in buffer[]
		bytesToCopy		= (reponseBuffer[2] == PICC_CMD_CT) ? 3 : 4;
		for (count = 0; count < bytesToCopy; count++)
		{
			uid->uidByte[uidIndex + count] = reponseBuffer[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(responseBufferPtr, 1, reinterpret_cast<uint16_t*>(&reponseBuffer[2]));
		if (result != 0)
			return result;
		if ((reponseBuffer[2] != responseBufferPtr[1]) || (reponseBuffer[3] != responseBufferPtr[2]))
			return CRC;
		if (responseBufferPtr[0] & 0x04) // Cascade bit set - UID not complete yes
		{ 
			cascadeLevel++;
		}
		else
		{
			uidComplete = true;
			uid->sak = responseBufferPtr[0];
		}
	} // End of while ( ! uidComplete)
	
	// Set correct uid->size
	uid->size = 3 * (cascadeLevel+1) + 1;

	return 0;
}

uint8_t Mfrc522::getUid(Uid *uid)
{
	uint8_t bufferOut[2];
	uint8_t reponseBuffer[5];

	bufferOut[0] = PICC_CMD_SEL_CL1;
	bufferOut[1] = 2 << 4;

	uint8_t rxLen = sizeof(reponseBuffer);

	uint8_t ret = transceive(bufferOut, 2, reponseBuffer, &rxLen);
	if(ret != 0)
		return ret;

	if(rxLen != 5)
		return ERR;

	uid->size = 4;

	for(uint8_t i = 0; i < uid->size; ++i)
	{
		uid->uidByte[i] = reponseBuffer[i];
	}

	return 0;
}

void Mfrc522::irq()
{
	if(mode == MODE_TRANSCEIVE)
	{
		uint8_t irqs = read(ComIrqReg);

		if(irqs)
		{
			if(serial)
			{
				serial->write("IRQS: ");
				serial->write((int)irqs);
				serial->putChar('\n');
			}
			write(ComIrqReg, 0b01111111); // clear irqs
			transceiveAsyncFinish(irqs);
		}
	}
	else if(serial)
		serial->write("IRQ wrong mode\n");
}

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 bufferLen = sizeof(reponseBuffer);
	uint8_t ret = wakeupTag(reponseBuffer, &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();
	if(serial)
	{
		serial->write_p(PSTR("Got version register: "));
		serial->write((int)version);
		serial->putChar('\n');
	}
	csReg->setBit(csPin, true);
	return version == 0x91 || version == 0x92;
}

bool Mfrc522::testFifo()
{
	uint8_t buffer[8] = {42, 43, 44, 45, 46, 47, 48, 49};
	write(FIFOLevelReg, 1 << 7);			// Flush fifo Buffer;
	write(FIFODataReg, buffer, sizeof(buffer));	 // Fill fifo
	
	if(serial)
		serial->write_p(PSTR("Fifo buffer contains: "));
	
	uint8_t len = read(FIFOLevelReg);
	read(FIFODataReg, reponseBuffer, len);
	bool ret = true;
	for(uint8_t i = 0; i < len; ++i)
	{
		if(buffer[i] != reponseBuffer[i])
			ret = false;
		if(serial)
		{
			serial->write((int)reponseBuffer[i]);
			serial->putChar(' ');
		}
	}
	if(serial)
		serial->putChar('\n');
	return ret;
}

bool Mfrc522::detectAsync(void (*tagEnterdCb)(Mfrc522*, void*), void* userData)
{
	if(irqDetect)
		return false;
	_tagEnterdCb = tagEnterdCb;
	_userData = userData;
	irqDetect = true;
	updateBit(CollReg, 7, false);
	uint8_t data = PICC_CMD_WUPA;
	if(transceiveAsync(&detectAsyncCb, nullptr, &data, 1, 7) != 0)
	{
		irqDetect = false;
		return false;
	}
	return true;
}

void Mfrc522::stopAsync()
{
	write(CommandReg, IDLE);
	write(FIFOLevelReg, 1 << 7);
	mode = MODE_IDLE;
	irqDetect = false;
}

void Mfrc522::detectAsyncCb(uint8_t ret, Mfrc522* reader, uint8_t* response, uint8_t responseLen, void* userData)
{
	if((ret == 0 || ret == COLLISION) && reader->_tagEnterdCb)
	{
		reader->stopAsync();
		reader->_tagEnterdCb(reader, reader->_userData);
	}
	else
	{
		reader->irqDetect = false;
		reader->detectAsync(reader->_tagEnterdCb, reader->_userData);
	}
}