// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/// @file	ap_limits.cpp
/// @brief	Imposes limits on location (geofence), altitude and other parameters.
///         Each breach will trigger an action or set of actions to recover. Adapted from geofence.
/// @author Andrew Tridgell
///         Andreas Antonopoulos

#include <AP_Limits.h>
#include <AP_Limit_Module.h>

const AP_Param::GroupInfo AP_Limits::var_info[] PROGMEM = {

	// @Param: ENABLED
	// @DisplayName: Enable Limits Library
	// @Description: Setting this to Enabled(1) will enable the limits system
	// @Values: 0:Disabled,1:Enabled
	// @User: Standard
	AP_GROUPINFO("ENABLED",		0,	AP_Limits,	_enabled),

	// @Param: REQUIRED
	// @DisplayName: Limits Library Required
	// @Description: Setting this to 1 will enable the limits pre-arm checklist
	// @Values: 0:Disabled,1:Enabled
	// @User: Standard
	AP_GROUPINFO("REQUIRED",	1,	AP_Limits,	_required),

	// @Param: DEBUG
	// @DisplayName: Enable Limits Debug
	// @Description: Setting this to 1 will turn on debugging messages on the console and via MAVLink STATUSTEXT messages
	// @Values: 0:Disabled,1:Enabled
	// @User: Standard
	AP_GROUPINFO("DEBUG",		2,	AP_Limits,	_debug),

	// @Param: SAFETIME
	// @DisplayName: Limits Safetime
	// @Description: Automatic return of controls to pilot. Set to 0 to disable (full RTL) or a number of seconds after complete recovery to return the controls to the pilot in STABILIZE
	// @Values: 0:Disabled,1-255:Seconds before returning control
	// @User: Standard
	AP_GROUPINFO("SAFETIME",	3,	AP_Limits,	_safetime),

	// @Param: CHANNEL
	// @DisplayName: Limits Channel
	// @Description: Channel for Limits on/off control. If channel exceeds LIMITS_ENABLE_PWM, it turns limits on, and vice-versa.
	// @Range: 1-8
	// @User: Standard
	AP_GROUPINFO("CHANNEL",		4,	AP_Limits,	_channel),

	// @Param: RECMODE
	// @DisplayName: Limits Recovery Mode
	// @Description: Select how Limits should "recover". Set to 0 for RTL-like mode, where the vehicle navigates back to home until it is "safe". Set to 1, for bounce-mode, where the vehicle will hold position when it hits a limit. RTL mode is better for large fenced areas, Bounce mode for smaller spaces. Note: RTL mode will cause the vehicle to yaw 180 degrees  (turn around) to navigate towards home when it hits a limit.
	// @Values: 0:RTL mode, 1: Bounce mode
    // @User: Standard
	AP_GROUPINFO("RECMODE",		5,	AP_Limits,	_recmode),

    AP_GROUPEND
};

AP_Limits::AP_Limits() {
	last_trigger = 0;
	last_action = 0;
	last_recovery = 0;
	last_clear = 0;
	last_status_update = 0;
	breach_count = 0;
	mods_enabled = 0;
	mods_required = 0;
	mods_triggered = 0;
	mods_recovering = 0;
	old_mode_switch = 0;
	_channel = 0;
	_state = LIMITS_INIT;
}

void AP_Limits::modules(AP_Limit_Module *m)
{
	_modules_head = m;
}

bool AP_Limits::init() {
	AP_Limit_Module	*m = modules_first();

	while (m) {
			m->init();
			m = modules_next();
	}
	return true;
}


bool AP_Limits::enabled() {
	return _enabled;
}

bool AP_Limits::debug() {
	return _debug;
}


AP_Limit_Module	*AP_Limits::modules_first() {
	_modules_current = _modules_head; // reset current to head of list
	return _modules_head;
}

AP_Limit_Module	*AP_Limits::modules_current() {
	return _modules_current;
}

AP_Limit_Module	*AP_Limits::modules_next() {
	if (_modules_current) {
		_modules_current = _modules_current->next(); // move to "next" (which might be NULL)
	}
	return _modules_current;
}

uint8_t	AP_Limits::modules_count() {
	_modules_count = 0;
	AP_Limit_Module	*m = _modules_head;

	while (m) {
		_modules_count++;
		m = m->next();
	}
	return _modules_count;
}

AP_Limit_Module	*AP_Limits::modules_last() {
	AP_Limit_Module	*m = _modules_head;
	while (m && m->next()) {
		m = m->next();
	}
	return m;
}

void AP_Limits::modules_add(AP_Limit_Module	*m) {
	if (_modules_head) { // if there is a module linked
		modules_last()->link(m); // add to the end of the list
	}
	else {
		_modules_head = m; // otherwise, this will be the "head"
	}
}

bool AP_Limits::required() {
	return _required;
}

bool AP_Limits::check_all() {
	return (bool) check_triggered(false); // required=false, means "all"
}

bool AP_Limits::check_required() {
	return (bool) check_triggered(true); // required=true, means "only required modules"
}

bool AP_Limits::check_triggered(bool only_required) {

		// check all enabled modules for triggered limits
		AP_Limit_Module *mod = _modules_head;

		// reset bit fields
		mods_triggered = 0;
		mods_enabled = 0;
		mods_required = 0;

		while (mod) {

			unsigned module_id = mod->get_module_id();

			// We check enabled, triggered and required across all modules
			// We set all the bit-fields each time we check, keeping them up to date

			if (mod->enabled()) {
				mods_enabled |= module_id;

				if (mod->required()) mods_required |= module_id;
				if (mod->triggered()) mods_triggered |= module_id;
			}

			mod = mod->next();
		}

		if (only_required) {
			return (bool) (mods_required & mods_triggered); // just modules that are both required AND triggered. (binary AND)
		}
		else return (bool) (mods_triggered);
}

AP_Int8 AP_Limits::state() {
	return _state;
}

AP_Int8 AP_Limits::safetime() {
	return _safetime;
}

void AP_Limits::set_state(int s) {
	_state = (int) s;
}


AP_Int8 AP_Limits::channel() {
	return _channel;
}

AP_Int8 AP_Limits::recmode() {
	return _recmode;
}