#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);
    }
}

void grbl::tcp_transport::send(std::string line) {
    if (is_connected) {
        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 > 0) {
                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)) {
                            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(100));
        }
    }
}

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

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