//
// 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));

    read_from_port();
}

void RoboticArmSerialPort::read_from_port() {
    async_read_until(
        port_, buffer_, 0xFA,
        [this](const boost::system::error_code &error, const size_t bytes_transferred) mutable {
            if (!error) {
                // 获取接收到的数据
                std::istream is(&buffer_);
                std::vector<uint8_t> received_data(static_cast<std::streamsize>(bytes_transferred));
                is.read(
                    reinterpret_cast<char *>(received_data.data()), static_cast<std::streamsize>(bytes_transferred)
                );

                std::cout << "received " << bytes_transferred << " bytes: ";
                for (size_t i = 0; i < bytes_transferred; ++i) {
                    std::cout << std::hex
                            << std::uppercase
                            << std::setw(2)
                            << std::setfill('0')
                            << static_cast<int>(received_data[i])
                            << " ";
                }
                std::cout << std::endl;
            } else {
                std::cerr << "error: " << error.message() << std::endl;
            }
        });
}

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