/*
 * autolevel.cpp
 *
 *  Created on: 06/09/2012
 *      Author: mmcgill
 */

#include "victim_id/class.hpp"

#include <crosbot/config.hpp>
#include <crosbot/thread.hpp>
#include <crosbot/utils.hpp>

#include "victim_id/victimId.hpp"
#include "victim_id/util.hpp"

#include <std_msgs/String.h>

#define LOG_START	"AutoVictimIdNode :: "

using namespace crosbot;

namespace casualty {

class AutoVictimIdNode {
public :
	void setup(ros::NodeHandle& nh);
	void shutdown();

	void callbackControl(std_msgs::StringPtr controlMsg);
private:
	AutoVictimIdPtr victimId;
	Mutex controlMutex;

	ros::Subscriber controlSub;

	void startVictimId();
	void stopVictimId();
};

void AutoVictimIdNode::setup(ros::NodeHandle& nh) {
	// Load victimid
	victimId = new AutoVictimId();

	// Load config
	ros::NodeHandle nhConfig("~");
	ConfigElementPtr config = new ROSConfigElement(nhConfig);
	victimId->configure(config);
	victimId->init(nh);

	// Register callbacks
	controlSub = nh.subscribe("/victim_id/control", 1, &AutoVictimIdNode::callbackControl, this);

	// Start when on launch, in paused mode
	startVictimId();
}

void AutoVictimIdNode::shutdown() {
	LOG("%s Shutting down\n", LOG_START);
	stopVictimId();

	// Cleanup
	victimId = NULL;
	LOG("%s Shutdown complete\n", LOG_START);
}

void AutoVictimIdNode::callbackControl(std_msgs::StringPtr controlMsg) {
	std::string control = controlMsg->data;
	if (control == "pause") {
	    victimId->setPaused(true);
	//} else if (control == "start") {
	//	startVictimId();
	} else if (control == "resume") {
	    victimId->setPaused(false);
	} else if (control == "stop") {
		stopVictimId();
	} else if (control == "toggle_dt") {
	    victimId->toggleRunDT();
	} else if (control == "explorer_mode_right") {
	    victimId->setExplorerMode(EXPLORER_MODE_RIGHT_WALL_STR);
	} else if (control == "explorer_mode_left") {
	    victimId->setExplorerMode(EXPLORER_MODE_LEFT_WALL_STR);
	}
}

void AutoVictimIdNode::startVictimId() {
	// Start Thread
	//LOG("%s start() grabbing lock\n", LOG_START);
	Lock lock(controlMutex);
	//LOG("%s start() has lock\n", LOG_START);
	if (!victimId->isAlive()) {
		//LOG("%s start() do start\n", LOG_START);
		victimId->start();
	}
}

void AutoVictimIdNode::stopVictimId() {
	Lock lock(controlMutex);
	if (victimId->isAlive()) {
		victimId->stop();
		victimId->join();
	}
	LOG("%s Stopped\n", LOG_START);
}

}; // namespace casualty {

int main(int argc, char** argv) {
    //ros::init(argc, argv, "autolevel");
	ros::init(argc, argv, "victim_id");

	// Setup the node
	ros::NodeHandle nh;
	casualty::AutoVictimIdNode victimIdNode;
	victimIdNode.setup(nh);

    while (ros::ok()) {
        LOG("%s Spinning\n", LOG_START);
        ros::spin();
    }

    // Shutdown the node
    victimIdNode.shutdown();

	return 0;
}

