#include <opengl-playground/imu-serial.hpp>
#include <iostream>
#include <stdio.h>
#include <string.h>
// Linux headers
#include <fcntl.h> // Contains file controls like O_RDWR
#include <errno.h> // Error integer and strerror() function
#include <termios.h> // Contains POSIX terminal control definitions
#include <unistd.h> // write(), read(), close()
#include <chrono>
#include <string>

ImuSerial::ImuSerial(const std::string &serial_device, unsigned int baud)
{
	this->quaternion = glm::quat(1.0f, 0.0f, 0.0f, 0.0f);
	this->serial_device = serial_device;
	this->baud = baud;
	fd = -1;
	stop_flag = 0;

	this->acc = this->gyr = glm::vec3(0.0f, 0.0f, 0.0f);
}

ImuSerial::~ImuSerial()
{
	if (this->fd >= 0)
		this->stop();
}


static speed_t baudrate(int baud)
{
    switch (baud) {
    case 9600:
	return B9600;
    case 19200:
	return B19200;
    case 38400:
	return B38400;
    case 57600:
	return B57600;
    case 115200:
	return B115200;
    case 230400:
	return B230400;
    case 460800:
	return B460800;
    case 500000:
	return B500000;
    case 576000:
	return B576000;
    case 921600:
	return B921600;
    case 1000000:
	return B1000000;
    case 1152000:
	return B1152000;
    case 1500000:
	return B1500000;
    case 2000000:
	return B2000000;
    case 2500000:
	return B2500000;
    case 3000000:
	return B3000000;
    case 3500000:
	return B3500000;
    case 4000000:
	return B4000000;
    default:
	return -1;
    }
}

static int open_serial(const char *serdev, unsigned int baud)
{
	int fd;
	struct termios tty;

	fd = open(serdev, O_RDWR);
	memset(&tty, 0, sizeof(tty));

	if (fd < 0) {
		std::cout << "Serial port not found or cannot be opened" << std::endl;
		return fd;
	}

	if (tcgetattr(fd, &tty) != 0) {
		std::cout << "Error getting serial configuration" << std::endl;
		close(fd);
		return -1;
	}

	tty.c_cflag &= ~PARENB;
	tty.c_cflag &= ~CSTOPB;
	tty.c_cflag |= CS8;
	tty.c_cflag &= ~CRTSCTS;
	tty.c_cflag |= CREAD | CLOCAL;
	tty.c_lflag &= ~ICANON;
	tty.c_lflag &= ~ECHO; // Disable echo
	tty.c_lflag &= ~ECHOE; // Disable erasure
	tty.c_lflag &= ~ECHONL; // Disable new-line echo
	tty.c_lflag &= ~ISIG; // Disable interpretation of INTR, QUIT and SUSP
	tty.c_lflag &= ~ISIG; // Disable interpretation of INTR, QUIT and SUSP
	tty.c_iflag &= ~(IGNBRK|BRKINT|PARMRK|ISTRIP|INLCR|IGNCR|ICRNL); // Disable any special handling of received bytes
	tty.c_oflag &= ~OPOST; // Prevent special interpretation of output bytes (e.g. newline chars)
	tty.c_oflag &= ~ONLCR; // Prevent conversion of newline to carriage return/line feed
	tty.c_cc[VTIME] = 0;    // Wait for up to 1s (10 deciseconds), returning as soon as any data is received.
	tty.c_cc[VMIN] = 0;

	cfsetispeed(&tty, baudrate(baud));
	if (tcsetattr(fd, TCSANOW, &tty) != 0) {
	    std::cout << "Error setting serial config" << std::endl;
	}

	return fd;
}

float parse_buffer(const std::string &str)
{
	return std::stof(str);
}

void update_quaternion(ImuSerial *self, const glm::quat &quat)
{
	std::lock_guard(self->quat_mutex);
	self->quaternion = quat;
}

void update_acc_value(ImuSerial *self, const glm::vec3 &acc)
{
	std::lock_guard(self->acc_mutex);
	self->acc = acc;
}

void update_gyr_value(ImuSerial *self, const glm::vec3 &gyr)
{
	std::lock_guard(self->gyr_mutex);
	self->gyr = gyr;
}

void working_thread(ImuSerial *self)
{
	using namespace std::literals::chrono_literals;
	char state = 0;
	std::string buffer;
	char data;
	float value;
	glm::quat quat;
	glm::vec3 acc;
	glm::vec3 gyr;

	quat = self->get_quaternion();

	while (!self->stop_flag) {
		if (read(self->fd, &data, 1) == 1) {
			if (state == 0) {
				switch (data) {
				case 'W':
				case 'I':
				case 'K':
				case 'J':
				case 'x':
				case 'y':
				case 'z':
				case 'X':
				case 'Y':
				case 'Z':
					state = data;
					buffer = "";
				}
			} else {
				if (data == '\n') {
					try {					
						value = parse_buffer(buffer);
						switch (state) {
						case 'W':
							quat.w = value;
							break;
						case 'I':
							quat.x = value;
							break;
						case 'J':
							quat.y = value;
							break;
						case 'K':
							quat.z = value;
							update_quaternion(self, quat);
							break;
						case 'x':
							acc.x = value;
							break;
						case 'y':
							acc.y = value;
							break;
						case 'z':
							acc.z = value;
							update_acc_value(self, acc);
							break;
						case 'X':
							gyr.x = value;
							break;
						case 'Y':
							gyr.y = value;
							break;
						case 'Z':
							gyr.z = value;
							update_gyr_value(self, gyr);
							break;
						}
					} catch(...) {
						std::cerr << "Invalid data received: " << buffer << std::endl;
					}
					state = 0;
				} else {
					buffer = buffer.append(&data, 1);
				}
			}

		} else {
			std::this_thread::sleep_for(50us);
		}
	}
}

void ImuSerial::start()
{
	this->fd = open_serial(this->serial_device.c_str(), this->baud);
	if (this->fd >= 0) {
		stop_flag = 0;
		worker = std::thread(working_thread, this);
	}
}

void ImuSerial::stop()
{
	stop_flag = 1;
	if (worker.joinable())
		worker.join();

	if (this->fd >= 0) {
		close(this->fd);
		this->fd = -1;
	}
}

void ImuSerial::set_device(const std::string &device)
{
	this->serial_device = device;
}

void ImuSerial::set_baudrate(const unsigned int baud)
{
	this->baud = baud;
}

unsigned int ImuSerial::get_baudrate()
{
	return this->baud;
}

const std::string &ImuSerial::get_device()
{
	return this->serial_device;
}

bool ImuSerial::runs()
{
	if (this->worker.joinable() && this->stop_flag == 0)
		return true;
	else
		return false;
}

glm::quat ImuSerial::get_quaternion()
{
	std::lock_guard(this->quat_mutex);
	glm::quat ret_val = this->quaternion;

	return ret_val;
}

glm::vec3 ImuSerial::get_accel()
{
	std::lock_guard(this->acc_mutex);
	return this->acc;
}

glm::vec3 ImuSerial::get_gyro()
{
	std::lock_guard(this->gyr_mutex);
	return this->gyr;
}

const glm::mat4 ImuSerial::get_rot_matrix()
{
	return glm::mat4_cast(this->get_quaternion());
}
