/*
 *  acm : an aerial combat simulator for X
 *  Copyright (C) 1991-1998  Riley Rainey
 *
 *  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.
 *
 */

#include <stdio.h>
#include <math.h>

#include "../util/memory.h"

#define pm_IMPORT
#include "pm.h"

#include "aps.h"
#include "damage.h"
#include "dis_if.h"
#include "gear.h"
#include "planes.h"
#include "prompt.h"
#include "terrain.h"
#include "../util/units.h"
#include "../wmm/wmm.h"

#ifdef WINNT
#include <float.h>
#endif

static double    CM, CN, C_Y;


/*
 *  twoorder: solve linear second-order differential equation with initial
 *      conditions known.
 *
 *  y_dot_dot - d * y_dot - k * y - q = 0
 *
 *  given the initial conditions:
 *
 *        y(0) == y.
 *        y_dot(0) == v.
 *
 *  Results are *newy=y(deltaT) and *newv=y_dot(deltaT) for deltaT seconds
 *  into the future.
 *  
 */

static void
twoOrder(double k, double d, double q, double y, double v,
	double *newy, double *newv)
{

	double  qk, s, s1, s2, t, ac, c1, c2, exp_s1_x, exp_s2_x;

/*
	Prevent numerical instabilities: since y,v are pitch/yaw angle and its
	derivative, it would be perfectly useless to consider factors too
	small.
*/

	if( fabs(d) < 1e-9 )
		d = 0.0;

	if( fabs(k) < 1e-9 )
		k = 0.0;
	
	if( k == 0.0 ){
		if( d == 0.0 ){
			/* The equation reduces to: y_dot_dot = q. */
			*newy = 0.5 * q * deltaT * deltaT + v * deltaT + y;
			*newv = q * deltaT + v;
		} else {
			/* The equation reduces to: y_dot_dot - d*y_dot - q = 0. */
			*newy = (v + q/d)/d*exp(d*deltaT) - q/d*deltaT + y - (v + q/d)/d;
			*newv = (v + q/d)*exp(d*deltaT) - q/d;
		}
		*newy = y;
		*newv = v;
		return;
	}
	qk = q/k;

	ac = d * d + 4.0 * k;

	if ( ac > 0.0 ) {
		/*
			The general solution for ac > 0 is:
			y = c1 * exp(s1*x) + c2 * exp(s2*x) - q/k
			where:
			s1 = ...see code...
			s2 = ...see code...
			c1 = ...see code...
			c2 = ...see code...
		*/
		t = sqrt(ac);
		s1 = (d + t) * 0.5;
		s2 = (d - t) * 0.5;
		c1 = (-s2*y - s2*qk + v) / t;
		c2 = ( s1*y + s1*qk - v) / t;
		exp_s1_x = exp(s1 * deltaT);
		exp_s2_x = exp(s2 * deltaT);
		*newy = c1 * exp_s1_x + c2 * exp_s2_x - qk;
		*newv = c1 * s1 * exp_s1_x + c2 * s2 * exp_s2_x;
	}
	else if (ac < 0.0) {
		/*
			The general solution for ac < 0 is:
			y = exp(s*x) * (c1 * cos(t*x) + c2 * sin(t*x)) - qk
			where:
			s = d/2
			t = sqrt(-ac)/2
			c1 = y0 + qk
			c2 = (v-c1*s)/t
		*/
		double cos_t_x, sin_t_x;
		s = d * 0.5;
		t = sqrt(-ac) * 0.5;
		exp_s1_x = exp(s*deltaT);
		cos_t_x = cos(t*deltaT);
		sin_t_x = sin(t*deltaT);
		c1 = y + qk;
		c2 = (v - c1*s ) / t;
		*newy = exp_s1_x * ( c1*cos_t_x + c2*sin_t_x ) - qk;
		*newv = exp_s1_x * ( (c1*s+c2*t) * cos_t_x + (c2*s-c1*t) * sin_t_x );
	}
	else {
		/*
			The general solution for ac = 0 is:
			y = (c1 + c2*x) * exp(s*x) - qk
			where:
			s = d/2
			c1 = y0 + qk
			c2 = v - c1*s
		*/
		s = 0.5*d;
		c1 = y + qk;
		c2 = v - c1*s;
		exp_s1_x = exp(s*deltaT);
		*newy = (c1 + c2*deltaT) * exp_s1_x - qk;
		*newv = (c2 + (c1 + c2*deltaT)*s) * exp_s1_x;
	}
}


/*
 *  calcCoefficients :  Calculate CLift and friends
 */

static void
calcCoefficients(craft * c, double *CLift, double *CDrag)
{

	double    CDAlpha, CDBeta;
	craftType *p = c->cinfo;
	double    h, base, k;

/*
 *  We used to interpolate these values, but now use several characteristic
 *  equations to compute these values for a given alpha value. The basic
 *  formulas are:
 *
 *
 *   C  = C        + (alpha * (C       + sin(curFlap) * cFlap ))
 *    L    LOrigin              LSlope
 *
 *
 *   C  = zero-lift-wave-and-body-drag + induced-drag +
 *    D     speed-brake-drag + flap drag + landing-gear-drag +
 *      drag-based-on-sideslip
 *
 *  There are independent equations defining drag resulting from alpha
 *  and beta values.  The hypoteneuse of those two values becomes the
 *  resultant CDrag value.
 */

	*CLift = interpolate_value(p->CLift, c->alpha);

	if( damage_isFunctioning(c, SYS_FLAPS) )
		*CLift += sin(c->curFlap) * p->cFlap;

	/*
		Rough estimation of the ground effect correction to the CLift:
		FIXME: also the drag coeff. should be corrected
	*/

	/* Height of the wing aerodynamic center above the ground (ft): */
	h = units_METERStoFEET(c->w.z - terrain_localAltitude(c)) + p->wingh;
	/* Limit ground effect, or we would "float" over the terrain... */
	if( h < 1.0 )
		h = 1.0;
	base = h / p->wings;
	/* Compute the expensive pow() only if k will be > 1.001: */
	if ( base < 8.5 ) {
		/* Ground effect correction coefficient to the CLift: */
		k = 1.0 + 0.025 * pow(base, -1.5);
		*CLift *= k;
	}

	CM = p->cmSlope + c->damageCM;

	CDAlpha = interpolate_value(p->CDb, c->mach) +
		*CLift * *CLift / (M_PI * p->aspectRatio);
	if( damage_isFunctioning(c, SYS_SPEEDBRAKE ) )
		CDAlpha += sin(c->curSpeedBrake) * p->cSpeedBrake;
	if( damage_isFunctioning(c, SYS_FLAPS) )
		CDAlpha += sin(c->curFlap) * p->cFlapDrag;

	CDAlpha += gear_get_drag(c);

	if (fabs(c->beta) > p->betaStall)
		CN = interpolate_value(p->CnBeta, fabs(c->alpha)) * fabs(sin(c->beta));
	else
		CN = interpolate_value(p->CnBeta, fabs(c->alpha));

	CDBeta = p->CDBOrigin + p->CDBFactor *
		sin(c->beta + p->CDBPhase);

	*CDrag = sqrt(CDAlpha * CDAlpha + CDBeta * CDBeta);

	C_Y = p->CYbeta * c->beta /* * fabs(cos(c->beta))*/;

}


double
pm_heading(VPoint * x)
{

	return atan2(x->y, x->x);
}

void
pm_euler(craft * c)
{
	VMatrixToEuler(&c->AXYZtoNED, &c->curRoll, &c->curPitch, &c->curHeading);
}


void
pm_calcGForces(craft * c, VPoint * f, double w)
{

	VPoint  t, t1;
	double	m_slugs;

	m_slugs = w / units_earth_g;

	t = *f;
	t.x = t.x / m_slugs;
	t.y = t.y / m_slugs;
	t.z = t.z / m_slugs;

	VReverseTransform_(&t, &c->AXYZtoNED, &c->linAcc);

	t.z -= units_earth_g;

	VReverseTransform_ (&t, &c->AXYZtoNED, &t1);

	c->G.x = t1.x / units_earth_g;
	c->G.y = t1.y / units_earth_g;
	c->G.z = t1.z / units_earth_g;

}


static void
calcAlphaBetaVT(craft * c)
{
	VPoint    C, air, *wind;
	double    delta;

	wind = air_get_wind(c->w.z);

	air.x = c->Cg.x + wind->x;
	air.y = c->Cg.y + wind->y;
	air.z = c->Cg.z + wind->z;

	VReverseTransform_(&air, &c->AXYZtoNED, &C);

	c->alpha = atan2(C.z, sqrt(C.y * C.y + C.x * C.x));
	c->beta = atan2(C.y, C.x);
	c->VT = VMagnitude(&C);
	delta = c->air.p / units_P0;
	c->IAS = C.x * sqrt(c->air.rho / units_RHO_0)
		/* compressibility correction */
		/* FIXME: mach no. is in the direction of VT, not x ! */
		* (1.0 + 1.0/8*(1.0 - delta)*c->mach*c->mach);
	if (c->IAS < 0.0)
		c->IAS = 0.0;
}


static double
elevatorSetting(craft * c)
{
	double s;

	s = c->pitchComm + aps_get_delta_elevator(c) + c->SeTrim;
	if (s < -1.0) s = -1.0;
	if (s > +1.0) s = +1.0;
	c->Se = s;

	return s;
}

static double
aileronSetting(craft * c)
{
	double    s;

	if( ! damage_isFunctioning(c, SYS_WINGS) )
		return 0.0;

	s = c->rollComm + aps_get_delta_ailerons(c) + c->SaTrim;
	if (s < -1.0) s = -1.0;
	if (s > +1.0) s = +1.0;
	c->Sa = s;

	return s;
}

static double
rudderSetting(craft * c)
{
	double    s;

	s = c->rudderComm + aps_ac_get_delta_rudder(c);
	if (s < -1.0) s = -1.0;
	if (s > +1.0) s = +1.0;
	c->Sr = s;

	return s;
}

char *
pm_flightCalculations(craft * c)
{

	craftType *p = c->cinfo;
	double    qS, s, CLift, CDrag, Sr;
	double    ClBeta;
	double    FLift, FDrag, FWeight, FSideForce;
	double    deltaRoll, deltaPitch, deltaYaw;
	double    y, newy;
	double    xa, inertia, torque, xd, r0;
	double    dNorth, dEast, dmag, dHeading_rad;
	double	  mass_slugs;
	VPoint    F, M, Fg, aeroF, aeroM, gearF, gearM, thrF, thrM;
	VMatrix   mtx, new_AXYZtoNED;
	dis_entity_appearance appearance;
	char      *killReason;

	air_update(&c->air, units_METERStoFEET(c->w.z));
	c->prevSg = c->Sg;
	calcAlphaBetaVT(c);
	Sr = rudderSetting(c);

/*
 *  A note about thrust:  Normal thrust diminishes in proportion to the
 *  decrease in air density.
 */

	c->mach = c->VT / c->air.mach1;

	appearance = dis_if_getEntityAppearance(c->disId);
	appearance &= ~(DISAppearanceAirAfterburnerOn | DISAppearancePlatformPowerplantOn);

/*
	Update thrust reverse position
*/

	if( c->thrust_reverse_on ){
		/* deploy thrust reverser within 2 s */
		c->thrust_reverse_pos -= deltaT / 2.0;
		if( c->thrust_reverse_pos < -0.5 )
			c->thrust_reverse_pos = -0.5;
	} else {
		/* retract thrust reverser within 2 s */
		c->thrust_reverse_pos += deltaT / 2.0;
		if( c->thrust_reverse_pos > 1.0 )
			c->thrust_reverse_pos = 1.0;
	}

/*
	Total engine force and moment.
*/

	c->curThrust = (*p->thrust) (c);
	if (c->flags & FL_AFTERBURNER) {
		appearance |= DISAppearanceAirAfterburnerOn;
	}
	appearance |= DISAppearancePlatformPowerplantOn;
	dis_if_setEntityAppearance(c->disId, appearance);

	VSetPoint(&thrF, c->thrust_reverse_pos * c->curThrust, 0.0, 0.0);
	VSetPoint(&thrM, 0.0, 0.0, 0.0);

/*
	Compute the resultant aerodynamic force vector on the aircraft. By the
	way, the variable "qS" should more properly be named "qS" -- it is the
	dynamic pressure times S, the reference wing area.
*/

	calcCoefficients(c, &CLift, &CDrag);
	ClBeta = interpolate_value(p->ClBeta, fabs(c->alpha));

	qS = c->air.rho * p->wingS * c->VT * c->VT * 0.5;
	if( damage_isFunctioning(c, SYS_WINGS) )
		FLift = CLift * qS;
	else
		FLift = 0.5 * CLift * qS;
	FDrag = CDrag * qS;
	FSideForce = C_Y * qS;
	FWeight = p->emptyWeight + c->fuel + c->payload;

/*
	Detect wings structural maximum limit. Note that FLift sign is reversed
	respect to the aircraft z axis so that FLift is positive when the
	craft is pulled up.
*/

	if( FLift > p->maxLoadZPositive ){
		c->damageBits |= SYS_WINGS | SYS_FLAPS
			| SYS_SPEEDBRAKE | SYS_HYD1 | SYS_HYD2;
		prompt_craft_print(c, "WINGS FAILURE due to excessive positive load");
	} else if( FLift < - p->maxLoadZNegative ){
		c->damageBits |= SYS_WINGS | SYS_FLAPS
			| SYS_SPEEDBRAKE | SYS_HYD1 | SYS_HYD2;
		prompt_craft_print(c, "WINGS FAILURE due to excessive negative load");
	}

/*
	Warn the pilot when the wings load exceeds 75% of maximum load.
*/

	if( FLift > 0.75*p->maxLoadZPositive
	|| FLift < -0.75 * p->maxLoadZNegative )
		c->damageBits |= FLAG_MAX_G_LOAD;
	else
		c->damageBits &= ~FLAG_MAX_G_LOAD;

/*
 *  Set stall warning flag.
 */
	
	if( ! gear_allWheelsGroundContact(c)
	&& c->alpha > c->cinfo->alpha_stall )
		c->damageBits |= FLAG_STALL_WARN;
	else
		c->damageBits &= ~FLAG_STALL_WARN;

/*
	Total aerodynamic force and moment.
	These expressions convert lift and drag forces from wind axes to the
	aircraft fixed axes.  The conversion is based on the wind to
	aircraft transformation matrix supplied in "Airplane Design" by
	Donald Crawford (page 90).
*/

	VSetPoint(&aeroF,
		FLift * sin(c->alpha) - FDrag * cos(c->alpha) * cos(c->beta),
		- FDrag * sin(c->beta) + FSideForce,
		-FLift * cos(c->alpha) - FDrag * cos(c->beta) * sin(c->alpha));
	
/*
 * Aerodynamic torque (for future improvements, for example engine torque,
 * landing gear torque, etc.):
 */

	VSetPoint(&aeroM, 0.0, 0.0, 0.0);

/*
	Detect if speed is greater than the maximum allowed speed Vne:
*/

	if( p->Vne > 0.0
	&& c->IAS > units_KTtoFPS(p->Vne)
	&& (c->damageBits & SYS_WINGS) == 0 ){
		c->damageBits |= SYS_WINGS | SYS_FLAPS | SYS_SPEEDBRAKE;
		prompt_craft_print(c, "WINGS FAILURE due to excessive airspeed");
	}

/*
	With flaps deployed, detect if speed is greater then the maximum allowed
	speed Vfe:
*/

	if( p->Vfe > 0.0
	&& c->curFlap > 0.0
	&& c->IAS > units_KTtoFPS(p->Vfe)
	&& (c->damageBits & SYS_WINGS) == 0 ){
		c->damageBits |= SYS_FLAPS;
		c->curFlap = 0.0;
		c->flapSetting = 0.0;
		prompt_craft_print(c, "FLAPS FAILURE due to excessive airspeed");
	}


/*
 *  Compute forces and moments due to the wheels friction on the terrain
 */

 	killReason = gear_ground_dynamics(c, &gearF, &gearM);
	if( killReason != NULL )
		return killReason;

/*
	Total force and moment.
	FIXME: weight still missing.
 */

	F.x = aeroF.x + gearF.x + thrF.x;
	F.y = aeroF.y + gearF.y + thrF.y;
	F.z = aeroF.z + gearF.z + thrF.z;

	M.x = aeroM.x + gearM.x + thrM.x;
	M.y = aeroM.y + gearM.y + thrM.y;
	M.z = aeroM.z + gearM.z + thrM.z;

/*
 *  Compute fuel consumption
 */

	c->fuel -= planes_fuelUsed(c) + c->leakRate * deltaT;
	if (c->fuel < 0.0)
		c->fuel = 0.0;

/*
 *  Resolve roll-axis (X-axis) changes
 */

/*
	FIXME: to avoid divisions by zero, c->VT cannot be == 0, that's why
	of this 0.001 term.
*/
	xa = p->wings * p->wings * p->wingS * c->air.rho * (c->VT + 0.001) * p->Clp;
	inertia = p->I.m[0][0];
/*
	FIXME: the momentum M is inserted in a quite arbitrary
	point, check if this is the correct one.
*/
	torque = qS * p->wings * 2.0 *
		(p->Clda * - aileronSetting(c) * p->maxAileron
		+ ClBeta * c->beta
		+ p->Cldr * Sr * p->maxRudder)
		//+ M.x  /* FIXME: check */
		+ c->damageCL * qS;
	xd = c->p + torque / xa;
	r0 = xd * inertia / xa;
	deltaRoll = xd * inertia / xa * exp(xa / inertia * deltaT)
		- deltaT * torque / xa - r0;
	//c->p = xd * exp(xa / inertia * deltaT) - torque / xa;
	c->p = xd * exp(xa / inertia * deltaT) - torque / xa + gearM.x / inertia * deltaT;

/*
 *  Resolve pitch-axis (Y-axis) changes
 */

	y = c->alpha + elevatorSetting(c) * p->effElevator;
	twoOrder(CM * qS * p->c / p->I.m[1][1],
		(0.25 * p->wingS * c->air.rho * p->c * p->c *
		c->VT * p->Cmq) / p->I.m[1][1],
		M.y / p->I.m[1][1],
		y, c->q, &newy, &(c->q));
	deltaPitch = newy - y;

/*
 *  Resolve yaw-axis (Z-axis) changes.
 *
 *  We do some trickery here.
 *  If the absolute value of the sideslip angle is greater than 90 degrees,
 *  we trick the code into believing that the sideslip angle is the negative
 *  of its reciprocal value (e.g. -176 becomes -4 degrees).  We do this with
 *  the (somewhat inaccurate) assumption that the CN value for that angle is
 *  roughly equal to the other.
 */

	y = Sr * p->effRudder - c->beta;

	if (y > M_PI / 2.0) {
		y = M_PI - y;
	}
	else if (y < -M_PI / 2.0) {
		y = -M_PI - y;
	}

	s = p->wings;
	twoOrder(CN * qS * s / p->I.m[2][2],
		(p->wingS * c->air.rho * s * s *
		c->VT * p->Cnr) / p->I.m[2][2],
		M.z / p->I.m[2][2],
		y, c->r, &newy, &(c->r));
	deltaYaw = newy - y;

/*
 *  Compute new aircraft trihedral, but don't set it yet.
 */

	VEulerToMatrix(deltaRoll, deltaPitch, deltaYaw, &mtx);
	VMatrixMultByRank(&mtx, &c->AXYZtoNED, &new_AXYZtoNED, 3);


/*
 *  Transform the total force F in AXYZ into Fg in NED and add weight
 */

	VTransform_(&F, &c->AXYZtoNED, &Fg);
	Fg.z += FWeight;

	pm_calcGForces(c, &Fg, FWeight);
		
/*
 *  Update our position (in flight mode).
 */
	mass_slugs = FWeight / units_earth_g;

	dNorth  = units_FEETtoMETERS(c->Cg.x * deltaT + Fg.x / mass_slugs * halfDeltaTSquared);
	dEast   = units_FEETtoMETERS(c->Cg.y * deltaT + Fg.y / mass_slugs * halfDeltaTSquared);
	c->w.z -= units_FEETtoMETERS(c->Cg.z * deltaT + Fg.z / mass_slugs * halfDeltaTSquared);

	dmag = sqrt(dNorth * dNorth + dEast * dEast);

	dHeading_rad = 0.0;

	earth_updateLatLonEx (&c->w,
		dNorth / dmag, dEast / dmag, dmag, &dHeading_rad);

/*
 *  Update velocity vector based on acceleration
 */

	c->Cg.x += Fg.x / mass_slugs * deltaT;
	c->Cg.y += Fg.y / mass_slugs * deltaT;
	c->Cg.z += Fg.z / mass_slugs * deltaT;

/*
 *  Now rotate the trihedral and velocity vector to reflect the change
 *  in heading at the new spheroid location.
 */

	VIdentMatrix( &mtx );
	VRotate( &mtx, ZRotation, dHeading_rad );
	VMatrixMultByRank( &mtx, &new_AXYZtoNED, &c->AXYZtoNED, 3 );
	c->AXYZtoNED = new_AXYZtoNED;
	VTransform_(&c->Cg, &mtx, &c->Cg);

	pm_euler(c);

	if( c->w.z - terrain_localAltitude(c) <= 0.0 ){
		if( ! damage_isFunctioning(c, SYS_WINGS) )
			return "crash due to structural failure";
		else if( c->Cg.z > 30.0 /* ft/s */ )
			return "crash on ground";
		else
			return "main landing gear wasn't down and locked or it was damaged";
	}

	earth_LatLonAltToXYZ(&c->w, &c->Sg);

	earth_generateWorldToLocalMatrix(&c->w, &c->XYZtoNED);

	aps_update( c );

	return NULL;
}


double pm_normalize_roll(double roll)
{
	while( roll < M_PI )  roll += 2*M_PI;
	while( roll > M_PI )  roll -= 2*M_PI;
	return roll;
}


double pm_normalize_pitch(double pitch)
{
	while( pitch < -M_PI/2 ) pitch += 2*M_PI;
	while( pitch > M_PI/2 ) pitch -= 2*M_PI;
	return pitch;
}


double pm_normalize_yaw(double yaw)
{
	while( yaw < 0 )  yaw += 2*M_PI;
	while( yaw >= 2*M_PI )  yaw -= 2*M_PI;
	return yaw;
}


void pm_hud_strings_alloc(craft *c)
{
	int i;

	for( i=0; i<6; i++ ){
		if( c->leftHUD[i] == NULL )
			c->leftHUD[i] = memory_allocate(32, NULL);
		c->leftHUD[i][0] = '\0';

		if( c->rightHUD[i] == NULL )
			c->rightHUD[i] = memory_allocate(32, NULL);
		c->rightHUD[i][0] = '\0';
	}
}


void pm_hud_strings_free(craft *c)
{
	int i;

	for( i=0; i<6; i++ ){
		memory_dispose(c->leftHUD[i]);   c->leftHUD[i] = NULL;
		memory_dispose(c->rightHUD[i]);  c->rightHUD[i] = NULL;
	}
}


void pm_thrust_reverse_toggle(craft *c)
{
	if( c->thrust_reverse_on ){
		c->thrust_reverse_on = FALSE;

	/* thrust setting must be idle to activate reverser: */
	} else if( c->cinfo->hasThrustReverser
	&& c->throttleComm <= 32768 * 25 / 100 ){
		c->thrust_reverse_on = TRUE;
	}
}


void pm_after_burner_toggle(craft *c)
{
	if ( c->cinfo->maxThrust != c->cinfo->maxABThrust )
		c->flags ^= FL_AFTERBURNER;
}


void pm_after_burner_on(craft *c)
{
	if ( c->cinfo->maxThrust != c->cinfo->maxABThrust )
		c->flags |= FL_AFTERBURNER;
}


void pm_after_burner_off(craft *c)
{
	c->flags &= ~FL_AFTERBURNER;
}


double
pm_mag_heading(craft * c)
{
	// Update local magnetic declination.
	if (curTime >= c->updActualMagneticField) {
		wmm_MagneticField mf;
		wmm_getMagneticField(0.0, c->w.latitude, c->w.longitude, c->w.z, &mf);
		VSetPoint(&c->actualMagneticField, mf.X, mf.Y, mf.Z);
		c->actualLocalVAR = - mf.Decl;
		// Update mag. var. every 1 NM or every 10 s, whichever comes first:
		double dt = units_NmToFeetFactor / (1.0 + c->VT);
		if( dt > 10.0 )
			dt = 10.0;
		c->updActualMagneticField = curTime + dt;
	}

	// Indicated magnetic field smoothly follows the calculated one:
	if (curTime >= c->updIndicatedMagneticField) {
		double k = 0.01;
		c->indicatedMagneticField.x += k * (c->actualMagneticField.x - c->indicatedMagneticField.x);
		c->indicatedMagneticField.y += k * (c->actualMagneticField.y - c->indicatedMagneticField.y);
		c->indicatedMagneticField.z += k * (c->actualMagneticField.z - c->indicatedMagneticField.z);
		c->indicatedLocalVAR += k * (c->actualLocalVAR - c->indicatedLocalVAR);
		c->updIndicatedMagneticField = curTime + 0.1;
	}

	return pm_normalize_yaw( c->curHeading + c->indicatedLocalVAR );
}
