//
// Created by robin on 5/6/16.
//


#include <stdio.h>    // Standard input/output definitions
#include <unistd.h>   // UNIX standard function definitions
#include <fcntl.h>    // File control definitions
#include <errno.h>    // Error number definitions
#include <termios.h>  // POSIX terminal control definitions
#include <string.h>   // String function definitions
#include <sys/ioctl.h>

#include "ArduinoSerial.h"

ArduinoSerial::ArduinoSerial(std::string port, int baudrate)
    : port_(port)
    , bauderate_(baudrate)
    , fd_(0)
{
}

ArduinoSerial::~ArduinoSerial()
{
    close();
}

BResult ArduinoSerial::open()
{
    struct termios toptions;

    //fd = open(serialport, O_RDWR | O_NOCTTY | O_NDELAY);
    fd_ = ::open(port_.c_str(), O_RDWR | O_NONBLOCK);

    if (fd_ == -1)  {
        return BResult().error(strerror(errno));
    }

    //int iflags = TIOCM_DTR;
    //ioctl(fd, TIOCMBIS, &iflags);     // turn on DTR
    //ioctl(fd, TIOCMBIC, &iflags);    // turn off DTR

    if (tcgetattr(fd_, &toptions) < 0) {
        return BResult().error(strerror(errno));
    }
    speed_t brate;
    switch(bauderate_) {
        case 4800:
            brate=B4800;
            break;
        case 9600:
            brate=B9600;
            break;
#ifdef B14400
            case 14400:
            brate=B14400;
            break;
#endif
        case 19200:
            brate=B19200;
            break;
#ifdef B28800
            case 28800:
            brate=B28800;
            break;
#endif
        case 38400:
            brate=B38400;
            break;
        case 57600:
            brate=B57600;
            break;
        case 115200:
            brate=B115200;
            break;
        default:
            brate = (speed_t)bauderate_;
    }
    cfsetispeed(&toptions, brate);
    cfsetospeed(&toptions, brate);

    // 8N1
    toptions.c_cflag &= ~PARENB;
    toptions.c_cflag &= ~CSTOPB;
    toptions.c_cflag &= ~CSIZE;
    toptions.c_cflag |= CS8;
    // no flow control
    toptions.c_cflag &= ~CRTSCTS;

    //toptions.c_cflag &= ~HUPCL; // disable hang-up-on-close to avoid reset

    toptions.c_cflag |= CREAD | CLOCAL;  // turn on READ & ignore ctrl lines
    toptions.c_iflag &= ~(IXON | IXOFF | IXANY); // turn off s/w flow ctrl

    toptions.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); // make raw
    toptions.c_oflag &= ~OPOST; // make raw

    // see: http://unixwiz.net/techtips/termios-vmin-vtime.html
    toptions.c_cc[VMIN]  = 0;
    toptions.c_cc[VTIME] = 0;
    //toptions.c_cc[VTIME] = 20;

    tcsetattr(fd_, TCSANOW, &toptions);
    if (tcsetattr(fd_, TCSAFLUSH, &toptions) < 0) {
        return BResult().error(strerror(errno));
    }

    return BResult().ok(true);
}

void ArduinoSerial::close()
{
    if (fd_ != 0) {
        ::close(fd_);
        fd_ = 0;
    }
}

Result<std::pair<SERIAL_PACKET_TYPE, std::string>> ArduinoSerial::read()
{
    SERIAL_PACKET_TYPE packetType = 0;
    auto res = readBytes((char*)&packetType, sizeof(packetType));
    if (!res) {
        return Result<std::pair<SERIAL_PACKET_TYPE, std::string>>().error(res.getError());
    }


    SERIAL_PACKET_LENGTH_TYPE length = 0;
    res = readBytes((char*)&length, sizeof(length));
    if (!res) {
        return Result<std::pair<SERIAL_PACKET_TYPE, std::string>>().error(res.getError());
    }

    if (length > 0) {
        char buff[length];

        res = readBytes(buff, length);
        if (!res) {
            return Result<std::pair<SERIAL_PACKET_TYPE, std::string>>().error(res.getError());
        }

        std::pair<SERIAL_PACKET_TYPE, std::string> pair(packetType, std::string(buff,  length));

        return Result<std::pair<SERIAL_PACKET_TYPE, std::string>>().ok(pair);
    }

    std::pair<SERIAL_PACKET_TYPE, std::string> pair(packetType, "");

    return Result<std::pair<SERIAL_PACKET_TYPE, std::string>>().ok(pair);
}

BResult ArduinoSerial::readBytes(char* buffer, size_t length)
{
    ssize_t res = 0;
    bool done = false;
    size_t pos = 0;
    while (!done)
    {
        res = ::read(fd_, buffer + pos, (size_t) length);
        if (res < 0)
        {
            return BResult().error(strerror(errno));
        }
        if (res > 0) {
            pos += res;
            if (pos >= length) {
                done = true;
            }
        }
        else {
            usleep(1);
        }
    }
    return BResult().ok(true);
}

BResult ArduinoSerial::write(const char *data, SERIAL_PACKET_LENGTH_TYPE length)
{
    auto res = writeBytes((const char*)&length, sizeof(length));
    if (!res) {
        return res;
    }
    return writeBytes(data, length);
}

BResult ArduinoSerial::writeBytes(const char *data, size_t length)
{
    auto res = ::write(fd_, data, length);
    if (res < 0) {
        return BResult().error(strerror(errno));
    }
    return BResult().ok(true);
}