
#include <canopen_motor_node/motor_chain.h>
#include <canopen_motor_node/handle_layer.h>
#include <socketcan_interface/xmlrpc_settings.h>
using namespace canopen;

MotorChain::MotorChain(const ros::NodeHandle &nh, const ros::NodeHandle &nh_priv) :
        RosChain(nh, nh_priv), motor_allocator_("canopen_402", "canopen::MotorBase::Allocator") {}

bool MotorChain::nodeAdded(XmlRpc::XmlRpcValue &params, const canopen::NodeSharedPtr &node, const LoggerSharedPtr &logger)
{
    std::string name = params["name"];
    std::string &joint = name;
    if(params.hasMember("joint")) joint.assign(params["joint"]);

    if(!robot_layer_->getJoint(joint)){
        ROS_ERROR_STREAM("joint " + joint + " was not found in URDF");
        return false;
    }

    std::string alloc_name = "canopen::Motor402::Allocator";
    if(params.hasMember("motor_allocator")) alloc_name.assign(params["motor_allocator"]);

    XmlRpcSettings settings;
    if(params.hasMember("motor_layer")) settings = params["motor_layer"];

    MotorBaseSharedPtr motor;

    try{
        motor = motor_allocator_.allocateInstance(alloc_name, name + "_motor", node->getStorage(), settings);
    }
    catch( const std::exception &e){
        std::string info = boost::diagnostic_information(e);
        ROS_ERROR_STREAM(info);
        return false;
    }

    if(!motor){
        ROS_ERROR_STREAM("Could not allocate motor.");
        return false;
    }

    motor->registerDefaultModes(node->getStorage());
    motors_->add(motor);
    logger->add(motor);

    HandleLayerSharedPtr handle = std::make_shared<HandleLayer>(joint, motor, node->getStorage(), params);

    canopen::LayerStatus s;
    if(!handle->prepareFilters(s)){
        ROS_ERROR_STREAM(s.reason());
        return false;
    }

    robot_layer_->add(joint, handle);
    logger->add(handle);

    return true;
}

bool MotorChain::setup_chain() {
    motors_.reset(new LayerGroupNoDiag<MotorBase>("402 Layer"));
    robot_layer_.reset(new RobotLayer(nh_));

    ros::Duration dur(0.0) ;

    if(RosChain::setup_chain()){
        add(motors_);
        add(robot_layer_);

        if(!nh_.param("use_realtime_period", false)){
            dur.fromSec(boost::chrono::duration<double>(update_duration_).count());
            ROS_INFO_STREAM("Using fixed control period: " << dur);
        }else{
            ROS_INFO("Using real-time control period");
        }
        cm_.reset(new ControllerManagerLayer(robot_layer_, nh_, dur));
        add(cm_);

        return true;
    }

    return false;
}
