#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;
}