/*
 *  acm - Auto-Pilot System module
 *  Copyright (C) 2007 Umberto Salsi
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; version 2 dated June, 1991.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program;  if not, write to the Free Software
 *  Foundation, Inc., 675 Mass Ave., Cambridge, MA 02139, USA.
 */

/*
	The proportional-integrative control algorithm ("PI" from here on) is
	extensively used in this module. The PI algo can be applied to a
	black-box system

	y(t) = f(x(t), t)

	were

	t = time
	x(t) = controlling variable (ex.: aileron position)
	y(t) = resulting output (ex.: roll speed)

	The basic problem is how to adjust the value of x in order to achieve
	and maintain a given target output y_T. The PI algo addresses just this
	problem. It is expressed by this formula:

	x(t) = x_ss + Gp * err(t) + Gi * integral(err(z)*dz, z=0...t)

	were the integral of the error ranges from 0 to t, and the error is
	defined by

	err(t) = y(t) - y_T

	and x_ss is a constant depending on the initial conditions, Gp is the
	proportional gain, Gi is the integral gain. The time derivative of the
	x(t) function is

	x_dot(t) = Gp * y_dot(t) + Gi * (y(t) - y_T)

	or, in the discrete domain:

	x(i+1) = x(i) + Gp * (y(i) - y(i-1)) + Gi * (y(i) - y_T) * dt

	were:

	i = current sample time
	i-/+1 = previous or next sample time
	x(i+1) = new calculated value for the controlling variable
	y(i) = current value of the controlled variable
	y(i-1) = previous value at t-dt of the controlled variable

	Try trickiest part of the PI algo is to choose a good controlled
	variable y(t) and a good value for the proportionl gain Gp and the
	integral gain Gi that work for all the aircraft models and in any
	flight condition of speed and altitude.

	The table below summarizes the algo used with every autopilot subsystem:

    A*  x(t)      y(t)              Algorithm used
	--  --------  ----------------  -------------------------------------
	AP  elevator  pitch rate        bare incremental algo
	AW  ailerons  roll rate         PI with varying target roll rate y_T
	AC  rudder    lateral accel.    PI
*/

#include <math.h>
#include <string.h>
#include "flaps.h"
#include "hsi.h"
#include "../util/memory.h"
#include "pm.h"
#include "terrain.h"
#include "gear.h"
#include "../util/units.h"

#define aps_IMPORT
#include "aps.h"

typedef struct {
	craft * c;

	/*
		Pitch rate control.
	*/
	_BOOL     pitch_rate_enabled;
	double    pitch_rate_transfer;
	double    pitch_rate_target;        /* pitch rate to hold (RAD/s) */
	double    pc_prev_time;        /* prev update time */
	double    pc_prev_pitch_dot;   /* previous pitch rate */
	double    delta_Se;         /* current elevator correction [-1.0,1.0] */

	/*
	 * Bank rate control.
	 */
	_BOOL     bank_rate_enabled;   /* bank rate control enabled */
	double    bank_rate_transfer;
	double    bank_rate_target;    /* target rate of bank (RAD/s) */
	double    bank_rate_prev_time;
	double    bank_rate_prev_a_dot;
	double    bank_rate_prev_a_dot_exp;
	double    delta_Sa;   /* current ailerons correction [-1.0,+1.0] */

	/*
		Hold altitude and hold VS autopilot (AP) state.
	*/
	_BOOL     ap_enabled;
	_BOOL     ap_hold_altitude;    /* hold altitude or hold VS? */
	double    ap_target_altitude;  /* target altitude (m) */
	double    ap_prev_time;        /* previous update (s) */
	double    ap_target_vs;        /* target vertical speed (m/s) */
	double    ap_prev_vs;          /* previous v.s. (m/s) */

	/*
		Rudder auto-coordination (AC).
	*/
	_BOOL     ac_enabled;
	double    ac_prev_a;           /* the lateral accel. c->G.y ... */
	double    ac_prev_time;        /* ... at this time */
	double    ac_delta_Sr;         /* AC rudder correction */

	/*
		AutoThrottle (AT).
	*/
	_BOOL     at_enabled;
	double    at_target_v;         /* target IAS speed (ft/s) */
	double    at_prev_v;           /* speed at the previous... */
	double    at_prev_time;        /* ...time (s) */

	/*
		Auto-turn (AW).
	*/
	_BOOL     aw_enabled;
	double    aw_bank_max;         /* max bank angle (RAD) */
	double    aw_w_target;         /* target z turn rate (RAD/s) */
	double    aw_prev_time;

	/*
		Auto Landing (AL).
	*/
	_BOOL     al_enabled;
	double    al_prev_adiff;
	double    al_prev_time;
	double    al_v_touchdown; /* 0.0 = still unknown (m/s) */
	_BOOL     al_touchdown; /* bouncing fix: set when first contact with ground */

	/*
		Auto Navigation (AN).
	*/
	_BOOL     an_enabled;
	double    an_prev_time;
	double    an_prev_cdi;

	/*
	 * Pitch and roll rate control.
	 */
	_BOOL     rate_control_enabled;

	/*
		Warning flags:
	*/
	_BOOL ap_warn;
	_BOOL an_warn;
	_BOOL al_warn;
	_BOOL aw_warn;
	_BOOL at_warn;
	_BOOL ac_warn;

} aps_Type;


static double qround(double value, double step)
/*
	Round the given positive value to the nearest quantized value.
	The quantum is 'step'.
	Warning: does not work properly with negative values.
*/
{
	/*
	if( value < 0.0 )
		return -step * floor(-value / step + 0.5);
	else
	*/
		return step * floor(value / step + 0.5);
}


static double
forceRange(double x, double min, double max)
{
	if( x >= min ){
		if( x <= max ){
			return x;
		} else {
			return max;
		}
	} else {
		return min;
	}
}


#define LO units_DEGtoRAD(30.0)
#define HI units_DEGtoRAD(60.0)


static double
get_pitch_rate(craft * c)
{
	double roll_abs, pitch_rate, k;

	roll_abs = fabs(c->curRoll);
	if( roll_abs > HI )
		return c->q;
	pitch_rate = c->q*cos(c->curRoll) - c->r*sin(c->curRoll);
	if( roll_abs > LO ){
		k = (roll_abs - LO)/(HI - LO);
		return (1.0-k)*pitch_rate + k*c->q;
	}
	return pitch_rate;
	
	/*
	if( fabs(c->curRoll) < units_DEGtoRAD(30.0) )
		return c->q*cos(c->curRoll) - c->r*sin(c->curRoll);
	else
		return c->q;
	*/
}


static void aps_destruct(void *p)
{
	aps_Type * aps = p;
	if ( aps == NULL )
		return;
	aps->c->aps = NULL;
}


/**
 * Retrieves the APS data from aircraft. If not already set, allocates a new
 * APS.
 */
static aps_Type * aps_get(craft * c)
{
	aps_Type * aps;

	if ( c->aps != NULL )
		return (aps_Type *) c->aps;

	aps = memory_allocate( sizeof( aps_Type ), aps_destruct );
	memset(aps, 0, sizeof(*aps));
	c->aps = aps;
	aps->c = c;

	/* Rests pitch rate control: */
	aps->pitch_rate_enabled = FALSE;
	aps->pitch_rate_transfer = 0.0;
	aps->delta_Se = 0.0;

	/* Reset bank rate control: */
	aps->bank_rate_enabled = FALSE;
	aps->bank_rate_transfer = 0.0;
	aps->delta_Sa = 0.0;

	/* AP disabled: */
	aps->ap_enabled = FALSE;

	/* AC disabled: */
	aps->ac_enabled = FALSE;
	aps->ac_delta_Sr = 0.0;

	/* AT disabled: */
	aps->at_enabled = FALSE;

	/* AW disabled: */
	aps->aw_enabled = FALSE;
	aps->aw_bank_max = units_DEGtoRAD(25);

	/* AL disabled: */
	aps->al_enabled = FALSE;

	/* AN disabled: */
	aps->an_enabled = FALSE;

	/* Rate control disabled: */
	aps->rate_control_enabled = FALSE;

	/*
		Reset warning flags:
	*/
	aps->ap_warn = FALSE;
	aps->an_warn = FALSE;
	aps->al_warn = FALSE;
	aps->aw_warn = FALSE;
	aps->ac_warn = FALSE;

	return aps;
}


/**
 * Enables pitch control and set target pitch rate.
 * @param c The aircraft.
 * @param target Pitch rate to set (RAD/s).
 */
static void
pitch_rate_set(craft * c, double target)
{
	aps_Type * aps;

	aps = aps_get(c);

	if( ! aps->pitch_rate_enabled ){
		/* Smooth engaging: */
		aps->pc_prev_time = curTime;
		aps->pc_prev_pitch_dot = get_pitch_rate(c);
	}

	aps->pitch_rate_enabled = TRUE;
	aps->pitch_rate_target = target;
}


/**
 * Releases the pitch rate controller.
 * @param c The aircraft.
 */
static void
pitch_rate_disable(craft * c)
{
	aps_Type * aps;

	if( c->aps == NULL )
		return;
	
	aps = (aps_Type *) c->aps;

	aps->pitch_rate_enabled = FALSE;
}


/* Min/max elevator correction: */
#define PC_DELTA_SE_MAX 1.0

/* Warning if elevator correction outside this range: */
#define PC_DELTA_SE_OUT_RANGE (0.5 * PC_DELTA_SE_MAX)

#define PITCH_DOT_MAX units_DEGtoRAD(20.0)


/**
 * If pitch rate control is enabled, and the nose or tail wheel is not in
 * contact with the ground, updates the elevator correction to attain the
 * pitch rate set. Otherwise, smoothly reset the elevator correction.
 * Transition from direct law to rate control and vice-versa is smoothly
 * performed within about 3 seconds.
 * @param c The aircraft.
 */
static void
pitch_rate_update(craft * c)
{
	aps_Type * aps;
	double dt, pitch_dot_exp, pitch_dot;

	if( c->aps == NULL )
		return;
	
	aps = (aps_Type *) c->aps;

	dt = curTime - aps->pc_prev_time;

	if( dt <= 0.0 )
		return;
	
	aps->pc_prev_time = curTime;

	if( aps->pitch_rate_enabled /* && ! gear_noseWheelGroundContact(c) */ ){

		aps->pitch_rate_transfer += 0.33 * deltaT;
		if ( aps->pitch_rate_transfer > 1.0 )
			aps->pitch_rate_transfer = 1.0;

		pitch_dot = get_pitch_rate(c);

		/*
		 * Basically, the target pitch rate is that set, but here we account
		 * for the transfer of control while the nose/tail wheel loose contact
		 * with the ground:
		 */
		pitch_dot_exp = aps->pitch_rate_transfer * aps->pitch_rate_target
			+ (1.0 - aps->pitch_rate_transfer) * pitch_dot;
		pitch_dot_exp = forceRange(pitch_dot_exp, -PITCH_DOT_MAX, PITCH_DOT_MAX);

		/* Authority: */
		double k = c->cinfo->I.m[1][1]
			/ (-c->cinfo->cmSlope * (1.0 + c->IAS * c->IAS) * c->cinfo->wingS);
		double Gp = 2000.0 * k;
		double Gi = 5000.0 * k;
		aps->delta_Se = aps->delta_Se
			+ Gp * (pitch_dot - aps->pc_prev_pitch_dot)
			+ Gi * (pitch_dot - pitch_dot_exp) * dt;

		if ( aps->delta_Se < -PC_DELTA_SE_OUT_RANGE
		|| aps->delta_Se > +PC_DELTA_SE_OUT_RANGE ){
			aps->ap_warn = TRUE;
		}

		aps->delta_Se = forceRange(aps->delta_Se,
			-PC_DELTA_SE_MAX, +PC_DELTA_SE_MAX);

		aps->pc_prev_pitch_dot = pitch_dot;

	} else {
		/* Smooth release: */
		aps->pitch_rate_transfer -= 0.33 * deltaT;
		if ( aps->pitch_rate_transfer < 0.0 )
			aps->pitch_rate_transfer = 0.0;

		if( aps->delta_Se != 0.0 ){
			aps->delta_Se *= 1.0 - 0.3*dt;
			if( fabs(aps->delta_Se) < 1e-3 )
				aps->delta_Se = 0.0;
		}
	}
}


double
aps_get_delta_elevator(craft * c)
{
	aps_Type * aps;

	if ( c->aps == NULL )
		return 0.0;

	aps = (aps_Type *) c->aps;
	return aps->pitch_rate_transfer * (aps->delta_Se - c->pitchComm);
}


static double
get_roll_rate(craft * c)
{
	double bank_abs, pitch_abs, bank_rate, tanCurPitch, k;

	bank_abs = fabs(c->curRoll);
	pitch_abs = fabs(c->curPitch);
	if( bank_abs > HI || pitch_abs > HI )
		return c->p;
	tanCurPitch = tan(c->curPitch);
	bank_rate = c->p + c->q*tanCurPitch*sin(c->curRoll) + c->r*tanCurPitch*cos(c->curRoll);
	if( bank_abs > LO || pitch_abs > LO  ){
		k = (bank_abs - LO)/(HI - LO);
		return (1.0-k)*bank_rate + k*c->p;
	}
	return bank_rate;
}


/**
 * Enables bank rate controller and set bank rate.
 * @param c The aircraft.
 * @param target Target bank rate (RAD/s).
 */
static void
bank_rate_set(craft * c, double target)
{
	aps_Type * aps;
	double bank_rate;

	aps = aps_get(c);
	if ( ! aps->bank_rate_enabled ){
		aps->bank_rate_enabled = TRUE;
		/* Smooth engaging: */
		bank_rate = get_roll_rate(c);
		aps->bank_rate_prev_a_dot = bank_rate;
		aps->bank_rate_prev_a_dot_exp = bank_rate;
	}
	aps->bank_rate_target = target;
}


static void
bank_rate_disable(craft * c)
{
	aps_Type * aps;

	aps = aps_get(c);
	aps->bank_rate_enabled = FALSE;
}


/* Max roll rate (RAD/s). */
#define BANK_DOT_MAX units_DEGtoRAD(60.0)

/* Max roll rate accel. (RAS/s^2). */
#define BANK_DOT_DOT_MAX units_DEGtoRAD(600.0)

/* Max aileron correction control range. */
#define BANK_RATE_SA_MAX 1.0


/**
 * If the bank rate controller is enabled, and both the main wheels are not
 * in contact with the ground, updates the ailerons correction to attain the
 * bank rate set. Otherwise, smoothly resets the ailerons correction.
 * @param c The aircraft.
 */
static void
bank_rate_update(craft * c)
{
	aps_Type * aps;
	double a_dot, a_dot_exp, a_dot_dot_exp, delta_Sa;

	aps = aps_get(c);

	aps->bank_rate_prev_time = curTime;

	if ( aps->bank_rate_enabled && ! gear_mainWheelsGroundContact(c) ){
		aps->bank_rate_transfer += 0.3*deltaT;
		if ( aps->bank_rate_transfer > 1.0 )
			aps->bank_rate_transfer = 1.0;

		a_dot = get_roll_rate(c);

		/*
		 * Basically, the target bank rate is that set, but here we account
		 * for the transfer of control while the main landing wheels loose
		 * contact with the ground:
		 */
		a_dot_exp = aps->bank_rate_transfer * aps->bank_rate_target
			+ (1.0 - aps->bank_rate_transfer) * a_dot;

		/*
			Compute roll angle accel., apply limit then recalculate:
		*/
		a_dot_dot_exp = (a_dot_exp - a_dot) / deltaT;
		a_dot_dot_exp = forceRange(a_dot_dot_exp, -BANK_DOT_DOT_MAX, BANK_DOT_DOT_MAX);
		a_dot_exp = a_dot + a_dot_dot_exp * deltaT;
		a_dot_exp = forceRange( a_dot_exp, -BANK_DOT_MAX, BANK_DOT_MAX);

		/* authority */
		double k = (0.003 * (1.0 + c->IAS * c->IAS)) * c->cinfo->wingS
			* c->cinfo->wings * c->cinfo->Clda * c->cinfo->maxAileron
			/ c->cinfo->I.m[0][0];

		delta_Sa = aps->delta_Sa
			+ 10.0 / k * (a_dot - a_dot_exp - aps->bank_rate_prev_a_dot + aps->bank_rate_prev_a_dot_exp)
			+ 30.0 / k * (a_dot - a_dot_exp) * deltaT;

		aps->delta_Sa = forceRange( delta_Sa, -BANK_RATE_SA_MAX, BANK_RATE_SA_MAX);

		aps->aw_warn = fabs(aps->delta_Sa) == BANK_RATE_SA_MAX;

		aps->bank_rate_prev_a_dot = a_dot;
		aps->bank_rate_prev_a_dot_exp = a_dot_exp;

	} else {
		aps->bank_rate_transfer -= deltaT;
		if ( aps->bank_rate_transfer < 0.0 )
			aps->bank_rate_transfer = 0.0;

		if( aps->delta_Sa != 0.0 ){

			/* Smooth release (-66% within 1.0 s): */
			aps->delta_Sa -= aps->delta_Sa * deltaT;
			if( fabs(aps->delta_Sa) < 1e-3 )
				aps->delta_Sa = 0.0;
		}
	}
}


double
aps_get_delta_ailerons(craft * c)
{
	aps_Type * aps;

	if ( c->aps == NULL )
		return 0.0;

	aps = (aps_Type *) c->aps;
	return aps->bank_rate_transfer * (aps->delta_Sa - c->rollComm);
}


/*
	AutoPilot (AP)
	==============
*/


_BOOL
aps_ap_enabled(craft * c)
{
	return (c->aps != NULL) && ( ((aps_Type *) (c->aps))->ap_enabled );
}


void
aps_ap_enable(craft * c)
{
	aps_Type * aps;

	aps = aps_get(c);

	if ( aps->ap_enabled )
		return;

	aps_al_disable(c);
	aps_rate_control_disable(c);

	aps->ap_enabled = TRUE;
	aps->ap_hold_altitude = (fabs(c->Cg.z) < 100.0/60.0);
	aps->ap_target_altitude = qround( c->w.z, units_FEETtoMETERS(100.0) );
	aps->ap_target_vs = units_FEETtoMETERS( - c->Cg.z );
	aps->ap_prev_time = 0.0;
	aps->ap_prev_vs = aps->ap_target_vs;
}


void aps_ap_disable(craft * c)
{
	aps_Type * aps;

	if( c->aps == NULL )
		return;

	aps = aps_get(c);

	aps->ap_enabled = FALSE;
	aps->ap_warn = FALSE;

	pitch_rate_disable(c);
}


void
aps_ap_toggle(craft * c)
{
	if( aps_ap_enabled(c) )
		aps_ap_disable(c);
	else
		aps_ap_enable(c);
}


void
aps_ap_set_vs(craft * c, double vs)
{
	aps_Type * aps;

	aps_ap_enable(c);
	aps = aps_get(c);
	aps->ap_hold_altitude = (fabs(vs) < units_FEETtoMETERS(100.0)/60.0);
	if( aps->ap_hold_altitude )
		aps->ap_target_altitude = qround( c->w.z, units_FEETtoMETERS(100.0) );
	else
		aps->ap_target_vs = vs;
}


#ifdef FIXME_NOT_USED

static void
aps_ap_set_alt(craft * c, double altitude /* m */)
{
	aps_Type * aps;

	aps_ap_enable(c);
	aps = aps_get(c);
	aps->ap_hold_altitude = TRUE;
	aps->ap_target_altitude = altitude;
}

#endif


#define AP_DELTA_T 0.2

/* Min vertical speed in hold-altitude mode: */
#define AP_VS_MIN    units_FEETtoMETERS(-1000.0/60.0)  /* m/s */

/* Max vertical speed in hold-altitude mode: */
#define AP_VS_MAX    units_FEETtoMETERS(100.0/60.0)  /* m/s */

/* Min/max vertical accel.: */
#define AP_VS_DOT_MAX  (0.5 * 9.8)  /* m/s^2 */


static void
aps_ap_update(craft * c)
{
	aps_Type * aps;
	double vs, vs_exp, vs_dot_exp, dt, err, a_dot;

	aps = aps_get(c);

	if( aps->ap_prev_time == 0.0 ){
		/* First time we are called. */
		dt = AP_DELTA_T;
	} else {
		dt = curTime - aps->ap_prev_time;
	}

	if( dt < AP_DELTA_T )
		return;

	aps->ap_prev_time = curTime;

	aps->ap_warn = FALSE;

	if( ! aps->ap_enabled )
		return;

	/* Current vertical speed vs (m/s): */
	vs = -units_FEETtoMETERS(c->Cg.z);

	/* Current vertical acceleration vs_dot (m/s^2): */
	/* vs_dot = (vs - aps->ap_prev_vs) / dt; */

	/* Expected vertical speed vs_exp (m/s): */

	if ( aps->ap_hold_altitude ) {

		/* Current altitude error (m): */
		err = c->w.z - aps->ap_target_altitude;

		if( fabs(err) > units_FEETtoMETERS(150.0) )
			aps->ap_warn = TRUE;

		/*
			Compute a vertical speed vs_exp suitable to
			attain the target altitude (m/s):
		*/

		vs_exp = forceRange(-err / 15.0, AP_VS_MIN, AP_VS_MAX);

	} else {

		vs_exp = aps->ap_target_vs;

		if( fabs(vs - vs_exp) > units_FEETtoMETERS(200.0/60.0) )
			aps->ap_warn = TRUE;
	}

	/*
		Expected vertical acceleration vs_dot_exp (m/s^2).
		Set max accel. if the v.s. error is 500 fpm or greater.
	*/
	vs_dot_exp = forceRange((vs_exp - vs)/2.0,
		-AP_VS_DOT_MAX, AP_VS_DOT_MAX);
	
	/*
		The angle of the climb path is

		a = pitch - AOA

		where AOA is the angle of attack. At constant speed the AOA
		is constant, so deriving this latter gives:

			a_dot = pitch_dot = c->q

		The vertical speed is

			vs = v*tan(a)

		where v=c->VT is the TAS. For small angles tan(a)=a, then:

			vs = v*a

		Since v is constant, deriving this latter gives:

			vs_dot = v*a_dot

		So changes in vertical acceleration can be controlled
		through c->q:

			a_dot = vs_dot / v
	*/

	a_dot = vs_dot_exp / units_FEETtoMETERS(c->VT + 0.1);

	pitch_rate_set(c, a_dot);

	aps->ap_prev_vs = vs;
}


/*
	AutoCoordination (AC)
	=====================
*/

void
aps_ac_enable(craft * c)
{
	aps_Type * aps;

	aps = aps_get(c);

	if( aps->ac_enabled )
		return;

	aps->ac_enabled = TRUE;
	aps->ac_prev_time = 0.0;
	aps->ac_prev_a = c->G.y;
	/* aps->ac_delta_Sr = 0.0; */
	aps->ac_warn = FALSE;
}


void
aps_ac_disable(craft * c)
{
	aps_Type * aps;

	if( c->aps == NULL )
		return;

	aps = aps_get(c);

	aps->ac_enabled = FALSE;
	/* aps->ac_delta_Sr = 0.0; */
	aps->ac_warn = FALSE;
	c->Sr = 0.0;
}


_BOOL
aps_ac_enabled(craft * c)
{
	return (c->aps != NULL) && ( ((aps_Type *) (c->aps))->ac_enabled );
}


void
aps_ac_toggle(craft * c)
{
	if( aps_ac_enabled(c) )
		aps_ac_disable(c);
	else
		aps_ac_enable(c);
}


double
aps_ac_get_delta_rudder(craft * c)
{
	aps_Type * aps;

	if ( c->aps == NULL )
		return 0.0;

	aps = (aps_Type *) c->aps;
	return aps->ac_delta_Sr;
}


#define AC_DELTA_T 0.01
#define AC_SR_DOT_MAX 0.1

static void
aps_ac_update(craft * c)
{
	aps_Type * aps;
	double dt, a, a_dot, a_dot_exp, delta_Sr;

	aps = aps_get(c);

	if( aps->ac_prev_time == 0.0 ){
		/* First time we are called. */
		dt = AC_DELTA_T;
	} else {
		dt = curTime - aps->ac_prev_time;
	}

	if( dt < AC_DELTA_T )
		return;

	aps->ac_prev_time = curTime;

	aps->ac_warn = FALSE;

	if( aps->ac_enabled && ! gear_noseWheelGroundContact(c) ) {

		/*
			Normal AC handling. Update aps->ac_delta_Sr so that to
			reduce lateral acceleration c->G.y.
		*/

		a = c->G.y;
		a_dot = (a - aps->ac_prev_a) / dt;
		a_dot_exp = - 0.5 * a;
		double k = 2000.0 / (1.0 + c->IAS * c->IAS); /* authority */
		delta_Sr = aps->ac_delta_Sr + 1.0 * k * (a_dot - a_dot_exp);
		aps->ac_delta_Sr = forceRange(delta_Sr, -1.0, 1.0);

		aps->ac_warn = fabs(aps->ac_delta_Sr) == 1.0;

		aps->ac_prev_a = a;
	}

	else if ( aps->ac_delta_Sr != 0.0 ){

		/*
			Smooth release (-66% within 2 s):
		*/

		if( aps->ac_delta_Sr != 0.0 ){
			aps->ac_delta_Sr -= aps->ac_delta_Sr / 2.0 * dt;
			if( fabs(aps->ac_delta_Sr) < 1.e-3 )
				aps->ac_delta_Sr = 0.0;
		}
	}

}


/*
	AutoThrottle (AT)
	=================
*/

#define AT_V_STEP units_KTtoFPS(5.0)  /* ft/s */


void aps_at_enable(craft * c)
{
	aps_Type * aps;
	double v;

	aps = aps_get(c);

	if( aps->at_enabled )
		return;

	v = c->IAS;
	if( v < 0.0 )  v = 0.0;
	aps->at_target_v = qround(v, AT_V_STEP);
	aps->at_prev_v = v;
	aps->at_prev_time = 0.0;
	aps->at_enabled = TRUE;
	aps->at_warn = FALSE;
}


void aps_at_disable(craft * c)
{
	aps_Type * aps;

	if( c->aps == NULL )
		return;

	aps = aps_get(c);

	if( ! aps->at_enabled )
		return;

	aps->at_enabled = FALSE;
	aps->at_warn = FALSE;
}


_BOOL
aps_at_enabled(craft * c)
{
	return (c->aps != NULL) && ( ((aps_Type *) (c->aps))->at_enabled );
}


void
aps_at_toggle(craft * c)
{
	if( aps_at_enabled(c) )
		aps_at_disable(c);
	else
		aps_at_enable(c);
}


double
aps_at_get_velocity(craft * c)
{
	aps_Type * aps;

	aps = (aps_Type *) c->aps;

	if ( aps == NULL || ! aps->at_enabled )
		return 0.0;

	return aps->at_target_v;
}


#define AT_DELTA_T 0.5  /* s */
#define AT_V_DOT_MAX (+0.1*units_earth_g)
#define AT_V_DOT_MIN (-0.5*units_earth_g)
#define AT_REALIGN_TIME 15.0  /* time for vel. realignment (s) */


static void
aps_at_update(craft * c)
{
	aps_Type * aps;
	double dt, v, v_dot, exp_v_dot, thr;
	_BOOL thr_alarm;

	aps = aps_get(c);

	if ( ! aps->at_enabled )
		return;

	if( aps->at_prev_time == 0.0 ){
		/* First time we are called. */
		dt = AT_DELTA_T;
	} else {
		dt = curTime - aps->at_prev_time;
	}

	if ( dt < AT_DELTA_T )
		return;
	
	aps->at_warn = FALSE;

	v = c->IAS;  /* ft/s */
	v_dot = (v - aps->at_prev_v) / dt;
	exp_v_dot = forceRange( (aps->at_target_v - v) / AT_REALIGN_TIME,
		AT_V_DOT_MIN, AT_V_DOT_MAX);
	thr = (1.0 + 0.2 * (exp_v_dot - v_dot)) * c->rpm;
	thr_alarm = FALSE;
	if ( thr <= 0.2 ) {
		thr = 0.2;
		if ( v_dot >= 0.0 )
			thr_alarm = TRUE;
	}
	else if ( thr >= 1.0 ) {
		thr = 1.0;
		if ( v_dot <= 0.0 )
			thr_alarm = TRUE;
	}
	aps->at_warn = thr_alarm;
	c->throttleComm = (int) (32768.0 * thr + 0.5);
	aps->at_prev_v = v;
	aps->at_prev_time = curTime;

#ifdef DEBUG
	printf("AT: v_target=%3.0f kt  v=%3.0f kt  v_dot=%g g  exp_v_dot=%g g\n",
		units_FPStoKT(aps->at_target_v), units_FPStoKT(v), v_dot/units_earth_g, exp_v_dot/units_earth_g);
#endif

}


void
aps_at_inc_velocity(craft * c)
{
	aps_Type * aps;

	aps = aps_get(c);
	aps->at_target_v = qround(aps->at_target_v + AT_V_STEP, AT_V_STEP);
}


void
aps_at_dec_velocity(craft * c)
{
	aps_Type * aps;

	aps = aps_get(c);
	aps->at_target_v = qround( fabs(aps->at_target_v - AT_V_STEP), AT_V_STEP);
}


/*
	AutoTurn (AW)
	=============
	Controls z turn rate or x roll rate, depending on which value gets
	set.
*/

#define AW_DELTA_T 0.02

void aps_bank_max_inc(craft * c)
{
	aps_Type * aps;
	int b;

	if( c->aps == NULL )
		return;

	aps = aps_get(c);

	b = (int) (units_RADtoDEG(aps->aw_bank_max) + 0.5);

	if( b >= 25 )
		return;

	aps->aw_bank_max = units_DEGtoRAD(b + 5);
}


void aps_bank_max_dec(craft * c)
{
	aps_Type * aps;
	int b;

	aps = aps_get(c);

	b = (int) (units_RADtoDEG(aps->aw_bank_max) + 0.5);

	if( b <= 5 )
		return;

	aps->aw_bank_max = units_DEGtoRAD(b - 5);
}


int aps_bank_max_get(craft * c)
{
	aps_Type * aps;

	if( c->aps == NULL )
		return 25;

	aps = aps_get(c);

	return (int) (units_RADtoDEG(aps->aw_bank_max) + 0.5);
}


void
aps_aw_enable(craft * c)
{
	aps_Type * aps;

	aps = aps_get(c);

	if ( aps->aw_enabled )
		return;

	aps_rate_control_disable(c);

	aps->aw_enabled = TRUE;
	/*
		aps->aw_bank_max = units_DEGtoRAD(25.0);

		This field must be initialized by aps_get once for all.
	*/
	aps->aw_w_target = c->r / cos(c->curRoll);
	aps->aw_prev_time = 0.0;
	aps->aw_warn = FALSE;
	aps_ac_enable(c);
}


_BOOL
aps_aw_enabled(craft * c)
{
	return (c->aps != NULL) && ( ((aps_Type *) (c->aps))->aw_enabled );
}


void
aps_aw_disable(craft * c)
{
	aps_Type * aps;

	if ( c->aps == NULL )
		return;

	aps = (aps_Type *) c->aps;

	if ( ! aps->aw_enabled )
		return;

	aps->aw_enabled = FALSE;
	bank_rate_disable(c);
}


void
aps_aw_set(craft * c, double w)
/*
	Sets the turn rate w (RAD/s) around the local earth vertical.
	w > 0 ==> turn left
	w < 0 ==> turn right
*/
{
	aps_Type * aps;

	aps_aw_enable(c);
	aps = (aps_Type *) c->aps;
	aps->aw_w_target = w;
}


/****** not used
static void
aps_aw_bank_max_set(craft * c, double bank)
{
	aps_data * aps;

	aps = (aps_data *) c->aps;
	if( aps == NULL )
		return;
	aps->aw_bank_max = bank;
}
*******/


/*
 * FIXME: what to return if the roll rate is enabled rather than turn rate?
 */
double
aps_aw_get(craft * c)
{
	aps_Type * aps;

	if ( c->aps == NULL )
		return 0.0;

	aps = aps_get(c);

	if ( aps->aw_enabled )
		return aps->aw_w_target;
	else
		return 0.0;
}


static void
aps_aw_update(craft * c)
{
	aps_Type * aps;
	double dt, a, a_exp, a_dot_exp;

	aps = aps_get(c);
	if ( ! aps->aw_enabled )
		return;

	if( aps->aw_prev_time == 0.0 ){
		/* First time we are called. */
		dt = AW_DELTA_T;
	} else {
		dt = curTime - aps->aw_prev_time;
	}

	if ( dt < AW_DELTA_T )
		return;

	aps->aw_prev_time = curTime;

	a = c->curRoll;

	/*
		Compute angle of bank to attain the required turn speed:
	*/
	a_exp = atan( c->VT * aps->aw_w_target / units_earth_g );

	a_exp = forceRange( a_exp, -aps->aw_bank_max, aps->aw_bank_max);

	/*
		Compute the bank rate to attain the expected angle of bank:
	*/
	a_dot_exp = (a_exp - a) / 2.0;
	a_dot_exp = forceRange(a_dot_exp, -units_DEGtoRAD(10.0), units_DEGtoRAD(10.0));

	bank_rate_set(c, a_dot_exp);
}




/*
	Auto Navigation (AN)
	====================
*/

void
aps_an_enable(craft * c)
{
	aps_Type * aps;

	aps = aps_get(c);
	aps_al_disable(c);
	aps_rate_control_disable(c);
		
	aps->an_enabled = TRUE;
	aps->an_prev_time = 0.0;
	aps->an_prev_cdi = 0.0;
	aps->an_warn = FALSE;
}


void
aps_an_disable(craft * c)
{
	aps_Type * aps;

	if( c->aps == NULL )
		return;
	
	aps = aps_get(c);
	if( ! aps->an_enabled )
		return;
	
	bank_rate_disable(c);
	aps->an_enabled = FALSE;
	aps->an_warn = FALSE;
}


_BOOL
aps_an_enabled(craft * c)
{
	return (c->aps != NULL) && ( ((aps_Type *) (c->aps))->an_enabled );
}


void
aps_an_toggle(craft * c)
{
	if( aps_an_enabled(c) )
		aps_an_disable(c);
	else
		aps_an_enable(c);
}


static double
min_angle_diff(double a, double b)
{
	double d;

	d = a - b;
	if( d > M_PI )
		d = - 2.0*M_PI + d;
	else if( d < - M_PI )
		d = 2.0*M_PI + d;
	return d;
}


static double
normalize_angle(double x)
{
	while( x < 0 )
		x = x + 2.0*M_PI;
	
	while( x > 2.0*M_PI )
		x = x - 2.0*M_PI;
	
	return x;
}


static void
aps_an_update(craft * c)
{
	aps_Type * aps;
	viewer *u;
	double dt, r, obs, magvar, cdi, th_diff, th, w,
		cdi_dot, cdi_expected, k;

	if( c->aps == NULL )
		return;
	
	aps = aps_get(c);

	if( ! aps->an_enabled )
		return;

	if( aps->an_prev_time == 0.0 ){
		/* First time we are called. */
		dt = 1.7;
	} else {
		dt = curTime - aps->an_prev_time;
	}

	if ( dt < 1.7 )
		return;
	
	aps->an_warn = FALSE;

	/*
		Get current radial:
	*/
	u = c->vl;
	if( u == NULL || ! hsi_vor_radial(u, &r) ){
		aps_an_disable(c);
		return;
	}

	/* Target radial: */
	obs = units_DEGtoRAD( hsi_get_obs(u) );

	/* Magnetic variation: */
	if( c->showMag )
		magvar = c->indicatedLocalVAR;
	else
		//magvar = c->hsiSelect->station->magvar; FIXME
		magvar = 0.0;

	/* Course deviation: */
	cdi = min_angle_diff(r, obs);
	if( cdi > units_DEGtoRAD(90.0) )
		cdi = units_DEGtoRAD(180.0) - cdi;
	else if( cdi < - units_DEGtoRAD(90.0) )
		cdi = - units_DEGtoRAD(180.0) - cdi;
	
	/*
	 * Set turn speed only if cdi_dot available.
	 */
	if( aps->an_prev_time > 0.0 ){
		/*
			Evaluates the velocity of CDI, then guess what the CDI will be
			within, say, 10 seconds so to compensate the inertia of the plane
			allowing also a smooth change of bank angle (it requires just about
			10 seconds to pass from -25 DEG to +25 DEG).
		*/
		cdi_dot = forceRange( (cdi - aps->an_prev_cdi)/dt,
			units_DEGtoRAD(-5.0), units_DEGtoRAD(5.0));
		cdi_expected = forceRange(cdi + cdi_dot * 10.0,
			units_DEGtoRAD(-10.0), units_DEGtoRAD(10.0));

		/*
			Compute approaching heading to OBS.	The heading error CDI (i.e. the
			diff. between the actual heading and the expected obs heading)
			gets amplified by a factor k.  This factor normally evaluates to
			20, but if the cdi moves too fast (i.e. we are close to the VOR)
			it gets reduced up to 1.2 for smooth movements.	 The max approching
			angle gets limited to +/-45 DEG.
		*/
		k = 1.2 + 18.8 / (1.0 + 500.0 * fabs(cdi_dot));
		th = normalize_angle(obs - magvar
			- forceRange(k * cdi_expected, units_DEGtoRAD(-45.0), units_DEGtoRAD(45.0)));

		/* Heading correction: */
		th_diff = min_angle_diff(th, c->curHeading);

		w = forceRange( 0.05 * th_diff,
			units_DEGtoRAD(-3.0), units_DEGtoRAD(3.0));

		aps_aw_set(c, w);
		/****
		bank_max = units_DEGtoRAD(5.0) + (25.0 - 5.0)/30.0 * fabs(th_diff);
		if( bank_max > units_DEGtoRAD(25.0) )
			bank_max = units_DEGtoRAD(25.0);
		aps_aw_bank_max_set(c, bank_max);
		****/
	}

	/* Update status: */
	aps->an_prev_time = curTime;
	aps->an_prev_cdi = cdi;

#ifdef DEBUG
	printf("r=%.0f"
		"  obs=%.0f"
		"  curHeading=%.0f"
		"  w=%.1f"
		"\n",
		units_RADtoDEG(r),
		units_RADtoDEG(obs),
		units_RADtoDEG(c->curHeading),
		units_RADtoDEG(w));
#endif

}


/*
	AutoLanding (AL)
	================
*/

void
aps_al_enable(craft * c)
{
	aps_Type * aps;

	aps = aps_get(c);
	if ( aps->al_enabled )
		return;

	aps_an_disable(c);
	aps_aw_disable(c);
	aps_rate_control_disable(c);
	aps_ap_enable(c);
	aps_ac_enable(c);

	aps->al_enabled = TRUE;
	aps->al_prev_adiff = 0.0;
	aps->al_prev_time = 0.0;
	aps->al_v_touchdown = 0.0;
	aps->al_touchdown = FALSE;
	aps->al_warn = FALSE;
}


void
aps_al_disable(craft * c)
{
	aps_Type * aps;

	if( c->aps == NULL )
		return;

	aps = aps_get(c);
	if ( ! aps->al_enabled )
		return;

	aps->al_enabled = FALSE;
	aps->al_warn = FALSE;
	aps_aw_disable(c);
}


_BOOL
aps_al_enabled(craft * c)
{
	return (c->aps != NULL) && ( ((aps_Type *) (c->aps))->al_enabled );
}


void
aps_al_toggle(craft * c)
{
	if( aps_al_enabled(c) )
		aps_al_disable(c);
	else
		aps_al_enable(c);
}


#define AL_ALIGN_TIME 30.0
/* Expected time for LOC alignment (s). Decrease for faster realignment,
   but beware of the Izz inertia and the rudder effectiveness because
   values too small may introduce oscillations.
*/


static void
aps_al_update(craft * c)
{
	aps_Type * aps;
	viewer *u;
	double dt, h, radial, adiff, adiff_dot, exp_adiff_dot, w, gs_offset,
		v, vs, flare_thr;
	int is_tricycle;

	aps = aps_get(c);

	if( ! aps->al_enabled )
		return;

	if( aps->al_prev_time == 0.0 ){
		/* First time we are called. */
		dt = 0.5;
	} else {
		dt = curTime - aps->al_prev_time;
	}

	if ( dt < 0.5 )
		return;

	aps->al_prev_time = curTime;

	aps->al_warn = FALSE;

	h = units_METERStoFEET(c->w.z - terrain_localAltitude(c))
		- c->cinfo->rm.z - c->cinfo->Gm - c->cinfo->cmMax;

	u = c->vl;

	if( h > 250.0 /* ft */ ){
		/*
			Get radial:
		*/
		if( u == NULL || ! hsi_loc_radial(u, &radial) ){
			aps_al_disable(c);
			return;
		}

		if (radial > M_PI / 2.0) {
			adiff = radial - 2.0*M_PI;
		}
		else {
			adiff = radial;
		}

		adiff_dot = (adiff - aps->al_prev_adiff) / dt;
		//adiff += adiff_dot * dt; /* guess adiff at next step */

		exp_adiff_dot = forceRange( -0.69 /*log(2)*/ * adiff / AL_ALIGN_TIME,
			units_DEGtoRAD(-0.1), units_DEGtoRAD(0.1));

		w = 10.0 * (adiff_dot - exp_adiff_dot);
		if( fabs(adiff) > units_DEGtoRAD(0.2) )
			w *= 2.0;
		w = forceRange(w, units_DEGtoRAD(-3.0), units_DEGtoRAD(3.0));

		aps_aw_set(c, w);

		aps->al_prev_adiff = adiff;

#ifdef DEBUG
		printf("adiff=%.1f"
			"  adiff_dot=%.3f"
			"  exp_adiff_dot=%.3f"
			"  w=%.1f  "
			"\n",
			units_RADtoDEG(adiff),
			units_RADtoDEG(w),
			units_RADtoDEG(adiff_dot),
			units_RADtoDEG(exp_adiff_dot));
#endif
	}

	v = units_FEETtoMETERS( c->IAS ); /* m/s */

	/*
		The flare maneuver starts when the radar altimeter indicates
		flare_thr feet from the ground. This value is calculated
		considering H_THR be a good value for a craft landing at V_THR
		speed; other values are interpolated.
	*/

	#define V_THR  160.0 /* kt */
	#define H_THR  50.0 /* ft */

	flare_thr = H_THR / V_THR * units_METERStoNM(v)*3600.0;
	
	/* Bouncing fix: */
	if( !aps->al_touchdown && gear_mainWheelsGroundContact(c) )
		aps->al_touchdown = TRUE;

	if( h > 250.0 && hsi_gs_offset(u, &gs_offset) ){
		/* Follow GS, if available */

		double vs_target;

		gs_offset = units_RADtoDEG( gs_offset );

		if( gs_offset < -0.2 ){
			vs = 0.0;
		} else {
			vs_target = -0.0524 /* tan(3 DEG) */ * v;
			vs = forceRange( (1.0 + 0.7*gs_offset) * vs_target,
				1.7*vs_target, 0.0);
		}

		aps_ap_set_vs(c, vs);

		/* Ensure brakes be off: */
		gear_brakesDisengage(c);

	} else if( h < flare_thr ){
		/*
			Flare maneuver.

			FIXME: for accuracy (and more realisitically)
			determinate v.s. based on radar altimeter rather than
			VSI.
		*/

		/* Constant -1 DEG: */
		vs = -units_DEGtoRAD(1.0)*v;

		/* Linear curve: glide path angle = tan(1+2*h/flare_thr): */
		/* vs = -units_DEGtoRAD(1.0 + 2.0*h/flare_thr) * v; */

		/* Quadratic curve: */
		/* vs = -units_DEGtoRAD(1.0 + h*h/(flare_thr*flare_thr)*2.0) * v; */

		if( h < 0.5*flare_thr ){

			/* Keep current engine setting: */
			aps_at_disable(c);

			if( aps->al_touchdown ){
				/* Main wheels in contact. */

				/* If this is the first contact, set v_touchdown: */
				if( aps->al_v_touchdown == 0.0 )
					aps->al_v_touchdown = v;

				/*
					Engine control:

					If thrust reverser available:
					- set engine idle
					- deploy thrust rev.
					- set engine 75% power
					- when speed < 0.4 v_touchdown, set engine idle
					- when speed < 0.2 v_touchdown, retract thr. rev.

					If thrust rev. not available:
					- set engine idle
				*/

				if( c->cinfo->hasThrustReverser ){
					if( v > 0.4 * aps->al_v_touchdown ){
						if( c->thrust_reverse_on ){
							/* 75% power with thrust rev. */
							c->throttleComm = (int) (75.0/100.0 * 32768);
						} else {
							/* Engine idle: */
							c->throttleComm = (int) (20.0/100.0 * 32768);
							pm_thrust_reverse_toggle(c);
						}
					} else {
						/* Engine idle: */
						c->throttleComm = (int) (20.0/100.0 * 32768);
					}
				} else {
					/* Engine idle: */
					c->throttleComm = (int) (20.0/100.0 * 32768);
				}

				/* Extend speed brakes: */
				flaps_speed_brakes_extend(c);
				/* note that the function above will be called several
				   times looping here, so ensuring the speed brakes
				   be fully extended */

				/*
					Pitch control.
					Once the main wheels are on ground, we
					have to deploy gently the nose/tail
					wheel. To this aim we control the pitch
					rate instead of the v.s.
				*/

				is_tricycle = (c->cinfo->rn.x > c->cinfo->rm.x);

				if( aps_ap_enabled(c) )
					aps_ap_disable(c);

				if( ! is_tricycle && v > 0.85 * aps->al_v_touchdown ){
					pitch_rate_set(c, 0.0);
				} else {
					if( is_tricycle ){
						/* Gently lower the nose: */
						pitch_rate_set(c, -units_DEGtoRAD(2.0));
					} else {
						/* Gently raise nose, avoiding to take-off again! */
						pitch_rate_set(c, units_DEGtoRAD(0.5));
					}
				}

				if( gear_noseWheelGroundContact(c) ){
					/* All the wheels in contact. */
					
					/* Keep current pitch so elevator transfers torque from nose to main: */
					pitch_rate_set(c, 0);

					/*
						Brakes are enabled only on tricycle landing gears
						when the nose is in contact and, if thrust rev.
						available, when the speed is half the touchdown
						speed:
					*/

					if( is_tricycle
					&& ( !c->cinfo->hasThrustReverser
						|| v < 0.5 * aps->al_v_touchdown )
					)
						/* Brakes on: */
						gear_brakesEngage(c);

					if( v < 0.2 * aps->al_v_touchdown ){
						aps_al_disable(c);
						aps_ap_disable(c);
						aps_aw_disable(c);
						pitch_rate_disable(c);
						if( c->thrust_reverse_on )
							pm_thrust_reverse_toggle(c);
					}
				}
			} else {
				aps_ap_set_vs(c, vs);
			}

		} else {
			aps_ap_set_vs(c, vs);
		}


	} else if( h <= 250.0 ){
		/* ILS signal unreliable -- follow standard slope 3 DEG: */
		aps_ap_set_vs(c, -0.0524 /* tan(3 DEG) */ * v );
	} else {
		aps_ap_set_vs(c, 0.0 );
	}
}


/**
 * Enables pitch and roll rate control.
 */
void aps_rate_control_enable(craft * c)
{
	aps_Type * aps;

	aps = aps_get(c);
	if ( aps->rate_control_enabled )
		return;
	aps_an_disable(c);
	aps_al_disable(c);
	aps_aw_disable(c);
	aps_ap_disable(c);
	aps_ac_enable(c);
	aps->rate_control_enabled = TRUE;
}


void aps_rate_control_disable(craft * c)
{
	aps_Type * aps;

	aps = aps_get(c);
	if ( ! aps->rate_control_enabled )
		return;
	aps->rate_control_enabled = FALSE;
	pitch_rate_disable(c);
	bank_rate_disable(c);
}


void aps_rate_control_toggle(craft * c)
{
	aps_Type * aps;

	aps = aps_get(c);
	if ( aps->rate_control_enabled )
		aps_rate_control_disable(c);
	else
		aps_rate_control_enable(c);
}


_BOOL aps_rate_control_enabled(craft * c)
{
	return (c->aps != NULL) && ( ((aps_Type *) (c->aps))->rate_control_enabled );
}


static void aps_rate_control_update(craft * c)
{
	aps_Type * aps;
	double pitch_dot, bank_dot;

	aps = aps_get(c);
	if ( ! aps->rate_control_enabled )
		return;
	pitch_dot = - c->pitchComm * PITCH_DOT_MAX;
	pitch_rate_set(c, pitch_dot);
	bank_dot = - c->rollComm * BANK_DOT_MAX;
	bank_rate_set(c, bank_dot);
}


/*
	Warning detection
	=================
*/

_BOOL aps_ap_warn(craft *c)
{
	return (c->aps != NULL)
		&& ( ((aps_Type *) (c->aps))->ap_warn );
}


_BOOL aps_at_warn(craft *c)
{
	return (c->aps != NULL)
		&& ( ((aps_Type *) (c->aps))->at_warn );
}


_BOOL aps_al_warn(craft *c)
{
	return (c->aps != NULL)
		&& ( ((aps_Type *) (c->aps))->al_warn );
}


_BOOL aps_an_warn(craft *c)
{
	return (c->aps != NULL)
		&& ( ((aps_Type *) (c->aps))->an_warn );
}


_BOOL aps_aw_warn(craft *c)
{
	return (c->aps != NULL)
		&& ( ((aps_Type *) (c->aps))->aw_warn );
}


_BOOL aps_ac_warn(craft *c)
{
	return (c->aps != NULL)
		&& ( ((aps_Type *) (c->aps))->ac_warn );
}


void aps_update(craft * c)
{
	if ( c->aps == NULL )
		return;

	aps_at_update(c);
	aps_an_update(c);
	aps_al_update(c);
	aps_ap_update(c);
	aps_ac_update(c);
	aps_aw_update(c);
	aps_rate_control_update(c);
	bank_rate_update(c);
	pitch_rate_update(c);
}


void aps_off(craft * c)
{
	aps_at_disable(c);
	aps_an_disable(c);
	aps_al_disable(c);
	aps_ap_disable(c);
	aps_ac_disable(c);
	aps_rate_control_disable(c);
	aps_aw_disable(c);
	bank_rate_disable(c);
	pitch_rate_disable(c);
}

/* End of aps.c */
