//
// Created by pengx on 2025/3/18.
//

#include "robotic_arm_serial_port.hpp"
#include <iomanip>
#include <iostream>

RoboticArmSerialPort::RoboticArmSerialPort(
    boost::asio::io_service &io_service, const std::string &port_name, const int baud_rate
): io_service_(io_service), port_(io_service, port_name) {
    port_.set_option(serial_port_base::baud_rate(baud_rate));
    port_.set_option(serial_port_base::character_size(8));
    port_.set_option(serial_port_base::parity(serial_port_base::parity::none));
    port_.set_option(serial_port_base::stop_bits(serial_port_base::stop_bits::one));
    std::cout << "\033[1;92m"
            << "RoboticArmSerialPort init success"
            << "\033[0m" << std::endl;
}

std::vector<uint8_t> RoboticArmSerialPort::read_from_port(const std::vector<uint8_t> &command) {
    write(command);

    boost::asio::streambuf buffer;
    boost::system::error_code ec;
    // 同步读取直到遇到 0xFA 或超时
    const size_t bytes_transferred = boost::asio::read_until(port_, buffer, 0xFA, ec);

    if (ec) {
        std::cerr << "Read error: " << ec.message() << std::endl;
        return {};
    }

    // 提取数据
    std::vector<uint8_t> frame(bytes_transferred);
    std::istream is(&buffer);
    is.read(reinterpret_cast<char *>(frame.data()), static_cast<std::streamsize>(bytes_transferred));
    return frame;
}

void RoboticArmSerialPort::write(std::vector<uint8_t> command) {
    for (const uint8_t i: command) {
        std::cout << std::hex
                << std::uppercase
                << std::setw(2)
                << std::setfill('0')
                << static_cast<int>(static_cast<uint8_t>(i))
                << " ";
    }
    std::cout << std::endl;
    boost::asio::write(port_, boost::asio::buffer(command));
}