#include "UartComIF.h"

#include <fsfw/serviceinterface/ServiceInterface.h>
#include <string.h>

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

UartComIF::UartComIF(object_id_t objectId): SystemObject(objectId){
}

UartComIF::~UartComIF() {}

ReturnValue_t UartComIF::initializeInterface(CookieIF * cookie) {

	std::string deviceFile;
	UartDeviceMapIter uartDeviceMapIter;

	if(cookie == nullptr) {
		return NULLPOINTER;
	}

    UartCookie* uartCookie = dynamic_cast<UartCookie*>(cookie);
    if (uartCookie == nullptr) {
        sif::error << "UartComIF::initializeInterface: Invalid UART Cookie!" << std::endl;
        return NULLPOINTER;
    }

    deviceFile = uartCookie->getDeviceFile();

    uartDeviceMapIter = uartDeviceMap.find(deviceFile);
	if(uartDeviceMapIter == uartDeviceMap.end()) {
	    int fileDescriptor = configureUartPort(uartCookie);
	    if (fileDescriptor < 0) {
	        return RETURN_FAILED;
	    }
	    size_t maxReplyLen = uartCookie->getMaxReplyLen();
		UartElements_t uartElements = {fileDescriptor, std::vector<uint8_t>(maxReplyLen), 0};
		std::pair status = uartDeviceMap.emplace(deviceFile, uartElements);
        if (status.second == false) {
            sif::debug << "UartComIF::initializeInterface: Failed to insert device "  << deviceFile
                    << "to Uart device map" << std::endl;
            return RETURN_FAILED;
        }
	}
	else {
		sif::debug << "UartComIF::initializeInterface: Uart device " << deviceFile << "already in "
		        << "use" << std::endl;
		return RETURN_FAILED;
	}

	return RETURN_OK;
}

int UartComIF::configureUartPort(UartCookie* uartCookie) {

    struct termios options;

    std::string deviceFile = uartCookie->getDeviceFile();
    int fd = open(deviceFile.c_str(), O_RDWR);

    if (fd < 0) {
        sif::debug << "UartComIF::configureUartPort: Failed to open uart " << deviceFile << "with"
                << " error code " << errno << strerror(errno) << std::endl;
        return fd;
    }

    /* Read in existing settings */
    if(tcgetattr(fd, &options) != 0) {
        sif::debug << "UartComIF::configureUartPort: Error " << errno << "from tcgetattr: "
                << strerror(errno) << std::endl;
        return fd;
    }

    setParityOptions(&options, uartCookie);
    setStopBitOptions(&options, uartCookie);
    setDatasizeOptions(&options, uartCookie);
    setFixedOptions(&options);

    /* Sets uart to non-blocking mode. Read returns immediately when there are no data available */
    options.c_cc[VTIME] = 0;
    options.c_cc[VMIN] = 0;

    configureBaudrate(&options, uartCookie);

    /* Save option settings */
    if (tcsetattr(fd, TCSANOW, &options) != 0) {
        sif::debug << "UartComIF::configureUartPort: Failed to set options with error " << errno
                << ": " << strerror(errno);
        return fd;
    }
    return fd;
}

void UartComIF::setParityOptions(struct termios* options, UartCookie* uartCookie) {
    /* Clear parity bit */
    options->c_cflag &= ~PARENB;
    switch (uartCookie->getParity()) {
    case Parity::EVEN:
        options->c_cflag |= PARENB;
        options->c_cflag &= ~PARODD;
        break;
    case Parity::ODD:
        options->c_cflag |= PARENB;
        options->c_cflag |= PARODD;
        break;
    default:
        break;
    }
}

void UartComIF::setStopBitOptions(struct termios* options, UartCookie* uartCookie) {
    /* Clear stop field. Sets stop bit to one bit */
    options->c_cflag &= ~CSTOPB;
    switch (uartCookie->getStopBits()) {
    case StopBits::TWO_STOP_BITS:
        options->c_cflag |= CSTOPB;
        break;
    default:
        break;
    }
}

void UartComIF::setDatasizeOptions(struct termios* options, UartCookie* uartCookie) {
    /* Clear size bits */
    options->c_cflag &= ~CSIZE;
    switch (uartCookie->getBitsPerWord()) {
    case 5:
        options->c_cflag |= CS5;
        break;
    case 6:
        options->c_cflag |= CS6;
        break;
    case 7:
        options->c_cflag |= CS7;
        break;
    case 8:
        options->c_cflag |= CS8;
        break;
    default:
        sif::debug << "UartComIF::setDatasizeOptions: Invalid size specified" << std::endl;
        break;
    }
}

void UartComIF::setFixedOptions(struct termios* options) {
    /* Disable RTS/CTS hardware flow control */
    options->c_cflag &= ~CRTSCTS;
    /* Turn on READ & ignore ctrl lines (CLOCAL = 1) */
    options->c_cflag |= CREAD | CLOCAL;
    /* Disable canonical mode */
    options->c_lflag &= ~ICANON;
    /* Disable echo */
    options->c_lflag &= ~ECHO;
    /* Disable erasure */
    options->c_lflag &= ~ECHOE;
    /* Disable new-line echo */
    options->c_lflag &= ~ECHONL;
    /* Disable interpretation of INTR, QUIT and SUSP */
    options->c_lflag &= ~ISIG;
    /* Turn off s/w flow ctrl */
    options->c_iflag &= ~(IXON | IXOFF | IXANY);
    /* Disable any special handling of received bytes */
    options->c_iflag &= ~(IGNBRK|BRKINT|PARMRK|ISTRIP|INLCR|IGNCR|ICRNL);
    /* Prevent special interpretation of output bytes (e.g. newline chars) */
    options->c_oflag &= ~OPOST;
    /* Prevent conversion of newline to carriage return/line feed */
    options->c_oflag &= ~ONLCR;
}

void UartComIF::configureBaudrate(struct termios* options, UartCookie* uartCookie) {
    switch (uartCookie->getBaudrate()) {
    case 50:
        cfsetispeed(options, B50);
        cfsetospeed(options, B50);
        break;
    case 75:
        cfsetispeed(options, B75);
        cfsetospeed(options, B75);
        break;
    case 110:
        cfsetispeed(options, B110);
        cfsetospeed(options, B110);
        break;
    case 134:
        cfsetispeed(options, B134);
        cfsetospeed(options, B134);
        break;
    case 150:
        cfsetispeed(options, B150);
        cfsetospeed(options, B150);
        break;
    case 200:
        cfsetispeed(options, B200);
        cfsetospeed(options, B200);
        break;
    case 300:
        cfsetispeed(options, B300);
        cfsetospeed(options, B300);
        break;
    case 600:
        cfsetispeed(options, B600);
        cfsetospeed(options, B600);
        break;
    case 1200:
        cfsetispeed(options, B1200);
        cfsetospeed(options, B1200);
        break;
    case 1800:
        cfsetispeed(options, B1800);
        cfsetospeed(options, B1800);
        break;
    case 2400:
        cfsetispeed(options, B2400);
        cfsetospeed(options, B2400);
        break;
    case 4800:
        cfsetispeed(options, B4800);
        cfsetospeed(options, B4800);
        break;
    case 9600:
        cfsetispeed(options, B9600);
        cfsetospeed(options, B9600);
        break;
    case 19200:
        cfsetispeed(options, B19200);
        cfsetospeed(options, B19200);
        break;
    case 38400:
        cfsetispeed(options, B38400);
        cfsetospeed(options, B38400);
        break;
    case 57600:
        cfsetispeed(options, B57600);
        cfsetospeed(options, B57600);
        break;
    case 115200:
        cfsetispeed(options, B115200);
        cfsetospeed(options, B115200);
        break;
    case 230400:
        cfsetispeed(options, B230400);
        cfsetospeed(options, B230400);
        break;
    case 460800:
        cfsetispeed(options, B460800);
        cfsetospeed(options, B460800);
        break;
    default:
        sif::debug << "UartComIF::configureBaudrate: Baudrate not supported" << std::endl;
        break;
    }
}

ReturnValue_t UartComIF::sendMessage(CookieIF *cookie,
        const uint8_t *sendData, size_t sendLen) {

	int fd;
	std::string deviceFile;
	UartDeviceMapIter uartDeviceMapIter;

	if(sendData == nullptr) {
        sif::debug << "UartComIF::sendMessage: Send Data is nullptr" << std::endl;
	    return RETURN_FAILED;
	}

	if(sendLen == 0) {
		return RETURN_OK;
	}

	UartCookie* uartCookie = dynamic_cast<UartCookie*>(cookie);
	if(uartCookie == nullptr) {
        sif::debug << "UartComIF::sendMessasge: Invalid Uart Cookie!" << std::endl;
		return NULLPOINTER;
	}

	deviceFile = uartCookie->getDeviceFile();
	uartDeviceMapIter = uartDeviceMap.find(deviceFile);
	if (uartDeviceMapIter == uartDeviceMap.end()) {
		sif::debug << "UartComIF::sendMessage: Device file " << deviceFile << "not in uart map"
		        << std::endl;
		return RETURN_FAILED;
	}

	fd = uartDeviceMapIter->second.fileDescriptor;

	if (write(fd, sendData, sendLen) != (int)sendLen) {
		sif::error << "UartComIF::sendMessage: Failed to send data with error code " << errno
		        << ": Error description: " << strerror(errno) << std::endl;
		return RETURN_FAILED;
	}

	return RETURN_OK;
}

ReturnValue_t UartComIF::getSendSuccess(CookieIF *cookie) {
	return RETURN_OK;
}

ReturnValue_t UartComIF::requestReceiveMessage(CookieIF *cookie,
		size_t requestLen) {

    int fd;
    std::string deviceFile;
    UartDeviceMapIter uartDeviceMapIter;
    uint8_t* bufferPtr;

    if(requestLen == 0) {
        return RETURN_OK;
    }

    UartCookie* uartCookie = dynamic_cast<UartCookie*>(cookie);
    if(uartCookie == nullptr) {
        sif::debug << "UartComIF::requestReceiveMessage: Invalid Uart Cookie!" << std::endl;
        return NULLPOINTER;
    }

    deviceFile = uartCookie->getDeviceFile();
    uartDeviceMapIter = uartDeviceMap.find(deviceFile);
    if (uartDeviceMapIter == uartDeviceMap.end()) {
        sif::debug << "UartComIF::requestReceiveMessage: Device file " << deviceFile
                << " not in uart map" << std::endl;
        return RETURN_FAILED;
    }

    fd = uartDeviceMapIter->second.fileDescriptor;
    bufferPtr = uartDeviceMapIter->second.replyBuffer.data();
    int bytesRead = read(fd, bufferPtr, requestLen);
    if (bytesRead != static_cast<int>(requestLen)) {
        sif::debug << "UartComIF::requestReceiveMessage: Only read " << bytesRead
        << " of " << requestLen << " bytes" << std::endl;
        return RETURN_FAILED;
    }
    else {
        uartDeviceMapIter->second.replyLen = bytesRead;
    }

    return RETURN_OK;
}

ReturnValue_t UartComIF::readReceivedMessage(CookieIF *cookie,
		uint8_t **buffer, size_t* size) {

    std::string deviceFile;
    UartDeviceMapIter uartDeviceMapIter;

    UartCookie* uartCookie = dynamic_cast<UartCookie*>(cookie);
	if(uartCookie == nullptr) {
        sif::debug << "UartComIF::readReceivedMessage: Invalid uart cookie!" << std::endl;
		return NULLPOINTER;
	}

	deviceFile = uartCookie->getDeviceFile();
    uartDeviceMapIter = uartDeviceMap.find(deviceFile);
    if (uartDeviceMapIter == uartDeviceMap.end()) {
        sif::debug << "UartComIF::readReceivedMessage: Device file " << deviceFile
                << " not in uart map" << std::endl;
        return RETURN_FAILED;
    }

	*buffer = uartDeviceMapIter->second.replyBuffer.data();
	*size = uartDeviceMapIter->second.replyLen;

	/* Length is reset to 0 to prevent reading the same data twice */
	uartDeviceMapIter->second.replyLen = 0;

	return RETURN_OK;
}