#include "TTLCommunicator.h"
#include <iostream>

#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <termios.h>
#include <unistd.h>
#include <errno.h>
#include <string.h>

TTLCommunicator::TTLCommunicator(const std::string& port, int baudrate)
    : port_name(port), baudrate(baudrate), connected(false) {
    serial_fd = -1;
}

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

bool TTLCommunicator::connect() {
    std::cout << "Attempting to connect to TTL device on port: " << port_name << " at baud rate: " << baudrate << std::endl;

    connected = open_serial_port();

    if (connected) {
        std::cout << "TTL connection established." << std::endl;
    } else {
        std::cout << "Warning: Failed to establish TTL connection." << std::endl;
    }

    return connected;
}

void TTLCommunicator::close() {
    if (connected) {
        close_serial_port();
        connected = false;
        std::cout << "TTL connection closed." << std::endl;
    }
}

bool TTLCommunicator::send_data(const std::string& data) {
    if (!connected) {
        std::cerr << "Error: Cannot send data, TTL not connected." << std::endl;
        return false;
    }

    if (serial_fd != -1) {
        ssize_t bytes_written = write(serial_fd, data.c_str(), data.length());
        if (bytes_written == -1) {
            std::cerr << "Error writing to serial port: " << strerror(errno) << std::endl;
            return false;
        } else {
            std::cout << "Sent " << bytes_written << " bytes: " << data;
            fsync(serial_fd); // Ensure data is sent
            return true;
        }
    }

    return false;
}

bool TTLCommunicator::open_serial_port() {
    // Open the serial port
    serial_fd = open(port_name.c_str(), O_RDWR | O_NOCTTY);
    if (serial_fd == -1) {
        std::cerr << "Error opening serial port " << port_name << std::endl;
        return false;
    }

    struct termios tty;

    // Get current attributes
    if (tcgetattr(serial_fd, &tty) != 0) {
        std::cerr << "Error getting serial attributes" << std::endl;
        ::close(serial_fd);
        return false;
    }

    // Set baud rate
    speed_t baud_rate;
    switch(baudrate) {
        case 9600: baud_rate = B9600; break;
        case 19200: baud_rate = B19200; break;
        case 38400: baud_rate = B38400; break;
        case 57600: baud_rate = B57600; break;
        case 115200: baud_rate = B115200; break;
        case 230400: baud_rate = B230400; break;
        case 460800: baud_rate = B460800; break;
        case 921600: baud_rate = B921600; break;
        default:
            std::cerr << "Unsupported baud rate: " << baudrate << std::endl;
            ::close(serial_fd);
            return false;
    }

    cfsetispeed(&tty, baud_rate);
    cfsetospeed(&tty, baud_rate);

    // 8N1: 8 data bits, no parity, 1 stop bit
    tty.c_cflag &= ~PARENB;  // No parity
    tty.c_cflag &= ~CSTOPB;  // 1 stop bit
    tty.c_cflag &= ~CSIZE;   // Clear data bits
    tty.c_cflag |= CS8;      // 8 data bits
    tty.c_cflag &= ~CRTSCTS; // No flow control
    tty.c_cflag |= CREAD | CLOCAL; // Enable reading, ignore control lines

    // Non-canonical mode
    tty.c_lflag &= ~ICANON;  // Non-canonical mode
    tty.c_lflag &= ~ECHO;    // No echo
    tty.c_lflag &= ~ECHOE;   // No erasure
    tty.c_lflag &= ~ISIG;    // No signal handling

    // No software flow control
    tty.c_iflag &= ~(IXON | IXOFF | IXANY);

    // No output processing
    tty.c_oflag &= ~OPOST;

    // Timeouts
    tty.c_cc[VMIN] = 0;      // Non-blocking read
    tty.c_cc[VTIME] = 5;     // 0.5 second timeout

    // Apply settings
    if (tcsetattr(serial_fd, TCSANOW, &tty) != 0) {
        std::cerr << "Error setting serial attributes" << std::endl;
        ::close(serial_fd);
        return false;
    }

    // Flush port
    tcflush(serial_fd, TCIOFLUSH);

    return true;
}

void TTLCommunicator::close_serial_port() {
    if (serial_fd != -1) {
        ::close(serial_fd);
        serial_fd = -1;
    }
    connected = false;
}