#!/usr/bin/env python
import rospy
import sys
import os
import yaml
import smach

from flexbe_core import Logger

from flexbe_testing.state_tester import StateTester

if __name__ == '__main__':
	rospy.init_node('flexbe_testing')

	Logger.initialize()

	filenames = list()
	if len(sys.argv) > 1:
		print 'Loading provided test cases...'
		for arg in sys.argv[1:]:
			if os.path.isfile(os.path.expanduser(arg)):
				filenames.append(os.path.expanduser(arg))

	test_cases = dict()
	for filename in filenames:
		with open(filename, 'r') as f:
			config = yaml.load(f)
		(folder, name) = os.path.split(filename)
		test_cases[name] = config

	perform_rostest = rospy.get_param('~perform_rostest', False)
	test_package = rospy.get_param('~package')

	if perform_rostest and test_package == '':
		rospy.logerr('Need to pass the name of the package under testing as arg "package" to the flexbe_testing launch file!')

	print 'Ready for testing! (%d tests)' % len(test_cases)
	tester = StateTester()

	success_cases = 0
	for test_name, test_config in test_cases.iteritems():
		if rospy.is_shutdown():
			break
		success_cases += tester.run_test(test_name, test_config)

	print ''
	result_color = '92' if success_cases == len(test_cases) else '93'
	print '\033[%smTesting completed, %d of %d tests successful.\033[0m' % (result_color, success_cases, len(test_cases))
	print ''

	if perform_rostest:
		tester.perform_rostest(test_package)

	
