#include <cstdio>
#include <cstring>

#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <termios.h>
#include <dirent.h>

#include <iostream>
#include <sstream>
#include <string>
#include <list>
#include <vector>


struct BaseException {
	std::string message;
	BaseException(const std::string & msg): message(msg) {}
};

#define DEF_EXCEPTION(Exception) struct Exception: public BaseException {\
    Exception(const std::string & msg): BaseException(msg) {}};

DEF_EXCEPTION(PortOpenException)

//************************************************************************************************

class SerialPort {
  protected:
	int port;
    
  public:
	SerialPort(): port(-1) {}
	SerialPort(const std::string & portDevPath): port(-1) {Open(portDevPath);}
	~SerialPort() {if(port != -1) Close();}
	
	void Open(const std::string & portDevPath);
	void Close() {close(port); port = -1;}
	
	int Write(const uint8_t * data, size_t length) {return write(port, data, length);}
	
	int Write(const std::string & string) {return write(port, string.c_str(), string.length());}
	
	template<typename T>
	int Write(const std::vector<T> & data) {return write(port, &data[0], data.length()*sizeof(T));}
	
	int Write(const uint8_t byte) {return Write(&byte, 1);}
	
	size_t Read(uint8_t * buffer, size_t bytes) {return read(port, buffer, bytes);}
	
	uint8_t Read() {uint8_t result; while(Read(&result, 1) == 0) ; return result;}
	
	void SetBlockingReads(bool block) {
        if(block) fcntl(port, F_SETFL, 0);
        else fcntl(port, F_SETFL, FNDELAY);
    }
	
	int GetFD() const {return port;}
	
	void FCntl(int key, int value) {fcntl(port, key, value);}
};

//************************************************************************************************

class TermIOS {
  protected:
	termios options;
	const SerialPort * port;
  public:
	TermIOS(const SerialPort & p): port(&p) {tcgetattr(port->GetFD(), &options);}
	
	TermIOS & SetDefaults(int baud = B38400);
	
	TermIOS & Set8N1() {//8N1: 8 bits, no parity
		options.c_cflag &= ~PARENB; options.c_cflag &= ~CSTOPB;
		options.c_cflag &= ~CSIZE; options.c_cflag |= CS8;
		return *this;
	}
	
	// B50 B1200 B9600 B19200 B38400 B57600 B115200
	TermIOS & SetIOBaud(int baud) {return SetIOBaud(baud, baud);}
	TermIOS & SetIOBaud(int ibaud, int obaud) {
		cfsetispeed(&options, ibaud);
		cfsetospeed(&options, obaud);
		return *this;
	}
	
	
	TermIOS & DisableHWFlow() {// disable HW flow control
		options.c_cflag &= ~CRTSCTS;//~CNEW_RTSCTS;
		return *this;
	}
	
	TermIOS & DisableSWFlow() {// disable SW flow control
		options.c_iflag &= ~(IXON | IXOFF | IXANY);
		return *this;
	}
	TermIOS & DisableFlow() {DisableHWFlow(); DisableSWFlow(); return *this;}
	
	
	void Commit() {tcsetattr(port->GetFD(), TCSANOW, &options);}
};


void SerialPort::Open(const std::string & portDevPath)
{
	if(port != -1)
		Close();
	
	port = open(portDevPath.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);
	if(port == -1)
	{
		std::ostringstream message;
		message << "Could not open " << portDevPath << std::endl;
		throw PortOpenException(message.str());
	}
	fcntl(port, F_SETFL, 0);// block on reads.
	// "This is also used after opening a serial port with the O_NDELAY option."
	// Why?
	
//	TermIOS(*this).SetDefaults().Commit();
}

TermIOS & TermIOS::SetDefaults(int baud)
{
	SetIOBaud(baud);
	
	options.c_cflag |= (CLOCAL | CREAD);
	
	DisableHWFlow();
	DisableSWFlow();
	
	// Raw input
	options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
	
	// Raw output
	options.c_oflag &= ~OPOST;
	
	Set8N1();
	return *this;
}

//************************************************************************************************
// Frame format:
// uint8_t size : size bytes data : uint16_t checksum
// Command frame:
// 0x00 0xXX 0xXX 0xXX
// Ack frame:
// 0x5A
// Error frames:
// 0xF0
// 0xF6 : uint8_t size : size bytes data

inline uint16_t crc_xmodem_update(uint16_t crc, uint8_t data)
{
	uint8_t j;
	crc = crc ^ ((uint16_t)data << 8);
	for(j = 0; j < 8; ++j) {
		if(crc & 0x8000)
			crc = (crc << 1) ^ 0x1021;
		else
			crc <<= 1;
	}
	return crc;
}

inline uint16_t crc_xmodem(uint8_t * data, uint8_t size)
{
	uint16_t crc = 0;
	uint8_t j;
	for(j = 0; j < size; ++j)
		crc = crc_xmodem_update(crc, *data++);
	return crc;
}

inline bool FrameGood(uint8_t * data, uint8_t size)
{
	uint16_t computedCRC = crc_xmodem(data, size);
	uint16_t receivedCRC = ((uint16_t)*(data + size) << 8) | *(data + size + 1);
	return computedCRC == receivedCRC;
}

bool GetFrame(uint8_t * buffer, uint8_t * size, SerialPort & port)
{
	uint8_t tries;
	for(tries = 0; tries < 3; ++tries)
	{
		uint16_t bytesRead = 0;
		
		*size = port.Read();
		
		while(bytesRead < (uint16_t)*size + 2)
			bytesRead += port.Read(buffer + bytesRead, (uint16_t)*size + 2 - bytesRead);
		
		if(FrameGood(buffer, *size)) {
			port.Write(0x5A);
			return true;
		}
		
		std::cerr << "CRC mismatch in GetFrame()" << std::endl;
		// Not good. Send failure and try again.
		port.Write(0xF0);
	}
	std::cerr << "GetFrame() failed" << std::endl;
	return false;
}

bool SendFrame(uint8_t * data, uint8_t size, SerialPort & port)
{
	uint8_t tries;
	for(tries = 0; tries < 3; ++tries)
	{
		uint16_t crc = crc_xmodem(data, size);
		port.Write(size);
		port.Write(data, size);
		port.Write((uint8_t)(crc >> 8));
		port.Write((uint8_t)(crc));
		
		uint8_t response = port.Read();
		if(response == 0x5A)
			return true;
		
		std::cout << "Transmission error in SendFrame(): " << response << std::endl;
	}
	std::cout << "SendFrame() failed" << std::endl;
	return false;
}

//************************************************************************************************

bool FindFiles(const std::string & dirName, const std::string & fNameStart,
               std::list<std::string> & matches);

//************************************************************************************************

int main(int argc, char * const argv[])
{
	std::list<std::string> matches;
	if(FindFiles("/dev", "tty.usb", matches))
	{
		std::cout << "matches.size() = " << matches.size() << std::endl;
		
		SerialPort port("/dev/" + matches.front());
		TermIOS termios(port);
//		termios.Set8N1();
//		termios.DisableFlow();
		termios.SetDefaults(B1200);
//		termios.SetDefaults(B19200);
//		termios.SetDefaults(B38400);
		termios.Commit();
		port.SetBlockingReads(true);
		
		uint8_t buffer[256];
		while(1)
		{
//			size_t bytesWritten = port.Write("Hello World");
//			std::cout << "bytesWritten = " << bytesWritten << std::endl;
//			size_t bytesRead = port.Read(buffer, 16);
//			if(bytesRead != 0)
//			{
////				std::cout << "bytesRead = " << bitsRead
////						  << ", buffer = " << std::string(buffer, buffer + bytesRead) << std::endl;
//				std::cout << "bytesRead = " << bytesRead;
//				for(int j = 0; j < bytesRead; ++j)
//					std::cout << " " << (int)buffer[j];
//				std::cout << std::endl;
//			}
			std::cout << "Trying..." << std::endl;
			char * data = "Hello World!";
			uint8_t size = strlen(data) + 1;
			if(SendFrame((uint8_t*)data, size, port))
			{
				std::cout << "Sent: " << data << std::endl;
				if(GetFrame((uint8_t*)data, &size, port))
					std::cout << "Response: " << data << std::endl;
			}
			usleep(100*1000);
		}
	}
	return 0;
}


bool FindFiles(const std::string & dirName, const std::string & fNameStart,
               std::list<std::string> & matches)
{
	DIR * dir = opendir(dirName.c_str());
	if(!dir) return false;
	dirent * ent = readdir(dir);
	while(ent != NULL)
	{
		if(ent->d_namlen > fNameStart.length())
		{
			// terribly inefficient, but it's not like it'll be called often...
			std::string name(ent->d_name, fNameStart.length());
			if(name == fNameStart) {
				std::cout << "FindFiles(): found file: " << ent->d_name << std::endl;
				matches.push_back(ent->d_name);
			}
		}
		ent = readdir(dir);
	}
	closedir(dir);
	return !matches.empty();
}