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

#include <cmath>

CallbackReturn mg::MgStepperInterface::on_init(const hardware_interface::HardwareInfo& info) {
    if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) {
        return CallbackReturn::ERROR;
    }

    /*
    if (info_.hardware_parameters.find("device_path") != info.hardware_parameters.end()) {
        serial_port_name = info_.hardware_parameters["device_path"];
    } else {
        serial_port_name = "/dev/ttyACM1";
    }
    */

    left_wheel_pos_state  = 0;
    right_wheel_pos_state = 0;
    left_wheel_vel_cmd    = 0;
    right_wheel_vel_cmd   = 0;
    return CallbackReturn::SUCCESS;
}

CallbackReturn mg::MgStepperInterface::on_configure(const rclcpp_lifecycle::State&) {
    /*
    try {
        if (!odrive_serial_interface.IsOpen()) {
            odrive_serial_interface.Open(serial_port_name);
        }
    } catch (const LibSerial::OpenFailed& er) {
        RCLCPP_FATAL(rclcpp::get_logger("MgStepperInterface"),
                     "Failed to open serial port (Is the stepper driver plugged in)");
        return CallbackReturn::ERROR;
    }
    */
    return CallbackReturn::SUCCESS;
}

CallbackReturn mg::MgStepperInterface::on_shutdown(const rclcpp_lifecycle::State&) {
    /*
    if (odrive_serial_interface.IsOpen()) {
        odrive_serial_interface.Close();
    }
    */
    return CallbackReturn::SUCCESS;
}

std::vector<hardware_interface::StateInterface> mg::MgStepperInterface::export_state_interfaces() {

    std::vector<hardware_interface::StateInterface> state_interfaces;
    state_interfaces.reserve(2);
    state_interfaces.emplace_back("left_motor", hardware_interface::HW_IF_POSITION, &left_wheel_pos_state);
    state_interfaces.emplace_back("right_motor", hardware_interface::HW_IF_POSITION, &right_wheel_pos_state);

    return state_interfaces;
}

std::vector<hardware_interface::CommandInterface> mg::MgStepperInterface::export_command_interfaces() {

    std::vector<hardware_interface::CommandInterface> command_interfaces;
    command_interfaces.reserve(2);
    command_interfaces.emplace_back("left_motor", hardware_interface::HW_IF_VELOCITY, &left_wheel_vel_cmd);
    command_interfaces.emplace_back("right_motor", hardware_interface::HW_IF_VELOCITY, &right_wheel_vel_cmd);

    return command_interfaces;
}

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

hardware_interface::return_type mg::MgStepperInterface::write(const rclcpp::Time&, const rclcpp::Duration&) {
    /*
    union {
        std::array<u_char, sizeof(double)> bytes;
        double                             data;
    } value{};

    try {
        odrive_serial_interface.Write("s;");
        value.data = -left_wheel_vel_cmd / (2 * M_PI);
        for (const auto& bt : value.bytes) { odrive_serial_interface.WriteByte(bt); }
        value.data = right_wheel_vel_cmd / (2 * M_PI);
        for (const auto& bt : value.bytes) { odrive_serial_interface.WriteByte(bt); }

    } catch (const std::runtime_error& e) { return hardware_interface::return_type::ERROR; }
    */
    return hardware_interface::return_type::OK;
}