/*
 * Copyright (C) 2008, Morgan Quigley and Willow Garage, Inc.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *   * Redistributions of source code must retain the above copyright notice,
 *     this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above copyright
 *     notice, this list of conditions and the following disclaimer in the
 *     documentation and/or other materials provided with the distribution.
 *   * Neither the names of Stanford University or Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived from
 *     this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include <px4.h>

using namespace px4;

/**
 * This tutorial demonstrates simple receipt of messages over the PX4 middleware system.
 */
void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) {
	PX4_INFO("I heard: [%llu]", msg.timestamp_last_valid);
}
void rc_channels_callback2(const PX4_TOPIC_T(rc_channels) &msg) {
	PX4_INFO("I heard (2): [%llu]", msg.timestamp_last_valid);
}

class RCHandler {
public:
	void  callback(const PX4_TOPIC_T(rc_channels) &msg) {
		PX4_INFO("RCHandler class heard: [%llu]", msg.timestamp_last_valid);
	}
};

RCHandler rchandler;


namespace px4
{
bool task_should_exit = false;
}

PX4_MAIN_FUNCTION(subscriber) {
	/**
	 * The ros::init() function needs to see argc and argv so that it can perform
	 * any ROS arguments and name remapping that were provided at the command line. For programmatic
	 * remappings you can use a different version of init() which takes remappings
	 * directly, but for most command-line programs, passing argc and argv is the easiest
	 * way to do it.  The third argument to init() is the name of the node.
	 *
	 * You must call one of the versions of px4::init() before using any other
	 * part of the PX4/ ROS system.
	 */
	px4::init(argc, argv, "listener");

	/**
	 * NodeHandle is the main access point to communications with the ROS system.
	 * The first NodeHandle constructed will fully initialize this node, and the last
	 * NodeHandle destructed will close down the node.
	 */
	px4::NodeHandle n;

	/* Define parameters */
	px4_param_t p_sub_interv = PX4_PARAM_INIT("SUB_INTERV", 100);
	px4_param_t p_test_float = PX4_PARAM_INIT("SUB_TESTF", 3.14f);

	/* Read the parameter back for testing */
	int32_t sub_interval;
	float test_float;
	PX4_PARAM_GET(p_sub_interv, &sub_interval);
	PX4_INFO("Param SUB_INTERV = %d", sub_interval);
	PX4_PARAM_GET(p_test_float, &test_float);
	PX4_INFO("Param SUB_TESTF = %f", (double)test_float);

	/**
	 * The subscribe() call is how you tell ROS that you want to receive messages
	 * on a given topic.  This invokes a call to the ROS
	 * master node, which keeps a registry of who is publishing and who
	 * is subscribing.  Messages are passed to a callback function, here
	 * called chatterCallback.  subscribe() returns a Subscriber object that you
	 * must hold on to until you want to unsubscribe.  When all copies of the Subscriber
	 * object go out of scope, this callback will automatically be unsubscribed from
	 * this topic.
	 *
	 * The second parameter to the subscribe() function is the size of the message
	 * queue.  If messages are arriving faster than they are being processed, this
	 * is the number of messages that will be buffered up before beginning to throw
	 * away the oldest ones.
	 */
	PX4_SUBSCRIBE(n, rc_channels, rc_channels_callback, sub_interval);
	PX4_SUBSCRIBE(n, rc_channels, rc_channels_callback2, 1000);
	PX4_SUBSCRIBE(n, rc_channels, RCHandler::callback, rchandler, 1000);
	PX4_INFO("subscribed");

	/**
	 * px4::spin() will enter a loop, pumping callbacks.  With this version, all
	 * callbacks will be called from within this thread (the main one).  px4::spin()
	 * will exit when Ctrl-C is pressed, or the node is shutdown by the master.
	 */
	n.spin();
	PX4_INFO("finished, returning");

	return 0;
}