/****************************************************************************
 *
 *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
 *   Author: Simon Wilks <sjwilks@gmail.com>
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. 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.
 * 3. Neither the name PX4 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.
 *
 ****************************************************************************/

/**
 * @file state_machine_helper_test.cpp
 * System state machine unit test.
 *
 */

#include "state_machine_helper_test.h"

#include "../state_machine_helper.h"
#include <unit_test.h>

class StateMachineHelperTest : public UnitTest
{
public:
	StateMachineHelperTest();
	virtual ~StateMachineHelperTest();

	virtual bool run_tests();

private:
	bool armingStateTransitionTest();
	bool mainStateTransitionTest();
	bool isSafeTest();
};

StateMachineHelperTest::StateMachineHelperTest() {
}

StateMachineHelperTest::~StateMachineHelperTest() {
}

bool StateMachineHelperTest::armingStateTransitionTest()
{
    // These are the critical values from vehicle_status_s and actuator_armed_s which must be primed
    // to simulate machine state prior to testing an arming state transition. This structure is also
    // use to represent the expected machine state after the transition has been requested.
    typedef struct {
        arming_state_t  arming_state;   // vehicle_status_s.arming_state
        bool            armed;          // actuator_armed_s.armed
        bool            ready_to_arm;   // actuator_armed_s.ready_to_arm
    } ArmingTransitionVolatileState_t;

    // This structure represents a test case for arming_state_transition. It contains the machine
    // state prior to transition, the requested state to transition to and finally the expected
    // machine state after transition.
    typedef struct {
        const char*                     assertMsg;                              // Text to show when test case fails
        ArmingTransitionVolatileState_t current_state;                          // Machine state prior to transition
        hil_state_t                     hil_state;                              // Current vehicle_status_s.hil_state
        bool                            condition_system_sensors_initialized;   // Current vehicle_status_s.condition_system_sensors_initialized
        bool                            safety_switch_available;                // Current safety_s.safety_switch_available
        bool                            safety_off;                             // Current safety_s.safety_off
        arming_state_t                  requested_state;                        // Requested arming state to transition to
        ArmingTransitionVolatileState_t expected_state;                         // Expected machine state after transition
        transition_result_t             expected_transition_result;             // Expected result from arming_state_transition
    } ArmingTransitionTest_t;

    // We use these defines so that our test cases are more readable
    #define ATT_ARMED true
    #define ATT_DISARMED false
    #define ATT_READY_TO_ARM true
    #define ATT_NOT_READY_TO_ARM false
    #define ATT_SENSORS_INITIALIZED true
    #define ATT_SENSORS_NOT_INITIALIZED false
    #define ATT_SAFETY_AVAILABLE true
    #define ATT_SAFETY_NOT_AVAILABLE true
    #define ATT_SAFETY_OFF true
    #define ATT_SAFETY_ON false

    // These are test cases for arming_state_transition
    static const ArmingTransitionTest_t rgArmingTransitionTests[] = {
        // TRANSITION_NOT_CHANGED tests

        { "no transition: identical states",
            { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
            vehicle_status_s::ARMING_STATE_INIT,
            { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_NOT_CHANGED },

        // TRANSITION_CHANGED tests

        // Check all basic valid transitions, these don't require special state in vehicle_status_t or safety_s

        { "transition: init to standby",
            { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
            vehicle_status_s::ARMING_STATE_STANDBY,
            { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },

        { "transition: init to standby error",
            { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
            vehicle_status_s::ARMING_STATE_STANDBY_ERROR,
            { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },

        { "transition: init to reboot",
            { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
            vehicle_status_s::ARMING_STATE_REBOOT,
            { vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },

        { "transition: standby to init",
            { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
            vehicle_status_s::ARMING_STATE_INIT,
            { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },

        { "transition: standby to standby error",
            { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
            vehicle_status_s::ARMING_STATE_STANDBY_ERROR,
            { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },

        { "transition: standby to reboot",
            { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
            vehicle_status_s::ARMING_STATE_REBOOT,
            { vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },

        { "transition: armed to standby",
            { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
            vehicle_status_s::ARMING_STATE_STANDBY,
            { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },

        { "transition: standby error to reboot",
            { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
            vehicle_status_s::ARMING_STATE_REBOOT,
            { vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },

        { "transition: in air restore to armed",
            { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
            vehicle_status_s::ARMING_STATE_ARMED,
            { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },

        { "transition: in air restore to reboot",
            { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
            vehicle_status_s::ARMING_STATE_REBOOT,
            { vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },

        // hil on tests, standby error to standby not normally allowed

        { "transition: standby error to standby, hil on",
            { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
            vehicle_status_s::ARMING_STATE_STANDBY,
            { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },

        // Safety switch arming tests

        { "transition: standby to armed, no safety switch",
            { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_NOT_AVAILABLE, ATT_SAFETY_OFF,
            vehicle_status_s::ARMING_STATE_ARMED,
            { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },

        { "transition: standby to armed, safety switch off",
            { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_OFF,
            vehicle_status_s::ARMING_STATE_ARMED,
            { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },

        // TRANSITION_DENIED tests

        // Check some important basic invalid transitions, these don't require special state in vehicle_status_t or safety_s

        { "no transition: init to armed",
            { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
            vehicle_status_s::ARMING_STATE_ARMED,
            { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },

        { "no transition: armed to init",
            { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
            vehicle_status_s::ARMING_STATE_INIT,
            { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },

        { "no transition: armed to reboot",
            { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
            vehicle_status_s::ARMING_STATE_REBOOT,
            { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },

        { "no transition: standby error to armed",
            { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
            vehicle_status_s::ARMING_STATE_ARMED,
            { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },

        { "no transition: standby error to standby",
            { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
            vehicle_status_s::ARMING_STATE_STANDBY,
            { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },

        { "no transition: reboot to armed",
            { vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
            vehicle_status_s::ARMING_STATE_ARMED,
            { vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },

        { "no transition: in air restore to standby",
            { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
            vehicle_status_s::ARMING_STATE_STANDBY,
            { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },

        // Sensor tests

        //{ "transition to standby error: init to standby - sensors not initialized",
        //    { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_NOT_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
        //    vehicle_status_s::ARMING_STATE_STANDBY,
        //    { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },

        // Safety switch arming tests

        { "no transition: init to standby, safety switch on",
            { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
            vehicle_status_s::ARMING_STATE_ARMED,
            { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
    };

	struct vehicle_status_s status = {};
	struct vehicle_status_flags_s status_flags = {};
	struct safety_s         safety = {};
	struct actuator_armed_s armed = {};
	struct battery_status_s battery = {};

    size_t cArmingTransitionTests = sizeof(rgArmingTransitionTests) / sizeof(rgArmingTransitionTests[0]);
    for (size_t i=0; i<cArmingTransitionTests; i++) {
        const ArmingTransitionTest_t* test = &rgArmingTransitionTests[i];

	const bool check_gps = false;

        // Setup initial machine state
        status.arming_state = test->current_state.arming_state;
        status_flags.condition_system_sensors_initialized = test->condition_system_sensors_initialized;
        status.hil_state = test->hil_state;
        // The power status of the test unit is not relevant for the unit test
        status_flags.circuit_breaker_engaged_power_check = true;
        safety.safety_switch_available = test->safety_switch_available;
        safety.safety_off = test->safety_off;
        armed.armed = test->current_state.armed;
        armed.ready_to_arm = test->current_state.ready_to_arm;

        // Attempt transition
        transition_result_t result = arming_state_transition(&status, battery, safety, test->requested_state, &armed,
				false /* no pre-arm checks */,
				nullptr /* no mavlink_log_pub */,
				&status_flags,
				5.0f, /* avionics rail voltage */
				(check_gps ? ARM_REQ_GPS_BIT : 0),
				2e6 /* 2 seconds after boot, everything should be checked */
				);

        // Validate result of transition
        ut_compare(test->assertMsg, test->expected_transition_result, result);
        ut_compare(test->assertMsg, status.arming_state, test->expected_state.arming_state);
        ut_compare(test->assertMsg, armed.armed, test->expected_state.armed);
        ut_compare(test->assertMsg, armed.ready_to_arm, test->expected_state.ready_to_arm);
    }

	return true;
}

bool StateMachineHelperTest::mainStateTransitionTest()
{
	// This structure represent a single test case for testing Main State transitions.
	typedef struct {
		const char* assertMsg;				// Text to show when test case fails
		uint8_t	 condition_bits;			// Bits for various condition_* values
		uint8_t	from_state;				// State prior to transition request
		main_state_t to_state;				// State to transition to
		transition_result_t	expected_transition_result;	// Expected result from main_state_transition call
	} MainTransitionTest_t;

	// Bits for condition_bits
	#define MTT_ALL_NOT_VALID		0
	#define MTT_ROTARY_WING			1 << 0
	#define MTT_LOC_ALT_VALID		1 << 1
	#define MTT_LOC_POS_VALID		1 << 2
	#define MTT_HOME_POS_VALID		1 << 3
	#define MTT_GLOBAL_POS_VALID		1 << 4

	static const MainTransitionTest_t rgMainTransitionTests[] = {

		// TRANSITION_NOT_CHANGED tests

		{ "no transition: identical states",
			MTT_ALL_NOT_VALID,
			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_NOT_CHANGED },

		// TRANSITION_CHANGED tests

		{ "transition: MANUAL to ACRO - rotary",
			MTT_ROTARY_WING,
			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ACRO, TRANSITION_CHANGED },

		{ "transition: MANUAL to ACRO - not rotary",
			MTT_ALL_NOT_VALID,
			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ACRO, TRANSITION_CHANGED },

		{ "transition: ACRO to MANUAL",
			MTT_ALL_NOT_VALID,
			commander_state_s::MAIN_STATE_ACRO, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },

		{ "transition: MANUAL to AUTO_MISSION - global position valid, home position valid",
			MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_MISSION, TRANSITION_CHANGED },

		{ "transition: AUTO_MISSION to MANUAL - global position valid, home position valid",
			MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
			commander_state_s::MAIN_STATE_AUTO_MISSION, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },

		{ "transition: MANUAL to AUTO_LOITER - global position valid",
			MTT_GLOBAL_POS_VALID,
			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_LOITER, TRANSITION_CHANGED },

		{ "transition: AUTO_LOITER to MANUAL - global position valid",
			MTT_GLOBAL_POS_VALID,
			commander_state_s::MAIN_STATE_AUTO_LOITER, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },

		{ "transition: MANUAL to AUTO_RTL - global position valid, home position valid",
			MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_RTL, TRANSITION_CHANGED },

		{ "transition: AUTO_RTL to MANUAL - global position valid, home position valid",
			MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
			commander_state_s::MAIN_STATE_AUTO_RTL, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },

		{ "transition: MANUAL to ALTCTL - not rotary",
			MTT_LOC_ALT_VALID,
			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED },

		{ "transition: MANUAL to ALTCTL - rotary, global position not valid, local altitude valid",
			MTT_ROTARY_WING | MTT_LOC_ALT_VALID,
			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED },

		{ "transition: MANUAL to ALTCTL - rotary, global position valid, local altitude not valid",
			MTT_ROTARY_WING | MTT_GLOBAL_POS_VALID,
			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED },

		{ "transition: ALTCTL to MANUAL - local altitude valid",
			MTT_LOC_ALT_VALID,
			commander_state_s::MAIN_STATE_ALTCTL, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },

		{ "transition: MANUAL to POSCTL - local position not valid, global position valid",
			MTT_GLOBAL_POS_VALID,
			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_POSCTL, TRANSITION_CHANGED },

		{ "transition: MANUAL to POSCTL - local position valid, global position not valid",
			MTT_LOC_POS_VALID,
			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_POSCTL, TRANSITION_CHANGED },

		{ "transition: POSCTL to MANUAL - local position valid, global position valid",
			MTT_LOC_POS_VALID,
			commander_state_s::MAIN_STATE_POSCTL, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },

		// TRANSITION_DENIED tests

		{ "no transition: MANUAL to AUTO_MISSION - global position not valid",
			MTT_ALL_NOT_VALID,
			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_MISSION, TRANSITION_DENIED },

		{ "no transition: MANUAL to AUTO_LOITER - global position not valid",
			MTT_ALL_NOT_VALID,
			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_LOITER, TRANSITION_DENIED },

		{ "no transition: MANUAL to AUTO_RTL - global position not valid, home position not valid",
			MTT_ALL_NOT_VALID,
			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },

		{ "no transition: MANUAL to AUTO_RTL - global position not valid, home position valid",
			MTT_HOME_POS_VALID,
			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },

		{ "no transition: MANUAL to AUTO_RTL - global position valid, home position not valid",
			MTT_GLOBAL_POS_VALID,
			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },

		{ "transition: MANUAL to ALTCTL - not rotary",
			MTT_ALL_NOT_VALID,
			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ALTCTL, TRANSITION_DENIED },

		{ "no transition: MANUAL to ALTCTL - rotary, global position not valid, local altitude not valid",
			MTT_ROTARY_WING,
			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ALTCTL, TRANSITION_DENIED },

		{ "no transition: MANUAL to POSCTL - local position not valid, global position not valid",
			MTT_ALL_NOT_VALID,
			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_POSCTL, TRANSITION_DENIED },
	};

	size_t cMainTransitionTests = sizeof(rgMainTransitionTests) / sizeof(rgMainTransitionTests[0]);
	for (size_t i=0; i<cMainTransitionTests; i++) {
		const MainTransitionTest_t* test = &rgMainTransitionTests[i];

		// Setup initial machine state
		struct vehicle_status_s current_vehicle_status = {};
		struct commander_state_s current_commander_state = {};
		struct vehicle_status_flags_s current_status_flags = {};

		current_commander_state.main_state = test->from_state;
		current_vehicle_status.is_rotary_wing = test->condition_bits & MTT_ROTARY_WING;
		current_status_flags.condition_local_altitude_valid = test->condition_bits & MTT_LOC_ALT_VALID;
		current_status_flags.condition_local_position_valid = test->condition_bits & MTT_LOC_POS_VALID;
		current_status_flags.condition_home_position_valid = test->condition_bits & MTT_HOME_POS_VALID;
		current_status_flags.condition_global_position_valid = test->condition_bits & MTT_GLOBAL_POS_VALID;
		current_status_flags.condition_auto_mission_available = true;

		// Attempt transition
		transition_result_t result = main_state_transition(current_vehicle_status, test->to_state, current_status_flags, &current_commander_state);

		// Validate result of transition
		ut_compare(test->assertMsg, test->expected_transition_result, result);
		if (test->expected_transition_result == result) {
			if (test->expected_transition_result == TRANSITION_CHANGED) {
				ut_compare(test->assertMsg, test->to_state, current_commander_state.main_state);
			} else {
				ut_compare(test->assertMsg, test->from_state, current_commander_state.main_state);
			}
		}
	}

	return true;
}

bool StateMachineHelperTest::isSafeTest()
{
	struct safety_s safety = {};
	struct actuator_armed_s armed = {};

	armed.armed = false;
	armed.lockdown = false;
	safety.safety_switch_available = true;
	safety.safety_off = false;
	ut_compare("is safe: not armed", is_safe(safety, armed), true);

	armed.armed = false;
	armed.lockdown = true;
	safety.safety_switch_available = true;
	safety.safety_off = true;
	ut_compare("is safe: software lockdown", is_safe(safety, armed), true);

	armed.armed = true;
	armed.lockdown = false;
	safety.safety_switch_available = true;
	safety.safety_off = true;
	ut_compare("not safe: safety off", is_safe(safety, armed), false);

	armed.armed = true;
	armed.lockdown = false;
	safety.safety_switch_available = true;
	safety.safety_off = false;
	ut_compare("is safe: safety off", is_safe(safety, armed), true);

	armed.armed = true;
	armed.lockdown = false;
	safety.safety_switch_available = false;
	safety.safety_off = false;
	ut_compare("not safe: no safety switch", is_safe(safety, armed), false);

	return true;
}

bool StateMachineHelperTest::run_tests()
{
	ut_run_test(armingStateTransitionTest);
	ut_run_test(mainStateTransitionTest);
	ut_run_test(isSafeTest);

	return (_tests_failed == 0);
}

ut_declare_test(stateMachineHelperTest, StateMachineHelperTest)
