#include "ergodox.hpp"


Ergodox::Ergodox(
	unsigned short vendor_id,
	unsigned short product_id,
	unsigned short usage,
	unsigned short usage_page
) :
	vendor_id(vendor_id),
	product_id(product_id),
	usage(usage),
	usage_page(usage_page),
	handle(NULL)
{
	hid_init();
	open();
}



/*
Initialize Ergodox interface.

Should be called once at start of execution. Arguments are the USB parameters of the keyboard.

Returns instance of singleton.
*/
Ergodox& Ergodox::init(
	unsigned short vendor_id,
	unsigned short product_id,
	unsigned short usage,
	unsigned short usage_page
) {
	static bool has_been_initialized = false;
	static Ergodox Instance(
		vendor_id,
		product_id,
		usage,
		usage_page
	);

	// This isn't strictly necessary, but there is no reason
	// to call init() twice.
	if (has_been_initialized) {
		has_been_initialized = true;
		throw std::runtime_error("Ergodox has already been initialized!");
	}

	return Instance;
}


// Instance is a static function variable,
// and will be deleted at end of program.
Ergodox::~Ergodox() {
	close();
	hid_exit();
}


/*
Try to open this keyboard's hid device.
Throws a runtime error if device is already open,
or if we can't find a device with the given params.

If we successfully open a device, this method sends a
CMD_HELLO packet.
*/
void Ergodox::open() {
	if (handle != NULL) {
		throw std::runtime_error("Device already open");
	}

	struct hid_device_info *devs, *cur_dev;
	const char *path_to_open = NULL;

	devs = hid_enumerate(vendor_id, product_id);
	cur_dev = devs;
	while (cur_dev) {
		if (cur_dev->vendor_id == vendor_id &&
			cur_dev->product_id == product_id &&
			cur_dev->usage == usage &&
			cur_dev->usage_page == usage_page
		) {
			path_to_open = cur_dev->path;
			break;
		}
		cur_dev = cur_dev->next;
	}

	if (path_to_open) {
		handle = hid_open_path(path_to_open);
		hid_free_enumeration(devs);
	} else {
		hid_free_enumeration(devs);
		throw std::runtime_error("Could not open hid device");
	}

	hid_set_nonblocking(handle, 1);

	write(CMD_HELLO, NULL, 0);
}


// Close hid device if it is open.
void Ergodox::close() {
	if (handle != NULL) {
		hid_close(handle);
	}
}


/*
Send data to Ergodox.

@param cmd Command byte
@param data Data to send
@returns data_len Data length. Must be shorter than packet_size.
*/
int Ergodox::write(uint8_t cmd, const uint8_t* data, uint8_t data_len) const {
	if (handle == NULL) {
		throw std::runtime_error("Cannot write, device is not open!");
	}

	if (data_len > packet_size) {
		throw std::runtime_error("Data length exceeds packet size");
	}

	if (data == NULL) {
		data = {0x00};
	}

	uint8_t packet[packet_size];
	packet[0] = 0x00; // Report number. Not seen by keyboard.
	packet[1] = cmd;  // First byte is always command

	// Copy data into rest of packet
	std::copy(data, data + data_len, packet + 2);

	return hid_write(handle, packet, packet_size + 1);
}


/*
Read data from Ergodox into read_buf. If a CMD_SEND_STATE packet is received, process it and return false.

@returns True if a new command was received, false otherwise.
*/
bool Ergodox::read() {
	if (handle == NULL) {
		throw std::runtime_error("Cannot write, device is not open!");
	}
	ssize_t res;
	res = hid_read(handle, read_buf, packet_size);

	if (res == 0) {
		return false;
	} else if (res < 0) {
		throw std::runtime_error("Read failed.");
	}

	switch(read_buf[0]) {

		// If keyboard sends a state packet, parse it.
		case CMD_SEND_STATE:
			if (animation_mode != read_buf[1]) {
				wprintf(L"Mode set to %#x\n", read_buf[1]);
				animation_mode = read_buf[1];
			}

			// Main code should not parse state packets.
			return false;
	}

	return res > 0;
}