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

#include <cmath>

CallbackReturn mg_wheel_interface::MgOdriveInterface::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/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 mg_wheel_interface::MgOdriveInterface::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("mg_wheel_interface"),
            "Failed to open serial port (Is the odrive plugged in?)");
        return CallbackReturn::ERROR;
    }
    return CallbackReturn::SUCCESS;
}


CallbackReturn mg_wheel_interface::MgOdriveInterface::on_shutdown(const rclcpp_lifecycle::State &)
{
    if(odrive_serial_interface.IsOpen()){
        std::string cmd_left;
        std::string cmd_right;
        int hash;

        cmd_left  = "v 1 " + std::to_string(0)  + " ";
        cmd_right = "v 0 " + std::to_string(0) + " ";

        hash = 0;
        for(const auto &c: cmd_left) {
            hash ^= c;
        }
        cmd_left += "*"+std::to_string(hash)+"\n";

        hash = 0;
        for(const auto &c: cmd_right) {
            hash ^= c;
        }
        cmd_right += "*"+std::to_string(hash)+"\n";
        odrive_serial_interface.Close();
    }
    return CallbackReturn::SUCCESS;
}


std::vector<hardware_interface::StateInterface> mg_wheel_interface::MgOdriveInterface::export_state_interfaces()
{
    std::vector<hardware_interface::StateInterface> state_interfaces;
    state_interfaces.reserve(2);
    state_interfaces.push_back(hardware_interface::StateInterface(
        "left_motor", hardware_interface::HW_IF_POSITION, &left_wheel_pos_state
    ));
    state_interfaces.push_back(hardware_interface::StateInterface(
        "right_motor", hardware_interface::HW_IF_POSITION, &right_wheel_pos_state
    ));
    return  state_interfaces;
}


std::vector<hardware_interface::CommandInterface> mg_wheel_interface::MgOdriveInterface::export_command_interfaces()
{
    std::vector<hardware_interface::CommandInterface> command_interfaces;
    command_interfaces.reserve(2);
    command_interfaces.push_back(hardware_interface::CommandInterface(
        "left_motor", hardware_interface::HW_IF_VELOCITY, &left_wheel_vel_cmd
    ));
    command_interfaces.push_back(hardware_interface::CommandInterface(
        "right_motor", hardware_interface::HW_IF_VELOCITY, &right_wheel_vel_cmd
    ));
    return  command_interfaces;
}

hardware_interface::return_type mg_wheel_interface::MgOdriveInterface::read(const rclcpp::Time &time, const rclcpp::Duration &period)
{
    std::string resp_l, resp_r;
    odrive_serial_interface.Write("f 1\n");
    odrive_serial_interface.ReadLine(resp_l,'\n',10);
    odrive_serial_interface.Write("f 0\n");
    odrive_serial_interface.ReadLine(resp_r,'\n',10);
    left_wheel_pos_state =   std::stod(resp_l)*2*M_PI;
    right_wheel_pos_state = -std::stod(resp_r)*2*M_PI;
    return hardware_interface::return_type::OK ;
}


hardware_interface::return_type mg_wheel_interface::MgOdriveInterface::write(const rclcpp::Time &time, const rclcpp::Duration &period)
{
    std::string cmd_left;
    std::string cmd_right;
    int hash;

    cmd_left  = "v 1 " + std::to_string(left_wheel_vel_cmd/2*M_PI)  + " ";
    cmd_right = "v 0 " + std::to_string(-right_wheel_vel_cmd/2*M_PI) + " ";

    hash = 0;
    for(const auto &c: cmd_left) {
        hash ^= c;
    }
    cmd_left += "*"+std::to_string(hash)+"\n";

    hash = 0;
    for(const auto &c: cmd_right) {
        hash ^= c;
    }
    cmd_right += "*"+std::to_string(hash)+"\n";


    try {
        odrive_serial_interface.Write(cmd_left + cmd_right);
    } catch(const std::runtime_error &e){
        return hardware_interface::return_type::ERROR;
    }
    return hardware_interface::return_type::OK;
}