#include <Arduino.h>
#include <SoftwareSerial.h>
#include "SM130.h"

SoftwareSerial rfid(7, 8);

SM130::SM130() {
	rfid.begin(19200);
}

bool SM130::send_command(uint8_t cmd, const uint8_t *cmd_args, const uint8_t cmd_args_length, uint8_t *ret, uint8_t *length) {
	uint8_t length_cmd = 1 + cmd_args_length;	//(1) + les arguments
	uint8_t crc = length_cmd + cmd;			//taille totale

	Serial.print("<command 0x");			//Print de la commande
	Serial.print(cmd, HEX);
	Serial.print("> Sent !");
	Serial.println();

	rfid.write((uint8_t)0xFF);			//Le header qui fait 0xFF
	rfid.write((uint8_t)0x00);			//Reserved byte toujours à 0x00
	rfid.write((uint8_t)length_cmd);		//Lenght of the payload data
	rfid.write((uint8_t)cmd);			//byte of the instruction command
	for (int i = 0; i < cmd_args_length; i++) {	//Datas de taille N bytes dans une boucle
		rfid.write((uint8_t)cmd_args[i]);
		crc += cmd_args[i];			// on incrémente le CRC
	}
	rfid.write((uint8_t)crc);			//on write le CRC

	return read_response(ret, length, NULL);
}

bool SM130::read_response(uint8_t *ret, uint8_t *length, uint8_t *cmd) {
	uint8_t crc = 0;
	uint8_t ret_header, reserved, ret_length, ret_cmd, ret_crc;
	
	Serial.println("<read_response>");
	
	//Header byte
	ret_header = rfid_read_sync();			//on recup le 1er byte de réponse qui doit etre 0xFF toujours
	if(ret_header != 0xFF) {			//et on le test ;)
		Serial.print("Bad header: 0x");
		Serial.print(ret_header, HEX);
		Serial.print(". Must be 0xFF !");
		goto error;
	}
	//Reserved Byte						//On incremente pas CRC du header byte d'apres la doc
	reserved = rfid_read_sync();			//et on continue !
	if(reserved != 0x00) {				//on test le byte reserved qui doit être à 0x00
		Serial.print("Bad reserved = 0x");
		Serial.print(ret_header, HEX);
		Serial.print(". Must be 0x00 !");
		goto error;
	}
	crc = reserved;					//on oublie pas le $ù%#[[ de CRC

	//Length byte
	ret_length = rfid_read_sync();
	Serial.print("Length = 0x");
	Serial.print(ret_length, HEX);
	Serial.println();
	crc += ret_length;

	//Command byte
	ret_cmd = rfid_read_sync();			//On est censé récupérer la même commande que le send_command
	if(cmd) {				//On test
		*cmd = ret_cmd;
	}
	ret_length--;
	Serial.print("command = 0x");
	Serial.print(ret_cmd, HEX);
	Serial.println();
	crc += ret_cmd;

	//Lecture de la réponse complête
	Serial.print("response = ");
	int i;
	while (i < ret_length) {
		ret[i] = rfid_read_sync();
		crc += ret[i];

		Serial.print("0x");
		Serial.print(ret[i], HEX);
		Serial.print (" ");
		i++;
	}
	
	Serial.println();

	//On check le CRC
	ret_crc = rfid_read_sync();
	if (ret_crc != crc) {
		Serial.print("Bad CRC 0x");
		Serial.print(ret_crc, HEX);
		Serial.print(" ! Should be : 0x");
		Serial.print(crc, HEX);
		Serial.println();
		return false;
	}

	*length = ret_length;
	Serial.println("</response>");
	return true;

error:
	while(rfid.available())
		rfid.read();

	Serial.println("</response>");
	return false;
	

}

uint8_t SM130::rfid_read_sync() {
	while(!rfid.available());
	return rfid.read();
}

bool SM130::seek_tag(uint8_t *id, uint8_t *length) {
	uint8_t cmd, tag_type, tag_len;
	uint8_t buf[10], buf_len =10, ret[10], len = 10;
	int i;
	send_command(0x82, NULL, 0, ret, &len);					//send de la commande 0x82 seek for tag
	read_response(buf, &buf_len, &cmd);
	Serial.println("<seek_tag>");

	if(cmd != 0x82) {							//check si c'est bien la bonne commande
		Serial.print("Bad command 0x");
		Serial.print(cmd, HEX);
		Serial.print(". Should be 0x82\r");
		goto error;
	}

	tag_type = buf[0];							//on à le type de tag ici
	switch (tag_type) {								//check de quel type de tag il s'agit (voir datasheet)
		case 0x01:
			tag_len = 7;
			Serial.println("tag_type = Mifare UltraLight");
			break;

		case 0x02:
			tag_len = 4;
			Serial.println("tag_type = Mifare Santard 1K");
			break;

		case 0x03:
			tag_len = 4;
			Serial.println("tag_type = Mifare Classic 4K");
			break;

		default:							//tag moisi
			Serial.print("Error : Unknown Mifare tag :");
			Serial.print(tag_type, HEX);
			Serial.println();
			goto error;

	}

	Serial.print("Tag serial = ");
	for (i = 0; i < tag_len; i++) {
		id[i] = buf[tag_len - i];
		Serial.print(id[i], HEX);
		Serial.print(" ");
	}

	Serial.println();
	*length = tag_len;
	Serial.println("</seek_tag>");
	return true;

error:
	while(rfid.available())
		rfid.read();
	Serial.println("</seek_tag>");
	return false;



}