#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),
	connected(false)
{
	hid_init();
}



/*
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() {
	disconnect();
	hid_exit();
}



/*
Try to open this keyboard's hid device.
Throws a runtime error if device is already open.

If we successfully open a device, this method sends a
CMD_HELLO packet.

@returns True if connection was successful, false otherwise.
*/
bool Ergodox::try_connect() {
	if (connected) {
		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;
	}

	// Return false if connection failed
	if (path_to_open) {
		connected = true;

		handle = hid_open_path(path_to_open);
		hid_set_nonblocking(handle, 1);
		hid_free_enumeration(devs);
		write(CMD_HELLO, NULL, 0);

		#define MAX_STR 255
		wchar_t wstr[MAX_STR];
		char nstr[MAX_STR];


		spdlog::info("Connected to device!");

		// Read metadata
		wstr[0] = 0x0000;
		hid_get_manufacturer_string(handle, wstr, MAX_STR);
		misc::to_narrow(wstr, nstr, MAX_STR);
		spdlog::info("Manufacturer String: {0}", nstr);

		wstr[0] = 0x0000;
		hid_get_product_string(handle, wstr, MAX_STR);
		misc::to_narrow(wstr, nstr, MAX_STR);
		spdlog::info("Product String: {0}", nstr);

	} else {
		connected = false;

		hid_free_enumeration(devs);
		return false;
	}

	return connected;
}


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


/*
Send data to Ergodox.

@param cmd Command byte
@param data Data to send
@param data_len Data length. Must be shorter than packet_size.
@returns True if successful, false otherwise.
*/
bool Ergodox::write(uint8_t cmd, const uint8_t* data, uint8_t data_len) {
	if (!connected) {
		throw std::runtime_error("Not connected, cannot write!");
	} else if (handle == NULL) {
		throw std::runtime_error("Tried to write a null handle, something is very wrong.");
	} else 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);

	ssize_t res;
	res = hid_write(handle, packet, packet_size + 1);

	if (res < 0) {
		spdlog::info("Lost device, disconnecting.");
		disconnect();
		return false;
	}
	return true;
}


/*
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 (!connected) {
		throw std::runtime_error("Not connected, cannot read!");
	} else if (handle == NULL) {
		throw std::runtime_error("Tried to read a null handle, something is very wrong.");
	}

	ssize_t res;
	res = hid_read(handle, read_buf, packet_size);

	if (res == 0) {
		return false;
	} else if (res < 0) {
		spdlog::info("Lost device, disconnecting.");
		disconnect();
		return false;
	}

	switch(read_buf[0]) {

		// If keyboard sends a state packet, parse it.
		case CMD_SEND_STATE:

			// Byte 1: animation mode
			if (animation_mode != read_buf[1]) {
				spdlog::info("Mode set to 0x{0:02x}", read_buf[1]);
				animation_mode = read_buf[1];
			}

			// Bytes 2,3,4,5: layer state
			uint32_t new_layer_state =
				(read_buf[5] << 24) |
				(read_buf[4] << 16) |
				(read_buf[3] << 8) |
				(read_buf[2] << 0);

			if (layer_state != new_layer_state) {
				layer_state = new_layer_state;
				spdlog::info("Layer set to 0b{0:032b}", layer_state);
			}

			// Byte 6: desired OS layout for layer
			if (layer_layout != read_buf[6]) {
				layer_layout = read_buf[6];
				spdlog::info("Layout set to 0x{0:02x}", read_buf[6]);
			}


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

	return true;
}


// Simple connectivity check
void Ergodox::test_connection() {
	write(CMD_RUTHERE, NULL, 0);
}

// Block until a connection is established.
void Ergodox::connect_loop() {
	spdlog::info("Trying to connect...");
	while (!connected) {
		try_connect();
		std::this_thread::sleep_for(std::chrono::milliseconds(RECONNECT_SLEEP_MS));
	}
}