

#include <ros/ros.h>

#include <crosbot/config.hpp>

#include "negotiator_drivers/flippers/arduino.h"
#include "negotiator_drivers/flippers/flippers.h"
#include "negotiator_drivers/flippers/pololu.h"

namespace casualty {

int node_main(int argc, char** argv) {
    ros::init(argc, argv, "flippers");
    ros::NodeHandle nh("~");

    // Get config
    crosbot::ROSConfigElementPtr cf = new crosbot::ROSConfigElement(nh);

    // Choose hardware interface
    FlippersHardwareInterface *interface = NULL;
    std::string interfaceType = cf->getParam("interface", "arduino");
    if (interfaceType == "pololu") {
        ROS_ERROR("Flippers: Pololu interface disabled");
        //ROS_INFO("Flippers: Pololu interface");
        //interface = new FlippersPololuInterface();
    } else {
        ROS_INFO("Flippers: Arduino interface");
        interface = new FlippersArduinoInterface();
    }

    // Create flippers ROS
    FlippersROSPtr flippers = new FlippersROS(interface);

    //  Configure flipper node
    flippers->configure(cf);

    flippers->connect();

    while (ros::ok()) {
        ros::spin();
    }

    flippers->disconnect();

    return 0;
}

} // namespace casualty

int main(int argc, char** argv) {
    return casualty::node_main(argc, argv);
}


