/*
* This file is part of libximu
*
* Copyright(c) sschulz <AT> techfak.uni-bielefeld.de
* http://opensource.cit-ec.de/projects/libximu
*
* This file may be licensed under the terms of the
* GNU Lesser General Public License Version 3 (the ``LGPL''),
* or (at your option) any later version.
*
* Software distributed under the License is distributed
* on an ``AS IS'' basis, WITHOUT WARRANTY OF ANY KIND, either
* express or implied. See the LGPL for the specific language
* governing rights and limitations.
*
* You should have received a copy of the LGPL along with this
* program. If not, go to http://www.gnu.org/licenses/lgpl.html
* or write to the Free Software Foundation, Inc.,
* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
*
* The development of this software was supported by the
* Excellence Cluster EXC 277 Cognitive Interaction Technology.
* The Excellence Cluster EXC 277 is a grant of the Deutsche
* Forschungsgemeinschaft (DFG) in the context of the German
* Excellence Initiative.
*/

#include "XIMU.h"

void test_slot_incoming_time(struct tm now){
	char buf[80];
	strftime (buf,80,"%c",&now);
	printf("> incoming time: %s\n",buf);
}

int main(int argc, char **argv){
	double euler[3];
	unsigned int deviceid;
	
	if (argc < 2){
		printf("usage: %s /dev/ttyUSB0 [optional: enable debug 0/1]\n\n",argv[0]);
		exit(-1);
	}

	//intantiate XIMU object
	XIMU ximu(argv[1], XIMU::XIMU_LOGLEVEL_LOG);
	
	//set loglevel
	if(argc == 3){
		printf("> requested debug loglevel\n");
		ximu.set_loglevel(XIMU::XIMU_LOGLEVEL_DEBUG);
	}
	
	//scan for device & exit on failure
	if (!ximu.get_device_detected()){
		printf("> no device found?!\n");
		exit(-1);
	}

	//test1: get decviceid:
	if (!ximu.get_register(XIMU::REGISTER_ADDRESS_DeviceID, &deviceid)){
		printf("> ERROR: cant fetch deviceid\n");
		exit(-1);
	}
	printf("> found device with id 0x%04X\n",deviceid);
	
	
	//test2: fetch euler data
	for(int i=0; i<10;){
		//wait for new data
		if(ximu.get_euler(euler)){
			printf("> [%3d] got euler angles [%6.2f, %6.2f, %6.2f ]\n",i,euler[0],euler[1],euler[2]);
			usleep(100000);
			i++;
		}else{
			usleep(100);
		}
	}
	
	//test3: connect to signal:
	printf("> activating callback slot for time info (&waiting 5s)\n");
	ximu.connect_slot_incoming_data_time(&test_slot_incoming_time);
	
	//wait 5s
	usleep(5*1000000);
	
	//disconnet slot
	printf("> disconecting callback\n");
	ximu.disconnect_slot_incoming_data_time();
	
	//test4: test tare:
	ximu.get_euler(euler);
	printf("> before tare -> got euler angles [%6.2f, %6.2f, %6.2f ]\n",euler[0],euler[1],euler[2]);
	printf("> sending tare...\n");
	ximu.send_command_algorithm_tare();
	usleep(1000*1000); //100ms sleep
	ximu.get_euler(euler);
	printf("> after tare  -> got euler angles [%6.2f, %6.2f, %6.2f ] (should be close to zero)\n",euler[0],euler[1],euler[2]);
	
// 	while(1);
	
	
	
	printf("> all done. exiting\n");
	
	return 0;

}