/*
 * flipper_plugin.cpp
 *
 *  Gazebo plugin to listen to joint controls and publish to transmission the joint states
 *
 *  Created on: 17 Oct 2014
 *      Author: timothyw
 */


#include "casualty_gazebo_plugins/autolevel/autolevel.h"

#include <autolevel/autolevel.hpp>
#include <std_msgs/Float64.h>

#define LOG_START_NODE      "CasualtyGazeboAutoLevel :: "
#define LOG_START_CHAIN     "CasualtyGazeboAutoLevelChain :: "
#define LOG_START_COMM      "CasualtyGazeboDXLCOMM :: "
#define LOG_START_SERVO     "CasualtyGazeboServo :: "
#define LOG_START_XSENS     "CasualtyGazeboXSens :: "


namespace casualty {
namespace gazebo {

GazeboXSens::GazeboXSens() :
    XSens(),
    levelPitchPosition(0),
    levelRollPosition(0)
{}

GazeboXSens::~GazeboXSens() {
}

void GazeboXSens::connect() {
    // Initialise values
    levelRollPosition = 0;
    levelPitchPosition = 0;

    // Subscribe
    ros::NodeHandle node;
    imuSub = node.subscribe("/negotiator/imu", 1, &GazeboXSens::callbackImu, this);
}

void GazeboXSens::disconnect() {
    // Un-subscribe
    imuSub.shutdown();
    transmissionLevelRollPub.shutdown();
    transmissionLevelPitchPub.shutdown();
}

tf::Quaternion GazeboXSens::getOrientation() {
    geometry_msgs::Quaternion orien = imuData.orientation;
    tf::Quaternion quaternion(orien.x, orien.y, orien.z, orien.w);
    //ROS_INFO("%sgetOrientation(): %f %f %.f %f", LOG_START_XSENS, orien.x, orien.y, orien.z, orien.w);

    return quaternion;
}

void GazeboXSens::callbackImu(sensor_msgs::ImuPtr imu) {
    imuData = *imu;
}

GazeboAutoLevelChain::GazeboAutoLevelChain() : AutoLevelChain()
{
}

GazeboAutoLevelChain::~GazeboAutoLevelChain() {
}

bool GazeboAutoLevelChain::readServos() {
    //ROS_INFO("%sreadServos() - does nothing", LOG_START_CHAIN);
    return true;
}

void GazeboAutoLevelChain::connect() {
    //ROS_INFO("%sconnect() - starts each pub/sub", LOG_START_CHAIN);
    for (unsigned int i = 0; i < numServos; i++) {
        if (servos[i] != NULL) {
            servos[i]->connect();
        }
    }
}

void GazeboAutoLevelChain::disconnect() {
    //ROS_INFO("%sdisconnect() - stops each pub/sub", LOG_START_CHAIN);
    for (unsigned int i = 0; i < numServos; i++) {
        if (servos[i] != NULL) {
            servos[i]->disconnect();
        }
    }
}

Dynamixel* GazeboAutoLevelChain::make(ConfigElementPtr cfg) {
    std::string topic = cfg->getParam("gazeboTopic", "/missing_topic");

    return new GazeboAutoLevelServo(topic, *this);
}

bool GazeboAutoLevelServo::readServo() {
    return true;
}

void GazeboAutoLevelServo::connect() {
    if (!active) {
        //ROS_INFO("%sconnect()", LOG_START_SERVO);

        //ROS_INFO("%sadvertise: %s", LOG_START_SERVO, pubName.c_str());
        ros::NodeHandle node;
        servoPub = node.advertise<std_msgs::Float64>(pubName, 1);
        //servoSub = node.subscribe(subName, 1, &GazeboAutoLevelServo::callbackServoPos, this);

        active = true;
    }
}

void GazeboAutoLevelServo::disconnect() {
    //ROS_INFO("%sdisconnect()", LOG_START_SERVO);
    active = false;
    servoPub.shutdown();
    //servoSub.shutdown();
}

void GazeboAutoLevelServo::setPos(double pos, double vel) {
    setPos(pos);
}

void GazeboAutoLevelServo::setPos(double pos) {
    if (active) {
        currentPose = pos;
        if (invert) currentPose = -currentPose;

        //ROS_INFO("%sSetting pos to: %lf (id:%d)", LOG_START_SERVO, currentPose, id);
        std_msgs::Float64 posMsg;
        posMsg.data = currentPose;
        servoPub.publish(posMsg);
    }
}

void GazeboAutoLevelServo::callbackServoPos(std_msgs::Float64 pos) {
    currentPose = pos.data;
}

} // namespace gazebo
} // namespace casualty


/**
 * ROS Node components
 */
int main(int argc, char** argv) {
    // Init ROS
    ros::init(argc, argv, "qualplan_test");

    // Node Handle - Use '~' when loading config parameters
    // Use without '~' when publish/subscribe/service
    ros::NodeHandle nh("~");

    // Create autolevel factories
    casualty::FactoryXSensPtr xsensFactory = new casualty::gazebo::GazeboFactoryXSens();
    casualty::FactoryAutoLevelChainPtr autolevelChainFactory = new casualty::gazebo::GazeboFactoryAutoLevelChain();

    // Load Config element
    crosbot::ConfigElementPtr config = new crosbot::ROSConfigElement(nh);

    // Create and configure AutoLevel
    casualty::AutoLevel autolevel(xsensFactory, autolevelChainFactory);
    autolevel.configure(config);

    // Start
    autolevel.start();

    while (ros::ok()) {
        ROS_INFO("%s Spinning", LOG_START_NODE);
        ros::spin();
    }

    // Stop
    autolevel.stop();

}


