#include "hexapod_robot/hexapod_robot_hwi.hpp"

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

#include <fcntl.h>

namespace hexapod_robot {

hardware_interface::CallbackReturn 
HexapodRobotHardwareInterface::on_init(const hardware_interface::HardwareInfo &info) {
    if (hardware_interface::SystemInterface::on_init(info) != hardware_interface::CallbackReturn::SUCCESS) {
        return hardware_interface::CallbackReturn::ERROR;
    }
    
    RCLCPP_INFO(rclcpp::get_logger("HexapodRobotHardwareInterface"), "on_init");
    
    for (const hardware_interface::ComponentInfo &joint: info_.joints) {
        joint_states_[joint.name]= 0.0;
        joint_commands_[joint.name] = 0.0;

        std::string i2c_address_parameter = joint.name + "_i2c_address";
        std::string channel_parameter = joint.name + "_channel";

        PWMOutput pwm_output;
        pwm_output.i2c_address = stoi(info_.hardware_parameters[i2c_address_parameter], nullptr, 0);
        pwm_output.channel     = stoi(info_.hardware_parameters[channel_parameter], nullptr, 0);

        pca9685_controller_available_[pwm_output.i2c_address] = false;
        joint_pwm_mapping_[joint.name] = pwm_output;

        RCLCPP_INFO(rclcpp::get_logger("HexapodRobotHardwareInterface"), "%s mapped to 0x%02x:%d", joint.name.c_str(), pwm_output.i2c_address, pwm_output.channel);
    }

    for (const auto& [i2c_address, available]: pca9685_controller_available_) {
        RCLCPP_INFO(rclcpp::get_logger("HexapodRobotHardwareInterface"), "require PCA9685 at i2c 0x%02x.", i2c_address);
    }

    i2c_available_ = false;
    i2c_fd_ = open(info_.hardware_parameters["i2c_device"].c_str(), O_RDWR);
    if (i2c_fd_ < 0) {
        RCLCPP_WARN(rclcpp::get_logger("HexapodRobotHardwareInterface"), "Cannot open %s, continuing in dummy mode.", info_.hardware_parameters["i2c_device"].c_str());
    } else {
        i2c_available_ = true;
    }

    return hardware_interface::CallbackReturn::SUCCESS;
}

hardware_interface::CallbackReturn
HexapodRobotHardwareInterface::on_activate(const rclcpp_lifecycle::State &) {
    RCLCPP_INFO(rclcpp::get_logger("HexapodRobotHardwareInterface"), "on_activate");
    return hardware_interface::CallbackReturn::SUCCESS;
}

hardware_interface::CallbackReturn
HexapodRobotHardwareInterface::on_deactivate(const rclcpp_lifecycle::State &) {
    RCLCPP_INFO(rclcpp::get_logger("HexapodRobotHardwareInterface"), "on_deactivate");
    return hardware_interface::CallbackReturn::SUCCESS;
}


std::vector<hardware_interface::StateInterface>
HexapodRobotHardwareInterface::export_state_interfaces() {
    std::vector<hardware_interface::StateInterface> state_interfaces;
    for (const hardware_interface::ComponentInfo &joint: info_.joints) {
        state_interfaces.emplace_back(hardware_interface::StateInterface(joint.name, hardware_interface::HW_IF_POSITION, &joint_states_[joint.name]));
    }
    return state_interfaces;
}

std::vector<hardware_interface::CommandInterface>
HexapodRobotHardwareInterface::export_command_interfaces() {
    std::vector<hardware_interface::CommandInterface> command_interfaces;
    for (const hardware_interface::ComponentInfo &joint: info_.joints) {
        command_interfaces.emplace_back(hardware_interface::CommandInterface(joint.name, hardware_interface::HW_IF_POSITION, &joint_commands_[joint.name]));
    }
    return command_interfaces;
}


hardware_interface::return_type
HexapodRobotHardwareInterface::read(const rclcpp::Time &, const rclcpp::Duration &) {
    for (const hardware_interface::ComponentInfo &joint: info_.joints) {
        joint_states_[joint.name]=joint_commands_[joint.name];
    }
    return hardware_interface::return_type::OK;
}

hardware_interface::return_type
HexapodRobotHardwareInterface::write(const rclcpp::Time &, const rclcpp::Duration &) {
    return hardware_interface::return_type::OK;
}

} // namespace

#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(hexapod_robot::HexapodRobotHardwareInterface, hardware_interface::SystemInterface)