#include "toid_control/toid_control.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"

#include <cmath>

CallbackReturn toid::StepperInterface::on_init(const hardware_interface::HardwareComponentInterfaceParams& info) {
    if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) {
        return CallbackReturn::ERROR;
    }

    if (info_.hardware_parameters.find("device_path") != info.hardware_info.hardware_parameters.end()) {
        serial_port_name_ = info_.hardware_parameters["device_path"];
    } else {
        serial_port_name_ = "/dev/ttyACM0";
    }

    left_wheel_pos_state_  = 0;
    right_wheel_pos_state_ = 0;
    left_wheel_vel_cmd_    = 0;
    right_wheel_vel_cmd_   = 0;
    return CallbackReturn::SUCCESS;
}

CallbackReturn toid::StepperInterface::on_configure(const rclcpp_lifecycle::State&) {
    try {
        serial_port_.open(serial_port_name_);
    } catch (const std::exception& er) {
        RCLCPP_ERROR(rclcpp::get_logger("StepperInterface"),
                     "Failed to open serial port (Is the stepper driver plugged in)");
    }
    return CallbackReturn::SUCCESS;
}

CallbackReturn toid::StepperInterface::on_shutdown(const rclcpp_lifecycle::State&) {
    if (serial_port_.is_open()) {
        boost::system::error_code ec;
        ec = serial_port_.close(ec);
    }
    return CallbackReturn::SUCCESS;
}

std::vector<hardware_interface::StateInterface> toid::StepperInterface::export_state_interfaces() {
    std::vector<hardware_interface::StateInterface> state_interfaces;
    state_interfaces.reserve(2);
    state_interfaces.emplace_back("drivewhl_l_joint", hardware_interface::HW_IF_POSITION, &left_wheel_pos_state_);
    state_interfaces.emplace_back("drivewhl_r_joint", hardware_interface::HW_IF_POSITION, &right_wheel_pos_state_);

    return state_interfaces;
}

std::vector<hardware_interface::CommandInterface> toid::StepperInterface::export_command_interfaces() {
    std::vector<hardware_interface::CommandInterface> command_interfaces;
    command_interfaces.reserve(2);
    command_interfaces.emplace_back("drivewhl_l_joint", hardware_interface::HW_IF_VELOCITY, &left_wheel_vel_cmd_);
    command_interfaces.emplace_back("drivewhl_r_joint", hardware_interface::HW_IF_VELOCITY, &right_wheel_vel_cmd_);

    return command_interfaces;
}

hardware_interface::return_type toid::StepperInterface::read(const rclcpp::Time&, const rclcpp::Duration&) {
    return hardware_interface::return_type::OK;
}

hardware_interface::return_type toid::StepperInterface::write(const rclcpp::Time&, const rclcpp::Duration&) {
    using namespace asio::buffer_literals;

    try {
        double values[] = {
            -left_wheel_vel_cmd_ / (2 * M_PI),
            right_wheel_vel_cmd_ / (2 * M_PI),
        };
        asio::write(serial_port_, "s;"_buf);
        asio::write(serial_port_, asio::buffer(values, sizeof(values)));
    } catch (const std::runtime_error& e) { 
        RCLCPP_ERROR_THROTTLE(
            this->get_logger(), 
            *this->get_clock(), 
            5000, 
            "Failed to write to serial port: %s",
            e.what()
        );
        boost::system::error_code ec;
        ec = serial_port_.close(ec);
        ec = serial_port_.open(serial_port_name_, ec);
    }
    return hardware_interface::return_type::OK;
}
