#pragma once

#include "uartconnect.hpp"
#include "comm.hpp"

#include <errno.h>
#include <termios.h>
#include <unistd.h>
#include <stdlib.h>
#include <stdio.h>
#include <fcntl.h>
#include <string.h>
#include <errno.h>
#include <thread>
#include <time.h>
#include <sys/socket.h>
#include <bluetooth/bluetooth.h>
#include <bluetooth/rfcomm.h>

class PcUart
{
protected:
	PcUart(const char* name)
	{
        if (!strncmp(name, "/dev/tty", 8) || !strncmp(name, "/dev/rfcomm", 11))
		{
			fd = open (name, O_RDWR | O_NOCTTY | O_SYNC);
			if (fd < 0)
			{
					printf ("error %d opening %s: %s\n", errno, name, strerror (errno));
					return;
			}
			struct termios tty;
			memset (&tty, 0, sizeof tty);
			if (tcgetattr (fd, &tty) != 0)
			{
					printf ("error %d from tcgetattr\n", errno);
					return;
			}

			//cfsetospeed (&tty, 115220);
			//cfsetispeed (&tty, 115220);
			cfsetospeed (&tty, 9600);
			cfsetispeed (&tty, 9600);

			tty.c_cflag = (tty.c_cflag & ~CSIZE) | CS8;     // 8-bit chars
			// disable IGNBRK for mismatched speed tests; otherwise receive break
			// as \000 chars
			tty.c_iflag &= ~IGNBRK;         // disable break processing
			tty.c_lflag = 0;                // no signaling chars, no echo,
											// no canonical processing
			tty.c_oflag = 0;                // no remapping, no delays
			tty.c_cc[VMIN]  = 0;            // read doesn't block
			tty.c_cc[VTIME] = 5;            // 0.5 seconds read timeout

			tty.c_iflag &= ~(IXON | IXOFF | IXANY); // shut off xon/xoff ctrl

			tty.c_cflag |= (CLOCAL | CREAD);// ignore modem controls,
											// enable reading
			tty.c_cflag &= ~(PARENB | PARODD);      // shut off parity
			tty.c_cflag |= 0;
			tty.c_cflag &= ~CSTOPB;
			tty.c_cflag &= ~CRTSCTS;

			if (tcsetattr (fd, TCSANOW, &tty) != 0)
			{
					printf ("error %d from tcsetattr\n", errno);
					return;
			}
			memset (&tty, 0, sizeof tty);
			if (tcgetattr (fd, &tty) != 0)
			{
					printf ("error %d from tggetattr\n", errno);
					return;
			}

			tty.c_cc[VMIN]  = 1; // blocking
			tty.c_cc[VTIME] = 5;            // 0.5 seconds read timeout

			if (tcsetattr (fd, TCSANOW, &tty) != 0)
					printf ("error %d setting term attributes\n", errno);
		}
		else
		{ // try bluetooth open
			struct sockaddr_rc addr = { 0 };
			addr.rc_family = AF_BLUETOOTH;
			addr.rc_channel = (uint8_t) 1;
			str2ba(name, &addr.rc_bdaddr);
			fd = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);
			connect(fd, (struct sockaddr *)&addr, sizeof(addr));
		}
	}
	bool uartwrite(const void* buf, size_t count)
	{
		return write (fd, buf, count) == (ssize_t)count; 
	}
	void startReader()
	{
		std::thread th(&PcUart::reader, this);
		th.detach();
	}
	void reader()
	{
		uint8_t c;
		while (1)
			if (read(fd, (void*)&c, 1) == 1)
				uartOnChar(c);
	}
	virtual void uartOnChar(uint8_t c) = 0;
	~PcUart()
	{
		if (fd)
			close(fd);
	}
	int fd;
};

class CommMaster : public Comm, public PcUart, public UartConnect<37>
{
public:
	CommMaster(const char* portname, unsigned msTimeout, unsigned retries) :
		PcUart(portname), sequence(0), sequenceAck(0), sndBuffer(msgBuffer+2), msTimeout(msTimeout), retries(retries)
	{
		startReader();
	}
	bool ack()
	{
		return sequence == sequenceAck;
	}

	virtual void addPoints(const uint8_t* xys, unsigned points)
	{
		if (!points)
			return;
		points = points<=16 ? points : 16;
		for (unsigned i=0; i<2*points; i++) {
			sndBuffer[i] = xys[i];
		}
		sendCommand(Command::kAddPoints, points);
	}
	virtual void deletePoints() { sendCommand(Command::kDeletePoints); }
	virtual void stopMove() { sendCommand(Command::kStopMove); }
	virtual void startMove() { sendCommand(Command::kStartMove); }
	virtual void setParam() { sendCommand(Command::kSetParam); }
	virtual void gotoXY(PosType x, PosType y)
	{
		writePosType(sndBuffer, x, 0);
		writePosType(sndBuffer, y, 1);
		sendCommand(Command::kGotoXY);
	}
	virtual void movedXdY(PosType dx, PosType dy)
	{
		writePosType(sndBuffer, dx, 0);
		writePosType(sndBuffer, dy, 1);
		sendCommand(Command::kMovedXdY);
	}
	virtual void gotoNextPoint() { sendCommand(Command::kGotoNextPoint); }
	virtual void setNPoint(uint16_t nPoint)
	{
		sndBuffer[0] = nPoint>>8;
		sndBuffer[1] = nPoint&0xff;
		sendCommand(Command::kSetNPoint);
	}
	virtual void setPos(PosType x, PosType y)
	{
		writePosType(sndBuffer, x, 0);
		writePosType(sndBuffer, y, 1);
		sendCommand(Command::kSetPos);
	}
private:
	void sendCommand(Command cmd, uint8_t len=1)
	{
		msgBuffer[0] = ++sequence;
		len = len ? (len<=16 ? len-1 : 15) : 0;
		msgBuffer[1] = (len<<4) | (uint8_t)cmd;
		sendMsg(msgBuffer);
		//printf("Command %u...", sequence);
		//fflush(stdout);
		struct timespec dt;
		unsigned ms, tries;
		for (tries=0; (tries<retries) && (sequence != sequenceAck); tries++)
		{
			for (ms=0; (ms<msTimeout) && (sequence != sequenceAck); ms++)
			{
				dt.tv_sec = 0;
				dt.tv_nsec = 1000000;
				while(nanosleep(&dt,&dt)<0 && errno==EINTR);
			}
		}
		/*if (sequence == sequenceAck)
			printf("\t\t\tack in %u ms\n", msTimeout*(tries-1)+ms-1);
		else
			printf("\t\t\ttimeout\n");*/
	}
	virtual void onMsg(uint8_t* buffer)
	{
		SlaveInfo* pInfo = (SlaveInfo*)buffer;
		sequenceAck = pInfo->sequence;
		memcpy(&info, pInfo, sizeof(info));
		printf("Seq %u, status %u, spaceSize %u, freeSpace %u, pos (%d,%d), nPoint %u\n",
			info.sequence, info.status, info.spaceSize, info.freeSpace, info.posX, info.posY, info.nPoint);
	}
	virtual void send(const uint8_t* buffer, unsigned len)
	{
		uartwrite(buffer, len);
	}
	virtual void uartOnChar(uint8_t c)
	{
		onChar(c);
	}
public:
	SlaveInfo info;
private:
	uint8_t sequence;
	uint8_t sequenceAck;
	uint8_t msgBuffer[kPacketSize-3];
	uint8_t* sndBuffer;
	unsigned msTimeout;
	unsigned retries;
};
