#include "grbl_communication.h"

#include <sys/socket.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <iostream>
#include <fcntl.h>
#include <chrono>

grbl::tcp_transport::tcp_transport(std::string address, uint16_t p)
        : ip{std::move(address)}, port{p} {
    live_check_thread = std::thread(&grbl::tcp_transport::worker, this);
}

void grbl::tcp_transport::open(grbl::transport_callbacks &cb) {
    if (is_connected) {
        close();
    }

    listener = &cb;
    fd = socket(AF_INET, SOCK_STREAM, 0);
    if (fd == -1) {
        std::cerr << "Error creating socket" << std::endl;
    }

    struct sockaddr_in serv_addr{};
    serv_addr.sin_family = AF_INET;
    serv_addr.sin_port = htons(port);

    if (inet_pton(AF_INET, ip.c_str(), &serv_addr.sin_addr) <= 0) {
        std::cerr << "Invalid address/ Address not supported" << std::endl;
        return;
    }

    auto status = connect(fd, (struct sockaddr *) &serv_addr, sizeof(serv_addr));
    if (status < 0) {
        std::cerr << "Connection failed";
        return;
    }

    is_connected = true;
    listener->on_connected(this);

    // set non-blocking please, after we connected
    fcntl(fd, F_SETFL, O_NONBLOCK);
}

void grbl::tcp_transport::close() {
    if (fd >= 0) {
        ::close(fd);

        is_connected = false;
        send_queue = std::queue<std::string>{};

        if (listener != nullptr) {
            listener->on_disconnected(this);
            listener = nullptr;
        }

        fd = -1;
    }
}

void grbl::tcp_transport::send(std::string line) {
    if (is_connected) {
        if (line.at(0) == (char) 0xff)
            send_queue.push(line);
        else
            send_queue.push(line + "\r");
    }
}

void grbl::tcp_transport::worker() {
    std::string received;
    uint8_t buffer[200];

    while (!should_quit) {
        if (is_connected) {
            // anything to write?
            if (!send_queue.empty()) {
                auto line = send_queue.front();
                auto result = write(fd, line.c_str(), line.size());
                if (result == -1) {
                    std::cerr << "Failed while writing." << std::endl;
                    close();
                }

                send_queue.pop();
            }

            // anything to read?
            auto read_bytes = read(fd, buffer, 200);
            if (read_bytes == -1 && (errno == EWOULDBLOCK || errno == EAGAIN)) {
                // do nothing
            } else if (read_bytes == -1) {
                std::cerr << "Failed while reading." << std::endl;
                close();
            } else {
                for (int i = 0; i < read_bytes; i++) {
                    auto is_eol = buffer[i] == '\r' || buffer[i] == '\n';
                    if (is_eol) {
                        if (!is_empty_line(received)) {
                            // starts with? https://stackoverflow.com/questions/1878001/how-do-i-check-if-a-c-stdstring-starts-with-a-certain-string-and-convert-a
                            if (received.rfind("Grbl", 0) == 0) {
                                listener->on_banner(received, this);

                            } else {
                                listener->on_line_received(received, this);
                            }
                        }
                        received.clear();
                    } else {
                        received.push_back(static_cast<char>(buffer[i]));
                    }
                }
            }

            // give some time to others
            std::this_thread::sleep_for(std::chrono::milliseconds(1));
        }
    }
}

grbl::tcp_transport::~tcp_transport() {
    close();
    should_quit = true;
    live_check_thread.join();
}

bool grbl::tcp_transport::is_empty_line(const std::string &line) {
    for (auto &c: line) {
        if (c != '\r' && c != '\n' && c != ' ') {
            return false;
        }
    }
    return true;
}

void grbl::tcp_transport::send_single_char_command(uint8_t data) const {
    if (is_connected)
        write(fd, &data, 1);
}

void grbl::tcp_transport::request_realtime_report() {
    send_single_char_command(0x80);
}

void grbl::tcp_transport::request_cycle_start() {
    send_single_char_command(0x81);
}

void grbl::tcp_transport::request_feed_hold() {
    send_single_char_command(0x82);
}

void grbl::tcp_transport::parser_state_report() {
    send_single_char_command(0x83);
}