#include "nfcbord.h"

#include <avr/io.h>
#include <stdlib.h>
#include <stdint.h>
#include <string.h>
#include <avr/wdt.h>
#include <stdio.h>
#include "writepin.h"
#include "watchdog.h"

NfcBoard nfcBoard(Serial::getInstance());

extern char buffer[SNPRINTF_BUFFER_SIZE];

ISR(WDT_vect)
{
	for(uint8_t i = 0; i < nfcBoard.watchDogBits.count(); ++i)
	{
		if(nfcBoard.watchDogBits[i] == 0)
			nfcBoard.watchDogBits[i] = 2;
		else
			nfcBoard.watchDogBits[i] = 0;
	}
}

NfcBoard::NfcBoard(Serial* serialIn):
serial(serialIn),
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);
	irqReg.read();
	probe();
	setEnabled(true);
}

void NfcBoard::poll(Serial* serial)
{
	for(uint8_t i = 0; i < watchDogBits.count(); ++i)
	{
		if(watchDogBits[i] == 2 && readers[i].type == TYPE_MFRC522)
		{
			if(serial)
			{
				serial->write_p(PSTR("Warning reader watchdog timeout for reader "));
				serial->write((int)i);
				serial->putChar('\n');
			}
			readers[i].device.mfrc522.reset();
			if(enabled_)
				readers[i].device.mfrc522.detectAsync(detectCb, this);
			watchDogBits[i] = 1;
		}
	}

	if(readPin(&PINC, PC3))
	{
		uint8_t* data = irqReg.read();
		for(uint8_t i = 0; i < NFC_PORTS; ++i)
		{
			if(*data & (1 << i))
			{
				for(uint8_t j = 0; j < readers.count(); ++j)
				{
					if(irqPins[j] == i)
					{
						if(readers[j].type == TYPE_MFRC522)
						{
							readers[j].device.mfrc522.irq();
							watchDogBits[j] = 1;
						}
						else
						{
							readers[j].device.proxy.irq();
						}
					}
				}
			}
		}
	}
}

void NfcBoard::setEnabled(bool enabled)
{
	if(enabled != enabled_)
	{
		enabled_ = enabled;
		for(uint8_t i = 0; i < readers.count(); ++i)
		{
			if(readers[i].type == TYPE_MFRC522)
			{
				if(enabled)
					readers[i].device.mfrc522.detectAsync(detectCb, this);
				else
					readers[i].device.mfrc522.stopAsync();
				watchDogBits[i] = 0;
			}
		}
		if(enabled)
			wdt_set(WDTO_4S);
		else
			wdt_disable();
	}
}

uint8_t NfcBoard::csToIrq(uint8_t cs)
{
	switch(cs)
	{
		case 0:
			return 4;
		case 1:
			return 2;
		case 2:
			return 1;
		case 3:
			return 0;
		case 4:
			return 7;
		case 5:
			return 6;
		case 6:
			return 5;
		case 7:
			return 3;
		default:
			return 8;
	}
}

void NfcBoard::probe()
{
	readers.clear();
	irqPins.clear();
	watchDogBits.clear();
	for(uint8_t i = 0; i < NFC_PORTS; ++i)
	{
		if(Mfrc522::probe(&spim, &csReg, i))
		{
			NfcPort port(Mfrc522(&spim, &csReg, i));
			readers.push_back(port);
			irqPins.push_back(csToIrq(i));
			watchDogBits.push_back(0);
		}
		else if(MfrcProxy::probe(&spim, &csReg, i))
		{
			NfcPort port(MfrcProxy(&spim, &csReg, i, proxyDetectCb, this));
			readers.push_back(port);
			irqPins.push_back(csToIrq(i));
			watchDogBits.push_back(0);
		}
	}
}

void NfcBoard::printNfcDevices()
{
	serial->write_p(PSTR("NFC DEVICES:\n"));
	for(uint8_t i = 0; i < readers.count(); ++i)
	{
		uint8_t id = readers[i].type == TYPE_MFRC522 ? i+0x80 : readers[i].device.proxy.getId();
		uint8_t cs = readers[i].type == TYPE_MFRC522 ?
			readers[i].device.mfrc522.getCs() : readers[i].device.proxy.getCs();
		snprintf_P(buffer, SNPRINTF_BUFFER_SIZE, PSTR("NFC NUMBER: %u CS: %x IRQ: %x TYPE: %d\n"),
				id, cs, irqPins[i], readers[i].type);
		serial->write(buffer, SNPRINTF_BUFFER_SIZE);
	}
}

int NfcBoard::dispatch(char* inBuffer)
{ 
	Mfrc522* mainReader = nullptr;
	for(uint8_t i = 0; i < readers.count(); ++i)
	{
		if(readers[i].type == TYPE_MFRC522)
		{
			mainReader = &readers[i].device.mfrc522;
		}
	}
	if(strcmp(inBuffer, "debug") == 0)
	{
		Mfrc522::serial = serial;
		return 0;
	}
	else if(strcmp(inBuffer, "quiet") == 0 )
	{
		Mfrc522::serial = nullptr;
		return 0;
	}
	else if(strcmp(inBuffer, "irqs") == 0 )
	{
		serial->write_p(PSTR("Irq pin detection test\n")); 
		while(!serial->dataIsWaiting())
		{
			uint8_t* data = irqReg.read();
			for(uint8_t i = 0; i < NFC_PORTS; ++i)
			{
				if(*data & (1 << i))
				{
					serial->write("IRQ: ");
					serial->write((int)i);
					serial->putChar('\n');
				}
			}
		}
		serial->write_p(PSTR("Finished\n"));
		return 0;
	}
	else if(strcmp(inBuffer, "select") == 0 )
	{
		bool enabled = enabled_;
		setEnabled(false);
		if(!mainReader)
		{
			serial->write_p(PSTR("No nfc reader present\n"));
			return -1;
		}
		serial->write_p(PSTR("Runing tag detection test\n")); 
		while(!serial->dataIsWaiting())
		{
			bool present = mainReader->cardPresent();
			if(present)
			{
				Uid uid;
				uint8_t res = mainReader->selectTag(&uid);
				if(res != 0)
					continue;

				serial->write_p(PSTR("Uid: "));
				for(uint8_t i = 0; i < uid.size; ++i)
				{
					serial->write((int)uid.uidByte[i]);
					if(i < uid.size-1)
						serial->putChar(':');
				}
				serial->putChar('\n');
			}
			_delay_ms(100);
		}
		setEnabled(enabled);
		serial->write_p(PSTR("Finished\n"));
		return 0;
	}
	else if(strcmp(inBuffer, "detect") == 0 )
	{
		bool enabled = enabled_;
		setEnabled(false);
		if(!mainReader)
		{
			serial->write_p(PSTR("No nfc reader present\n"));
			return -1;
		}
		serial->write_p(PSTR("Runing fast tag detection test\n"));
		while(!serial->dataIsWaiting())
		{
			bool present = mainReader->cardPresent();
			if(present)
			{
				Uid uid;
				uint8_t res = mainReader->getUid(&uid);
				if(res != 0)
					continue;
				
				serial->write_p(PSTR("Uid: "));
				for(uint8_t i = 0; i < uid.size; ++i)
				{
					serial->write((int)uid.uidByte[i]);
					if(i < uid.size-1)
						serial->putChar(':');
				}
				serial->putChar('\n');
			}
		}
		setEnabled(enabled);
		serial->write_p(PSTR("Finished\n"));
		return 0;
	}
	else if(strcmp(inBuffer, "enable") == 0 )
	{
		if(enabled_)
		{
			serial->write_p(PSTR("Nfc tag listening allready enabled\n"));
			return -1;
		}
		setEnabled(true);
		serial->write_p(PSTR("Nfc tag listening enabled\n"));
		return 0;
	}
	else if(strcmp(inBuffer, "disable") == 0 )
	{
		if(!enabled_)
		{
			serial->write_p(PSTR("Nfc tag listening already disabled\n"));
			return -1;
		}
		setEnabled(false);
		serial->write_p(PSTR("Nfc tag listening disabled\n"));
		return 0;
	}
	else if(strcmp(inBuffer, "list") == 0 )
	{
		printNfcDevices();
		return 0;
	}
	else if(strcmp(inBuffer, "probe") == 0 )
	{
		bool enabled = enabled_;
		setEnabled(false);
		probe();
		printNfcDevices();
		setEnabled(enabled);
		return 0;
	}
	else if(strcmp(inBuffer, "read") == 0 )
	{
		int16_t addr = -1;
		int16_t csPin = -1;

		char* token = strtok(NULL, " ");
		if(token)
			csPin = strtol(token, nullptr, 16);
		if(csPin < 0 || csPin > 7)
		{
			serial->write_p(PSTR("A valid cs pin must be specified\n"));
			return -1;
		}
		token = strtok(NULL, " ");
		if(token)
			addr = strtol(token, nullptr, 16);
		if(addr < 0 || addr > 0xFF)
		{
			serial->write_p(PSTR("A address must be specified\n"));
			return -1;
		}
		
		uint8_t result = 0;
		for(uint8_t i = 0; i < readers.count(); ++i)
		{
			if(readers[i].type == TYPE_MFRC522)
			{
				if(readers[i].device.mfrc522.getCs() == csPin)
				{
					result = readers[i].device.mfrc522.read(addr);
					serial->write_p(PSTR("read sucessfull\n"));
				}
			}
			else
			{
				if(readers[i].device.proxy.getCs() == csPin)
				{
					result = readers[i].device.proxy.read(addr);
					serial->write_p(PSTR("read sucessfull\n"));
				}
			}
		}

		snprintf_P(buffer, SNPRINTF_BUFFER_SIZE, PSTR("Got: 0x%02x in return\n"), result);
		serial->write(buffer, SNPRINTF_BUFFER_SIZE);
		return 0;
	}
	else if(strcmp(inBuffer, "write") == 0 )
	{
		int16_t addr = -1;
		int16_t csPin = -1;
		int16_t data = -1;

		char* token = strtok(NULL, " ");
		if(token)
			csPin = strtol(token, nullptr, 16);
		if(csPin < 0 || csPin > 7)
		{
			serial->write_p(PSTR("A valid cs pin must be specified\n"));
			return -1;
		}
		token = strtok(NULL, " ");
		if(token)
			addr = strtol(token, nullptr, 16);
		if(addr < 0 || addr > 0xFF)
		{
			serial->write_p(PSTR("A address must be specified\n"));
			return -1;
		}
		token = strtok(NULL, " ");
		if(token)
			data = strtol(token, nullptr, 16);
		if(data < 0 || data > 0xFF)
		{
			serial->write_p(PSTR("Data must be specified\n"));
			return -1;
		}
		
		for(uint8_t i = 0; i < readers.count(); ++i)
		{
			if(readers[i].type == TYPE_MFRC522)
			{
				if(readers[i].device.mfrc522.getCs() == csPin)
				{
					readers[i].device.mfrc522.write(addr, data);
					serial->write_p(PSTR("Write sucessfull\n"));
				}
			}
			else
			{
				if(readers[i].device.proxy.getCs() == csPin)
				{
					serial->write_p(PSTR("Mfrc522 proxies are not writable\n"));
					return -1;
				}
			}
		}
		return 0;
	}
	return -3;
}

void NfcBoard::printTag(const uint8_t id, const Uid& uid, Serial* serial)
{
	serial->write("NFC ");
	serial->write((int)id);
	serial->write(" TAG ");
	for(uint8_t i = 0; i < uid.size; ++i)
	{
		serial->write((int)uid.uidByte[i]);
		if(i < uid.size-1)
			serial->putChar(':');
	}
	serial->putChar('\n');
}

void NfcBoard::proxyDetectCb(MfrcProxy* proxy, const Uid uid, void* data)
{
	NfcBoard* instance = reinterpret_cast<NfcBoard*>(data);

	printTag(proxy->getId(), uid, instance->serial);
}

void NfcBoard::detectCb(Mfrc522* reader, void* data)
{
	NfcBoard* instance = reinterpret_cast<NfcBoard*>(data);
	
	Uid uid;
	if(reader->getUid(&uid) == 0)
	{
		uint8_t i;
		for(i = 0; i < instance->readers.count(); ++i)
		{
			if(&instance->readers[i].device.mfrc522 == reader)
				break;
		}
		printTag(i+0x80, uid, instance->serial);
		reader->startTimeout(&timeoutCb, data);
	}
	else
	{
		reader->detectAsync(detectCb, data);
	}
}

void NfcBoard::timeoutCb(uint8_t ret, Mfrc522* reader, uint8_t* response, uint8_t responseLen, void* userData)
{
	reader->detectAsync(&detectCb, userData);
}