#include "libyhy.h"
#include <stdint.h>
#include <fcntl.h>
#include <termios.h>
#include <unistd.h>
#include <string.h>

#define MIN(a,b) (((a) < (b)) ? (a) : (b))
#ifdef DEBUG
	#include <stdio.h>
	#define DBG(x) (x)
	static void xxd(uint8_t* buf, size_t len){
		size_t n;
		for(n = 0; n < len; n++){
			printf("%02X ", buf[n]);
		}
	}
#else
	#define DBG(x)
#endif

#define DEFAULT_NODE_ID 0

//TODO
//0x AA substitution where applicable
//Confirm checksum on receive
//Handle status flags on return

#pragma pack(push, 1)
typedef struct {
	uint16_t magic; //0xAABB
	uint16_t csum_span;
	uint16_t node;
	uint16_t command;
} yhy_transmit_t;

typedef struct {
	uint16_t magic; //0xAABB
	uint16_t csum_span;
	uint16_t node;
	uint16_t command;
	uint8_t status; //0 on success
} yhy_response_t;
#pragma pack(pop)

enum /*_yhy_command*/ {
	YHY_SET_BAUDRATE = 0x0101,
		/* uint8_t baudrate_id
		 * 0 -> 4800
		 * 1 -> 9600
		 * 2 -> 14400
		 * 3 -> 19200
		 * 4 -> 38800
		 * 5 -> 38400
		 * 6 -> 57600
		 * 7 -> 115200
		 */
	YHY_SET_NODEID = 0x0102,
		/* uint16_t nodeid */
	YHY_GET_NODEID = 0x0103,
		/* No payload */
	YHY_READ_VERSION = 0x0104,
		/* No payload */
	YHY_BEEP = 0x0106,
		/* uint8_t beeptime in 10ms increments */
	YHY_LED = 0x0107,
		/* uint8_t LED status bitfield
		 * Bit 0 -> Green LED
		 * Bit 1 -> Red LED
		 */
	YHY_ANTENNA = 0x010C,
		/* uint8_t anntenna state
		 *	* 0 -> Antenna off
		 *	* 1 -> Antenna on
		 */
	YHY_MIFARE_REQUEST = 0x0201,
		/* uint8_t command
		 *	* 0x52 -> Wake up all cards (WUPA)
		 *	* 0x26 -> Request idle cards (REQA)
		 */
	YHY_MIFARE_ANTICOLL = 0x0202,
		/* No payload */
	YHY_MIFARE_SELECT = 0x0203,
		/* uint32_t Card UID */
	YHY_MIFARE_HLTA = 0x0204,
		/* No payload */
	YHY_MIFARE_AUTH = 0x0207,
		/* uint8_t auth_mode
		 *	* 0x60 -> Key A
		 *	* 0x61 -> Key B
		 * uint8_t block
		 * uint8_t[6] key
		 */
	YHY_MIFARE_READ = 0x0208,
		/* Payload uint8_t block */
	YHY_MIFARE_WRITE = 0x0209,
		/* uint8_t block
		 * uint8_t[16] data */
	YHY_ULTRALIGHT_SELECT = 0x0212,
		/* No payload */
	YHY_ULTRALIGHT_WRITE = 0x0213
		/* uint8_t page
		 * uint8_t[4] data */
};

static ssize_t yhy_send_command(int fd, uint16_t node, uint16_t cmd, uint8_t* payload, size_t len){
	size_t n;
	uint8_t checksum = 0x00;
	yhy_transmit_t header = {
		.magic = htobe16(0xAABB),
		.csum_span = htole16(len + 5),
		.node = node,
		.command = htole16(cmd)
	};

	for(n = 4; n < sizeof(header); n++){
		checksum ^= ((uint8_t*)(&header))[n];
	}

	for(n = 0; n < len; n++){
		checksum ^= payload[n];
	}
	
	DBG(printf("Out: "));
	DBG(xxd((uint8_t*) &header, sizeof(header)));
	DBG(xxd(payload, len));
	DBG(xxd(&checksum, 1));
	DBG(printf("\n"));

	write(fd, &header, sizeof(header));
	if(len){
		write(fd, payload, len);
	}
	write(fd, &checksum, 1);
	return 0;
}

static ssize_t yhy_receive_response(int fd, uint8_t* response, size_t max_len){
	//maximum received data is probably firmware version, assume 1024 bytes
	uint8_t recv_buffer[1024] = "";
	yhy_response_t* resp = (yhy_response_t*) recv_buffer;
	read(fd, recv_buffer, sizeof(yhy_response_t));

	//UL_SELECT returns wrong length from device
	if(resp->command == YHY_ULTRALIGHT_SELECT && resp->status == 0){
		DBG(printf("Fixing UL_SELECT response length (was %d)\n", le16toh(resp->csum_span)));
		resp->csum_span = htole16(0x0D);
	}

	uint16_t bytes_left = le16toh(resp->csum_span) - 5;
	read(fd, recv_buffer + sizeof(yhy_response_t), bytes_left);

	DBG(printf("In: "));
	DBG(xxd(recv_buffer, sizeof(yhy_response_t)));
	DBG(printf(" | "));
	DBG(xxd(recv_buffer + sizeof(yhy_response_t), bytes_left));
	DBG(printf("\n"));

	if(bytes_left > 1 && response && max_len){
		memcpy(response, recv_buffer + sizeof(yhy_response_t), MIN(bytes_left - 1, max_len));
	}
	return bytes_left - 1;
}

static ssize_t yhy_sync(int fd, uint16_t node, uint16_t cmd, uint8_t* payload, size_t payload_len, uint8_t* response, size_t response_len){
	yhy_send_command(fd, node, cmd, payload, payload_len);
	return yhy_receive_response(fd, response, response_len);
}

int yhy_open(char* device){
	struct termios tty;
	
	int fd = open(device, O_RDWR);
	if(fd < 0) {
		return -1;
	}

	if(tcgetattr(fd, &tty)){
		return -1;
	}

	cfsetospeed(&tty, B115200);
	cfsetispeed(&tty, B115200);

	tty.c_iflag &= ~(IXON | IXOFF | IXANY);
	tty.c_cflag |= (CLOCAL | CREAD);
	tty.c_cflag &= ~PARENB;
	tty.c_cflag &= ~CSTOPB;
	tty.c_cflag &= ~CSIZE;
	tty.c_cflag &= ~CRTSCTS;
	tty.c_cflag |= CS8;
	tty.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);

	if(tcsetattr(fd, TCSAFLUSH, &tty)){
		return -1;
	}

	return fd;
}

int yhy_sync_read_version(int fd, char* version, size_t max_len){
	ssize_t bytes = yhy_sync(fd, DEFAULT_NODE_ID, YHY_READ_VERSION, NULL, 0, (uint8_t*) version, max_len);
	if(bytes >= max_len){
		version[max_len] = 0;
	}
	else{
		version[bytes] = 0;
	}
	return bytes;
}

int yhy_sync_beep(int fd, uint8_t time){
	return yhy_sync(fd, DEFAULT_NODE_ID, YHY_BEEP, &time, 1, NULL, 0);
}

int yhy_sync_led(int fd, uint8_t led_bits){
	return yhy_sync(fd, DEFAULT_NODE_ID, YHY_LED, &led_bits, 1, NULL, 0);
}

int yhy_sync_antenna(int fd, uint8_t enable){
	uint8_t status = enable ? 1 : 0;
	return yhy_sync(fd, DEFAULT_NODE_ID, YHY_ANTENNA, &status, 1, NULL, 0);
}

int yhy_sync_request(int fd, uint8_t wakeup, uint16_t* atqa){
	*atqa = 0;

	//Select WUPA or REQA
	uint8_t wup = wakeup ? 0x52 : 0x26;
	return yhy_sync(fd, DEFAULT_NODE_ID, YHY_MIFARE_REQUEST, &wup, 1, (uint8_t*) atqa, 2);
}

int yhy_sync_anticoll(int fd, uint8_t* serial, size_t length){
	return yhy_sync(fd, DEFAULT_NODE_ID, YHY_MIFARE_ANTICOLL, NULL, 0, serial, length);
}

int yhy_sync_select(int fd, uint8_t* serial, size_t length, uint8_t* sak){
	*sak = 0;
	return yhy_sync(fd, DEFAULT_NODE_ID, YHY_MIFARE_SELECT, serial, length, sak, 1);
}

int yhy_sync_hlta(int fd){
	return yhy_sync(fd, DEFAULT_NODE_ID, YHY_MIFARE_HLTA, NULL, 0, NULL, 0);
}

int yhy_sync_auth(int fd, uint8_t key_a, uint8_t block, uint8_t key[6]){
	struct {
		uint8_t mode;
		uint8_t block;
		uint8_t key[6];
	} auth_payload = {
		.mode = (key_a ? 0x60 : 0x61),
		.block = block
	};
	memcpy(&(auth_payload.key), key, 6);
	return yhy_sync(fd, DEFAULT_NODE_ID, YHY_MIFARE_AUTH, (uint8_t*) (&auth_payload), sizeof(auth_payload), NULL, 0);
}

//For some reason, this fails with blocks 0 and 10 on all NTAGs
int yhy_sync_read(int fd, uint8_t block, uint8_t* data, size_t max_len){
	return yhy_sync(fd, DEFAULT_NODE_ID, YHY_MIFARE_READ, &block, 1, data, max_len);
}

int yhy_sync_write(int fd, uint8_t block, uint8_t data[16]){
	struct {
		uint8_t block;
		uint8_t data[16];
	} write_payload = {
		.block = block
	};
	memcpy(&(write_payload.data), data, 16);
	return yhy_sync(fd, DEFAULT_NODE_ID, YHY_MIFARE_WRITE, (uint8_t*) (&write_payload), sizeof(write_payload), NULL, 0);
}

int yhy_sync_ulselect(int fd, uint8_t* serial, size_t length){
	return yhy_sync(fd, DEFAULT_NODE_ID, YHY_ULTRALIGHT_SELECT, NULL, 0, serial, length);
}

int yhy_sync_ulwrite(int fd, uint8_t block, uint8_t data[4]){
	struct {
		uint8_t block;
		uint8_t data[4];
	} write_payload = {
		.block = block
	};
	memcpy(&(write_payload.data), data, 4);
	return yhy_sync(fd, DEFAULT_NODE_ID, YHY_ULTRALIGHT_WRITE, (uint8_t*) (&write_payload), sizeof(write_payload), NULL, 0);
}