#include "ISO7816-reader.h"
#include <avr/pgmspace.h>

//#define F_CARD 3579545UL //Card external oscillator frequency
#define F_CARD (F_CPU/2)

#define FI_DEFAULT 372
#define FI_RFU FI_DEFAULT //sane default for reseved value
#define DI_DEFAULT 1
#define DI_RFU DI_DEFAULT //sane default for reseved value
uint16_t rate_conversion_fi[16] = {372, 372, 558,744,1116, 1488, 1860, FI_RFU, FI_RFU, 512, 768, 1024, 1536, 2024, FI_RFU, FI_RFU};
uint16_t rate_adjustment_di[16] = {DI_RFU, 1,2,4,8,16,32,64, 12,20, DI_RFU, DI_RFU, DI_RFU, DI_RFU, DI_RFU, DI_RFU};


#define USART_NUMBER 0
#define USART_NAME CARD
#define BAUD (F_CARD*DI_DEFAULT/FI_DEFAULT) //initial baudrate
#define PARITY EVEN
#define STOPBITS 2
#include "../common/serial.h"

uint16_t ISO7816_read_byte(void);

// uint16_t char_guard_time;
// uint16_t block_guard_time;

uint8_t seq_number = 0;

#ifdef CARD_PRESENCE_INTERUPT
	//TODO: FixMe: XPASTE doesn't work here, why?
	#define INTVECT_NAME INT2_vect //XPASTE(CARD_PRESENCE_INTERUPT, _vect)
	ISR(INTVECT_NAME){
		ISO7816_card_presence_changed(ISO7816_is_card_present());
	}
#endif

void ISO7816_init(void){
	CARD_init(); //Init serial interface.that the card connects to
	
	CARD_PRESENCE_REG_DDR &= (uint8_t)~(1 << 2); //Sense pin as input
	CARD_PRESENCE_REG_PORT |= (1 << 2); //Sense pullup 
	
	//Reset and power pin as output
	CARD_RESET_REG_DDR |= (1 << CARD_RESET_PIN);
	CARD_POWER_REG_DDR |= (1 << CARD_POWER_PIN);
	
#ifdef CARD_PRESENCE_INTERUPT
	EIMSK |= (1 << CARD_PRESENCE_INTERUPT);
	EICRA |= (1 << CARD_PRESENCE_INTERUPT_EDGE); 
#endif
	
}

uint8_t ISO7816_is_card_present(void){
	return (CARD_PRESENCE_REG_PIN & (1 << CARD_PRESENCE_PIN)) == 0;
}

uint8_t ISO7816_card_power(uint8_t on){
	if (on){
		CARD_POWER_REG_PORT |= (1 << CARD_POWER_PIN);
	} else {
		CARD_POWER_REG_PORT &= ~(1 << CARD_POWER_PIN);
	}
}

void ISO7816_card_reset(void) {
	CARD_RESET_REG_DDR |= (1 << CARD_RESET_PIN); //Reset pin as output
	CARD_RESET_REG_PORT &= (uint8_t)~(1 << CARD_RESET_PIN);
	_delay_ms(10); //pull reset line low for a while to reset the card.
	CARD_RESET_REG_PORT |= (1 << CARD_RESET_PIN);
}

uint8_t ISO7816_readATR(ISO7816_ATR* atr){
	uint16_t timeout;
	uint8_t c = 0;
	uint8_t checksum = 0;
	
	CARD_init();  //Reset serial interface (Specifically the Baudrate to default for reading the ATR)
	CARD_trash_receive_buffer(); //Discard anything in the HW receive buffers
	
	ISO7816_card_reset(); //Reset the card itself
	
	seq_number = 0;
	
	while (c != 0x3B){
		timeout = 2000;
		while (!CARD_data_available() && ISO7816_is_card_present() && timeout != 0) {timeout--; _delay_ms(1);}
		if (!ISO7816_is_card_present()) return ERR_ATR_CARD_REMOVED;
		if (timeout == 0) return ERR_ATR_TIMEOUT;
		c = CARD_read_char();
	}
	atr->initial_char = c;
	
	uint16_t ret;
	
	//Read T0
	ret = ISO7816_read_byte();
	if (!ISO7816_is_card_present()) return ERR_ATR_CARD_REMOVED;
	if (ret & 0xFF00) return ERR_ATR_TIMEOUT;
	c = ret & 0xFF;
	checksum ^= c;
	atr->t0 = c;
	
	uint8_t num_hist_bytes = NUM_HIST_BYTES(*atr);
	uint8_t next_interface_bytes = INTERFACE_BYTE_SET(*atr, 0);
	uint8_t i = 0;
	while (next_interface_bytes){
		if (i >= ATR_MAX_INTERFACE_BYTE_SETS) return ERR_TOO_MANY_ATR_INTERFACE_BYTES;
		
		if (next_interface_bytes & 0x01) {
			ret = ISO7816_read_byte();
			if (ret & 0xFF00) return ret;
			c = ret & 0xFF;
			checksum ^= c;
			atr->interface_bytes[i].ta = c;
		}
		
		if (next_interface_bytes & 0x02) {
			ret = ISO7816_read_byte();
			if (ret & 0xFF00) return ret;
			c = ret & 0xFF;
			checksum ^= c;
			atr->interface_bytes[i].tb = c;
		}
		
		if (next_interface_bytes & 0x04) {
			ret = ISO7816_read_byte();
			if (ret & 0xFF00) return ret;
			c = ret & 0xFF;
			checksum ^= c;
			atr->interface_bytes[i].tc = c;
		}
		if (next_interface_bytes & 0x08) {
			ret = ISO7816_read_byte();
			if (ret & 0xFF00) return ret;
			c = ret & 0xFF;
			checksum ^= c;
			atr->interface_bytes[i].td = c;
			next_interface_bytes = c >> 4;
		} else {
			next_interface_bytes = 0; //No TD => No more interface bytes
		}
		i++;
	}
	for (i = 0; i < num_hist_bytes; i++){
		ret = ISO7816_read_byte();
		if (!ISO7816_is_card_present()) return ERR_ATR_CARD_REMOVED;
		if (ret & 0xFF00) return ERR_ATR_TIMEOUT;
		c = ret & 0xFF;
		
		checksum ^= c;
		atr->historical_bytes[i] = c;
	}
	
	ret = ISO7816_read_byte();
	if (!ISO7816_is_card_present()) return ERR_ATR_CARD_REMOVED;
	if (ret & 0xFF00) return ERR_ATR_TIMEOUT;
	c = ret & 0xFF;
	
	atr->checksum = c;
	if (checksum != c) return ERR_ATR_CHECKSUM_FAIL;
	
	return 0;
}

// void ISO7816_dumpATR(ISO7816_ATR* atr){
// 	uint8_t num_hist_bytes = atr->t0 & 0x0F;
// 	uint8_t next_interface_bytes = atr->t0 >> 4;
// 	printf_P(PSTR("-- ATR Dump --\n"));
// 	printf_P(PSTR("TS: 0x%02X\n"), atr->initial_char);
// 	printf_P(PSTR("T0: 0x%02X (nHist: %i)\n"), atr->t0, num_hist_bytes);
// 	
// 	uint8_t i = 0;
// 	while (next_interface_bytes){
// 		if (next_interface_bytes & 0x01) {printf_P(PSTR("TA%i: 0x%02X  "), i+1, atr->interface_bytes[i].ta);}
// 		if (next_interface_bytes & 0x02) {printf_P(PSTR("TB%i: 0x%02X  "), i+1, atr->interface_bytes[i].tb);}
// 		if (next_interface_bytes & 0x04) {printf_P(PSTR("TC%i: 0x%02X  "), i+1, atr->interface_bytes[i].tc);}
// 		if (next_interface_bytes & 0x08) {printf_P(PSTR("TD%i: 0x%02X (Proto: T=%i)"), i+1, atr->interface_bytes[i].td, ATR_PROTO(*atr, i));}
// 		printf_P(PSTR("\n"));
// 		next_interface_bytes = atr->interface_bytes[i].td >> 4;
// 		i++;
// 	}
// 	printf_P(PSTR("Historical Chars: "));
// 	for (i = 0; i < num_hist_bytes; i++){
// 		printf_P(PSTR("%c"), atr->historical_bytes[i]);
// 	}
// 	printf_P(PSTR("\nChecksum: 0x%02X\n"), atr->checksum);
// 	printf_P(PSTR("-- End ATR ---\n"));
// }
// 

// uint8_t ISO7816_find_extended_interface_byte_set(ISO7816_ATR *atr);
// 
// uint8_t ISO7816_set_protol(ISO7816_ATR* atr){
// 	uint16_t Fi;
// 	uint16_t Di;
// 	
// 	if (ATR_PROTO(*atr, 0) != 1) return ERR_PROTOCOL_NOT_SUPPORTED;
// 	
// 	//Extract and calculate baudrate from ATR (TA1)
// 	Fi = rate_conversion_fi[ATR_FI(*atr,0)];
// 	Di = rate_adjustment_di[ATR_DI(*atr,0)];
// 	uint32_t baud = F_CARD*(uint32_t)Di/(uint32_t)Fi;
// 	
// 	//1 etu = F/D/F_CARD //TODO: Are F and D the default values ot Fi, Di?
// 	uint16_t etu = (Fi * 1000000UL) / Di / F_CARD; //µs
// 	
// 	uint8_t N = ATR_N(*atr,0);
// // 	uint16_t char_guard_time;
// 	//uint16_t
// 	block_guard_time = 22*etu;
// 	
// 	if (N == 255){
// 		char_guard_time = 11*etu; //µS, Special case for T=1. See ISO7816-3 Chapter 11.2
// 	} else {
// 		if (ISO7816_find_extended_interface_byte_set(atr)){
// 			//Use values derived from TA1 as factor (R)
// 			char_guard_time = 12*etu + ((uint32_t)Fi*1000000UL*N)/Di/F_CARD;//µS
// 		} else {
// 			//Use default values for F and D as factor (R)
// 			char_guard_time = 12*etu + (FI_DEFAULT*1000000UL*N)/DI_DEFAULT/F_CARD;//µS
// 		}
// 	}
// 	
// 	CARD_switch_baudrate(baud);
// 	
// 	printf_P(PSTR("-- Interface Configuration --\n"));
// 	printf_P(PSTR("Fi: %i, Di: %i, N: %i\n"), Fi, Di, N);
// 	printf_P(PSTR("Baudrate: %lu bit/s, Char GuardTime: %iµS, Block GuardTime: %iµS\n"), baud, char_guard_time, block_guard_time);
// 	
// 	return 0;
// }

// /*
//  * Finds the first set of interface bytes encoding T=15.
//  * The presence of this indicates which time base to take for Guard time calculation.
//  * Whoever came up with this: Please shoot yourself before you reproduce.
//  */
// uint8_t ISO7816_find_extended_interface_byte_set(ISO7816_ATR *atr){
// 	uint8_t next_interface_bytes = INTERFACE_BYTE_SET(*atr,0);
// 	uint8_t i = 0;
// 	while (next_interface_bytes){
// 		if (ATR_PROTO(*atr, i) == 15) return 15;
// 		i++;
// 		next_interface_bytes = INTERFACE_BYTE_SET(*atr,i);
// 	}
// 	return 0; //T=15 is not valid in entry 0 so we can use this as "not found or invalid" result.
// }

uint16_t ISO7816_read_byte(void){
	uint16_t timeout = 3000; //0,3 sec
	while(!CARD_data_available() && (ISO7816_is_card_present()) && timeout) {timeout--; _delay_us(100);}
	if (! ISO7816_is_card_present()){
		return ISO7816_ERR_CARD_REMOVED;
	} else if (CARD_data_available()){
		uint8_t c = CARD_read_char();
		return c;
	} else {
		return ISO7816_ERR_TIMEOUT;
	}
}

/**
 * Sends an APDU to the card and reads the response
 * @param apdu_header pointer to the APDU Header
 * @param apdu_payload pointer to the APDU Payload (body).
 * @param apdu_payload_length length of the APDU Payload. Allowed values: 0 to 250 (inclusive)
 * @param answer pointer to a buffer where the answer will be written to.
 * @param answer_payload_length length of the expected answer.
 * @returns The number of answer payload bytes (should be answer_payload_length, but can be smaller)
 *          OR 0xFFFF if a timeout occured
 * 			OF 0xFFFE if a checksum error occured
 * WARNING: the answer buffer should be at least answer_payload_length bytes long to prevent buffer overflows
**/
uint16_t ISO7816_send_apdu(ISO7816_APDU_Header* apdu_header,
						   uint8_t*  apdu_payload,
						   uint8_t   apdu_payload_length,
						   uint16_t* answer_status,
						   uint8_t*  answer_payload,
						   uint8_t   answer_payload_length){
	uint8_t c;
	uint8_t checksum = 0;
	
	CARD_switch_mode_transmit();
	
	printf("=> ");
		
	//NAD (Node Address, not used)
	c = 0x00; checksum ^= c; printf("%02x ", c); CARD_transmit_char(c);
	
	//Protocol control byte
	c = seq_number ? 0x40 : 0x00; checksum ^= c; printf("%02x ", c); CARD_transmit_char(c);
	seq_number = !seq_number;
	
	//Message Length
	uint8_t apdu_legth = sizeof(ISO7816_APDU_Header);
	if (apdu_payload_length)   apdu_legth += apdu_payload_length + 1;
	if (answer_payload_length) apdu_legth++;
	c = apdu_legth; checksum ^= c; printf("%02x ", c); CARD_transmit_char(c);
	
	//APDU Header
	for (uint8_t i = 0; i < sizeof(ISO7816_APDU_Header); i++){
		c = ((uint8_t*)apdu_header)[i]; checksum ^= c; printf("%02x ",c); CARD_transmit_char(c);
	}
	
	//APDU Payload length (if 0 this must not be sent)
	if (apdu_payload_length){
		c = apdu_payload_length; checksum ^= c; printf("%02x ", c); CARD_transmit_char(c);
	}
	
	//APDU Payload
	for (uint16_t i = 0; i < apdu_payload_length; i++){
		c = apdu_payload[i]; checksum ^= c; printf("%02x ", c); CARD_transmit_char(c);
	}
	
	//If a 0 byte answer is expected, LE must not be send
	if (answer_payload_length){
		//Send expected answer length byte
		c = answer_payload_length; checksum ^= c; printf("%02x ", c); CARD_transmit_char(c);
	}
	
	printf("%02x ", checksum);
	CARD_transmit_char(checksum);
	
	CARD_switch_mode_receive();
	
	printf("\n<= ");
	
	//########### Read Answer ###########
	
	uint16_t ret;	
	
	checksum = 0;
	
	//Answer Node Address (don't care)
	ret = ISO7816_read_byte();
	if (ret & 0xFF00){
		ret = ISO7816_read_byte(); //Retry for the first byte (the card may take some time to answer).
		if (ret & 0xFF00) return ISO7816_ERR_TIMEOUT;
	}
	printf("%02x ", ret & 0xFF);
	checksum ^= ret & 0xFF;
	
	//Protocol control byte (don't care)
	ret = ISO7816_read_byte();
	if (ret & 0xFF00) return ISO7816_ERR_TIMEOUT;
	printf("%02x ", ret & 0xFF);
	checksum ^= ret & 0xFF;
	
	//Result APDU Length
	ret = ISO7816_read_byte();
	if (ret & 0xFF00) return ISO7816_ERR_TIMEOUT;
	printf("%02x ", ret & 0xFF);
	checksum ^= ret & 0xFF;
	uint8_t result_apdu_length = ret & 0xFF;
	
	answer_payload_length = MIN(answer_payload_length, result_apdu_length - 2);
	
	//Answer Body
	for (uint16_t i = 0; i < answer_payload_length; i++){
		ret = ISO7816_read_byte();
		if (ret & 0xFF00) return ISO7816_ERR_TIMEOUT;
		printf("%02x ", ret & 0xFF);
		checksum ^= ret & 0xFF;
		answer_payload[i] = ret & 0xFF;
	}
	
	//Answer Status byte 1 (High)
	ret = ISO7816_read_byte();
	if (ret & 0xFF00) return ISO7816_ERR_TIMEOUT;
	printf("%02x ", ret & 0xFF);
	checksum ^= ret & 0xFF;
	*answer_status = ret << 8;
	
	//Answer Status byte 2 (Low)
	ret = ISO7816_read_byte();
	if (ret & 0xFF00) return ISO7816_ERR_TIMEOUT;
	printf("%02x ", ret & 0xFF);
	checksum ^= ret & 0xFF;
	*answer_status |= ret & 0xFF;
	
	
	//Checksum
	ret = ISO7816_read_byte();
	if (ret & 0xFF00) return ISO7816_ERR_TIMEOUT;
	printf("%02x ", ret & 0xFF);
	if (checksum != (ret & 0xFF)) return ISO7816_ERR_CHECKSUM_FAIL;
	
	printf("\n");
	return answer_payload_length;
	
}