/*
 *  ACM - Landing gear module
 *  Copyright (C) 2008  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.
 */

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

#include "../util/error.h"
#include "../util/memory.h"
#include "../util/units.h"
#include "sounds.h"
#include "damage.h"
#include "pm.h"
#include "prompt.h"
#include "terrain.h"

#define gear_IMPORT
#include "gear.h"

#define GEARDOWN (M_PI / 2.0)
#define MU_SKID 0.85


typedef struct gear_Type {
	/** If gear is in locked down position and can sustain the weight. */
	_BOOL isHandleDown;
	/** Nose, left, right gear extension angle (RAD). */
	double noseAngle;
	double leftAngle;
	double rightAngle;
	/** If nose, left, right are in contact with ground. */
	_BOOL noseGroundContact;
	_BOOL leftGroundContact;
	_BOOL rightGroundContact;
	/** Current nose wheel steering angle (RAD). */
	double noseSteeringAngle;
	/** If brakes engaged. */
	_BOOL isBraking;
	/** Braking smoothing factor in [0,1]. */
	double brakingFactor;
} gear_Type;


static gear_Type * get(craft * c)
{
	gear_Type * g;
	g = (gear_Type *) c->gear;
	if( g == NULL )
		error_internal("missing gear data structure for %s", c->name);
	return g;
}


void gear_free(craft * c)
{
	memory_dispose(c->gear);
	c->gear = NULL;
}


void gear_allocate(craft * c)
{
	gear_Type * g = memory_allocate( sizeof(gear_Type), NULL );
	g->isHandleDown = FALSE;
	g->noseAngle = 0.0;
	g->leftAngle = 0.0;
	g->rightAngle = 0.0;
	g->noseGroundContact = FALSE;
	g->leftGroundContact = FALSE;
	g->rightGroundContact = FALSE;
	g->noseSteeringAngle = 0.0;
	g->isBraking = FALSE;
	g->brakingFactor = 0.0;
	c->gear = g;
}


_BOOL gear_isHandleDown(craft * c)
{
	gear_Type * g;
	g = get(c);
	return g->isHandleDown;
}


static _BOOL gear_isUp(craft * c)
{
	gear_Type * g;
	g = get(c);
	return g->noseAngle == 0.0
		&& g->leftAngle == 0.0
		&& g->rightAngle == 0.0;
}


/*************
static _BOOL gear_isDown(craft * c)
{
	gear * g;
	g = get(c);
	return g->noseAngle == GEARDOWN
		&& g->leftAngle == GEARDOWN
		&& g->rightAngle == GEARDOWN;
}


static _BOOL gear_isMoving(craft * c)
{
	return ! gear_isUp(c) && ! gear_isDown(c);
}
**********/

int gear_nosePosition(craft * c)
{
	gear_Type * g;
	g = get(c);
	if( g->noseAngle == 0.0 )
		return 0;
	else if( g->noseAngle == GEARDOWN )
		return 2;
	else
		return 1;
}


int gear_leftPosition(craft * c)
{
	gear_Type * g;
	g = get(c);
	if( g->leftAngle == 0.0 )
		return 0;
	else if( g->leftAngle == GEARDOWN )
		return 2;
	else
		return 1;
}


int gear_rightPosition(craft * c)
{
	gear_Type * g;
	g = get(c);
	if( g->rightAngle == 0.0 )
		return 0;
	else if( g->rightAngle == GEARDOWN )
		return 2;
	else
		return 1;
}


void gear_up(craft *c)
{
	gear_Type * g;
	g = get(c);
	g->isHandleDown = FALSE;
	g->noseAngle = 0.0;
	g->leftAngle = 0.0;
	g->rightAngle = 0.0;
}


void gear_down(craft *c)
{
	gear_Type * g;
	g = get(c);
	g->isHandleDown = TRUE;
	g->noseAngle = GEARDOWN;
	g->leftAngle = GEARDOWN;
	g->rightAngle = GEARDOWN;
}


_BOOL gear_allWheelsGroundContact(craft * c)
{
	gear_Type * g;
	g = get(c);
	return g->noseGroundContact && g->leftGroundContact && g->rightGroundContact;
}


_BOOL gear_someWheelGroundContact(craft * c)
{
	gear_Type * g;
	g = get(c);
	return g->noseGroundContact || g->leftGroundContact || g->rightGroundContact;
}


_BOOL gear_noseWheelGroundContact(craft * c)
{
	gear_Type * g;
	g = get(c);
	return g->noseGroundContact;
}


_BOOL gear_mainWheelsGroundContact(craft * c)
{
	gear_Type * g;
	g = get(c);
	return g->leftGroundContact || g->rightGroundContact;
}


void gear_handle_up(craft *c)
{
	gear_Type * g;
	g = get(c);
	if( ! g->isHandleDown )
		return;
	g->isHandleDown = FALSE;
	sounds_playSound(c, sounds_GearUp, FALSE);
}


void gear_handle_down(craft *c)
{
	gear_Type * g;
	g = get(c);
	if( g->isHandleDown )
		return;
	g->isHandleDown = TRUE;
	sounds_playSound(c, sounds_GearDown, FALSE);
}


void gear_handle_toggle(craft *c)
{
	gear_Type * g;
	g = get(c);
	if( g->isHandleDown )
		gear_handle_up(c);
	else
		gear_handle_down(c);
}


_BOOL gear_brakesEngaged(craft * c)
{
	gear_Type * g;
	g = get(c);
	return g->isBraking;
}


void gear_brakesEngage(craft * c)
{
	gear_Type * g;
	g = get(c);
	g->isBraking = TRUE;
}


void gear_brakesDisengage(craft * c)
{
	gear_Type * g;
	g = get(c);
	g->isBraking = FALSE;
}


void gear_ground_altitude_pitch(craft *c, double *altitude, double *pitch)
{
	craftType *p;
	double w, l, zn, zm, delta_cn, delta_cm, ne, me, F, M, dF_dcn, dF_dcm,
		dM_dcn, dM_dcm, new_F, new_M, z, dcn, dcm, err2max;
	int i;


	void force_and_moment(double ne, double me, double *F, double *M)
	/*
	 *  Calculate static forces due to springs for the aircraft at
	 *  rest on the horizontal ground. 'ne' is the nose oleo extension,
	 *  'me' is the main oleo extension.
	 *  
	 *  Return total vertical force applied to the CM and total moment
	 *  around y axis.
	 */
	{
		double s, z, cosa, sina, Fn, Fm, ln, lm;

		/*
			Compute sine and cosine of the rest pitch angle
		*/

		if( p->rn.x > p->rm.x )
			s = 1.0;
		else
			s = -1.0;

		z = zn + ne - zm - me;
		cosa = l / sqrt(l*l + z*z);
		sina = s * z / sqrt(l*l + z*z);

		Fn = (p->cnMax - ne)*p->Kn / cosa;
		Fm = (p->cmMax - me)*p->Km / cosa;
		*F = w - Fn - Fm;

		ln = p->rn.x*cosa + (zn + ne)*sina;
		lm = p->rm.x*cosa + (zm + me)*sina;
		*M = Fn*ln + Fm*lm;
	}


	p = c->cinfo;
	w = p->emptyWeight + c->fuel + c->payload;  /* weight (lbf) */
	l = fabs(p->rn.x - p->rm.x);
	zn = p->rn.z + p->Gn;
	zm = p->rm.z + p->Gm;

	/*
	 *  We are looking for the zero of the total force F(ne,me) and total
	 *  moment M(ne,me) being ne,me the extension of the springs. We
	 *  start from the middle point then moving the point (ne,me) in
	 *  the box [0,cnMax]x[0,cmMax]:
	 */

	ne = 0.5*p->cnMax;
	me = 0.5*p->cmMax;

	/*
	 *  Partial derivatives are numerically calculated using these steps:
	 */

	delta_cn = 1e-5 * p->cnMax;
	delta_cm = 1e-5 * p->cmMax;

	/*
	 *  The loop continues until the changes in ne,me become less than
	 *  about one hundredth of the max extension. Calculate the square
	 *  of this error:
	 */

	err2max = 1e-6 * (p->cnMax * p->cnMax + p->cmMax * p->cmMax);

	i = 0;
	do {

		/*
		 *  Evaluate the derivatives of F(ne,me) and M(ne,me): this
		 *  gives the plane that best approximates the functions F,M
		 *  around the point (ne,me):
		 *  
		 *  dF = dF_dcn  dcn + dF_dcm * dcm dM = dM_dcn  dcn + dM_dcm
		 *  * dcm
		 */

		force_and_moment(ne, me,  &F, &M);
#ifdef DEBUG
		printf("FIXME: ne=%f  me=%f  F=%f  M=%f\n", ne, me, F, M);
#endif

		force_and_moment(ne + delta_cn, me,  &new_F, &new_M);
		dF_dcn = (new_F - F) / delta_cn;
		dM_dcn = (new_M - M) / delta_cn;

		force_and_moment(ne, me + delta_cm,  &new_F, &new_M);
		dF_dcm = (new_F - F) / delta_cm;
		dM_dcm = (new_M - M) / delta_cm;

		/*
		 *  Move the point (ne,me) along the approximating planes in
		 *  order to cancel F,M:
		 *  
		 *  -F = dF_dcn  dcn + dF_dcm * dcm -M = dM_dcn  dcn + dM_dcm
		 *  * dcm
		 *  
		 *  Resolving this system of equations gives us dcn,dcm.
		 */

		dcm = (dM_dcn*F - dF_dcn*M) / (dF_dcn*dM_dcm - dF_dcm*dM_dcn);
		me += dcm;

		dcn = -F/dF_dcn -dF_dcm/dF_dcn*dcm;
		ne += dcn;

		if( ne < 0.0 )  ne = 0.0;
		if( ne > p->cnMax - delta_cn )  ne = p->cnMax - delta_cn;

		if( me < 0.0 )  me = 0.0;
		if( me > p->cmMax - delta_cm )  me = p->cmMax - delta_cm;

		i++;
#ifdef DEBUG
		printf("FIXME: %d) ne=%.0f%%  me=%.0f%%\n",
			i, 100.0 * ne / p->cnMax, 100.0 * me / p->cmMax);
#endif
		if( i > 9 ){

			/*
			 *  Usually two or three loops are enough, but
			 *  still there are cases in which this algo does
			 *  not converge.
			 */

			printf("WARNING: unbalanced gear parameters, can't find a good rest position.\n");
			break;
		}

	} while( dcn*dcn + dcm*dcm > err2max );


	z = zn + ne - zm - me;
	if( p->rn.x < p->rm.x )
		z = -z;
	*pitch = asin( z / sqrt(l*l + z*z) );

	*altitude = (zm + me)*cos(*pitch) + (-p->rm.x)*sin(*pitch);

#ifdef DEBUG
	printf("FIXME: alt=%f  pitch=%f\n", *altitude, units_RADtoDEG(*pitch));
#endif
}


static void
gear_move_up_down(craft *c)
{
	gear_Type * g;
	g = get(c);
	if (damage_isFunctioning(c, SYS_NOSEGEAR)) {
		if (g->isHandleDown) {
			if (g->noseAngle != GEARDOWN) {
				g->noseAngle += c->cinfo->gearRate * deltaT;
				if (g->noseAngle > GEARDOWN)
					g->noseAngle = GEARDOWN;
			}
		}
		else {
			if (g->noseAngle != 0.0) {
				g->noseAngle -= c->cinfo->gearRate * deltaT;
				if (g->noseAngle < 0.0)
					g->noseAngle = 0.0;
			}
		}
	}

	if (damage_isFunctioning(c, SYS_LEFTMAIN)) {
		if (g->isHandleDown) {
			if (g->leftAngle != GEARDOWN) {
				g->leftAngle += 0.8*c->cinfo->gearRate * deltaT;
				if (g->leftAngle > GEARDOWN)
					g->leftAngle = GEARDOWN;
			}
		}
		else {
			if (g->leftAngle != 0.0) {
				g->leftAngle -= 0.8*c->cinfo->gearRate * deltaT;
				if (g->leftAngle < 0.0)
					g->leftAngle = 0.0;
			}
		}
	}

	if (damage_isFunctioning(c, SYS_RIGHTMAIN)) {
		if (g->isHandleDown) {
			if (g->rightAngle != GEARDOWN) {
				g->rightAngle += 0.9*c->cinfo->gearRate * deltaT;
				if (g->rightAngle > GEARDOWN)
					g->rightAngle = GEARDOWN;
			}
		}
		else {
			if (g->rightAngle != 0.0) {
				g->rightAngle -= 0.9*c->cinfo->gearRate * deltaT;
				if (g->rightAngle < 0.0)
					g->rightAngle = 0.0;
			}
		}
	}
}


static double
spring_and_damper_force(double K, double D, double eMax, double e, double e_dot)
/*
 *  Compute spring + damper force
 *  K = spring factor (lbf/s^2)
 *  D = damper factor (lbf s /ft)
 *  eMax = max extension (ft)
 *  e = current extension (ft)
 *  e_dot = extension rate (ft/s)
 *  
 *  Return: the resultant vertical force (lbf), always <= 0.0.  If the result
 *  is 0.0, the wheel lost contact with ground.
 */
{
	double spring, damper;

	if( e >= eMax )
		return 0.0;
	
	spring = - K * (eMax - e);

	if( e < 0.1 )
		/* add resistance in the last 0.1 feet */
		spring -= 100 * K * (e - 0.1);

	damper = D * e_dot;

	if( damper + spring > 0.0 ){
		/* wheel lost contact with ground */
		spring = 0.0;
		damper = 0.0;
	}

	return spring + damper;
}


static inline double sgn(double x)
{
	if( x > 0.0 )
		return 1.0;
	else if( x < 0.0 )
		return -1.0;
	else
		return 0.0;
}


#define SLIP_MAX units_DEGtoRAD(3)


static _BOOL               /* return TRUE if wheel is skidding     */
wheel_friction(
	double deflection,     /* steering angle, positive right (RAD) */
	double vx, double vy,  /* forward and rightward velocity (ft/s)*/
	double Fn,             /* normal force (lbf)                   */
	double muStatic,       /* rolling, near rest mu factor         */
	double muKinetic,      /* rolling mu factor                    */
	double muLateral,      /* lateral mu factor                    */
	VPoint *F              /* resulting friction force (lbf)       */
	, char *debug /* FIXME: remove */
)
/*
 *  This function computes the friction force F of the wheel due to the
 *  rolling friction and the lateral friction. The rolling friction brakes
 *  the longitudinal motion, while the lateral friction is responsible for
 *  the steering and the lateral skidding. Basically the longitudinal force
 *  is proportional to the normal force:
 *  
 *          force = mu  Fn
 *  
 *  where mu becomes larger at small speed (muStatic) and smaller at high
 *  speed (muKinetic).
 *  
 *  SLIP_MAX is the maximum angle of slip between wheel rolling direction
 *  and actual velocity.  Below this angle the lateral friction is linerly
 *  dependent on the slip angle:
 *  
 *          force = Fn  muSkid * slip / SLIP_MAX
 *  
 *  above that angle the lateral force is constant:
 *  
 *          force = Fn  muSkid
 *  
 *  This model for wheel steering is well described in this paper:
 *
 *  How to Drive Like a Racecar Driver: Vehicle Stabilization at the Limits
 *  of Handling Yung-Hsiang (Judy) Hsu, Shad Laws, Chris Gerdes Stanford
 *  University http://ddl.stanford.edu/atthelimits
 *  
 *  See also:
 *  
 *  Tire Model in Driving Simulator
 *  http://code.eng.buffalo.edu/dat/sites/tire/tire.html where the relation
 *  between steering and braking forces and the relation between velocity
 *  and varying mu coefficients are discussed.  In our model the nose wheel
 *  can steer but cannot brake, and vice-versa the left and right wheels
 *  can brake but do not steer.
 *  
 *  The Physics of Racing Brian Beckman http://phors.locost7.info/contents.htm
 */
{
	_BOOL skid;
	double rx, ry, v_lateral, k, v_forward, F_forward, F_lateral, slip;

	skid = FALSE;

	if( deflection == 0.0 ){
		rx = 1.0;
		ry = 0.0;
	} else {
		rx = cos(deflection);
		ry = sin(deflection);
	}

	/*
	 *  Evaluate the longitudinal static or rolling friction along the
	 *  longitudinal vector (rx,ry):
	 */

	v_forward = vx*rx + vy*ry;
	k = v_forward / 0.5;
	if( fabs(k) > 1.0 )
		k = sgn(k);
	if( fabs(v_forward) < 0.5 /* ft/s */ )
		F_forward = - k * muStatic  * Fn;
	else
		F_forward = - k * muKinetic * Fn;

	/*
	 *  Evaluate the lateral friction along the lateral vector (-ry,rx):
	 */

	v_lateral = rx*vy - ry*vx;
	k = v_lateral / 3.0;
	if( fabs(k) > 1.0 )
		k = sgn(k);
	/*
	 *  Slip angle between wheel direction and velocity, normalized to
	 *  the range +/-90 DEG:
	 */
	slip = atan2( v_lateral, v_forward );
	if( ! (slip >= -M_PI && slip <= M_PI) )
		slip = 0.5 * M_PI * sgn(v_lateral);
	if( slip > 0.5*M_PI )
		slip = M_PI - slip;
	else if( slip < -0.5*M_PI )
		slip = -slip - M_PI;

	if( fabs(slip) < SLIP_MAX ){
		/* linear behavior: */
		F_lateral = - k * muLateral * fabs(slip) / SLIP_MAX * Fn;
	} else {
		/* saturation */
		if( fabs(slip) > 2*SLIP_MAX && fabs(k) > 0.5 )
			skid = TRUE;
		F_lateral = - k * muLateral * Fn;
	}

	//F_lateral = 0.0;  /* FIXME */
	//F_forward = 0.0;  /* FIXME */

	F->x = F_forward * rx - F_lateral * ry;
	F->y = F_forward * ry + F_lateral * rx;
	F->z = 0.0;

#ifdef DEBUG
	printf("FIXME: %5s, Fn=%6.0f  v=%5.1f %5.1f  defl=%5.1f  slip=%5.1f  F=%7.0f %7.0f\n",
		debug, Fn, vx, vy, units_RADtoDEG(deflection), units_RADtoDEG(slip), F->x, F->y);
#endif

	return skid;
}


double gear_get_drag(craft * c)
{
	gear_Type * g;

	if( gear_isUp(c) )
		return 0.0;
	g = get(c);
	return (sin(g->noseAngle) + sin(g->leftAngle) + sin(g->rightAngle))
		/ 3.0 * c->cinfo->cGearDrag;
}


char *
gear_ground_dynamics(craft * c, VPoint *gearF, VPoint *gearM)
{
	gear_Type * g;
	craftType *p;
	double    ne, ne_dot, re, re_dot, le, le_dot;
	VPoint    nw, nw_dot, rw, rw_dot, lw, lw_dot;
	VPoint    r, r_dot, q, q_dot;
	double    ground_z, muStatic, muKinetic, k, v;
	VPoint    F, M, mt, Cg, Fl, Fr, Fn, FnMu, FlMu, FrMu;
	VPoint    omega;
	VMatrix   NEDtoSWC, AXYZtoSWC;
	_BOOL     skid;

	g = get(c);
	gear_move_up_down(c);

	/*
	 * Update braking factor.
	 * Engage/disengage within 1 s.
	 */
	if (g->isBraking){
		g->brakingFactor += deltaT;
		if( g->brakingFactor > 1.0 )
			g->brakingFactor = 1.0;
	} else {
		g->brakingFactor -= deltaT;
		if( g->brakingFactor < 0.0 )
			g->brakingFactor = 0.0;
	}

	if( g->noseAngle < GEARDOWN
	&& g->leftAngle < GEARDOWN
	&& g->rightAngle < GEARDOWN ){
		/* none of the wheels is fully extended */
		VSetPoint(gearF, 0, 0, 0);
		VSetPoint(gearM, 0, 0, 0);
		return NULL;
	}

	ground_z = units_METERStoFEET(c->w.z - terrain_localAltitude(c));
	if( ground_z <= 0.0 ){
		return "sunken under the terrain...";
	} else if( ground_z > 500.0 ){
		/* very far from the terrain */
		VSetPoint(gearF, 0, 0, 0);
		VSetPoint(gearM, 0, 0, 0);
		return NULL;
	}

	p = c->cinfo;

/*
 *  The Simplified Word Coordinates (SWC) is a reference frame centered
 *  on the aircraft CM with the z axis pointing downward to the ground and
 *  the x axis oriented as the current heading. This frame makes easier to
 *  detect wheels contact with the ground. The ground is the plane z=ground_z
 *  (ft). We compute two useful rotational matrices:
 *  
 *  NEDtoSWC    that rotates from NED to SWC, and
 *  
 *  AXYZtoSWC   that rotates from aircraft frame to SWC
 */

	VIdentMatrix(&NEDtoSWC);
	VRotate(&NEDtoSWC, ZRotation, -c->curHeading);

	AXYZtoSWC = c->AXYZtoNED;
	VRotate(&AXYZtoSWC, ZRotation, -c->curHeading);

/*
 *  Cg (ft/s) is the CM ground speed vector with components in SWC.
 */

	VTransform(&c->Cg, &NEDtoSWC, &Cg);
	v = VMagnitude(&Cg);

/*
 *  Set nose wheel steering angle c->curNWDef (RAD).
 *  
 *  Since mouse and joystick give very little force feedback, steering at
 *  high speed often results in a catastrophic disaster :-) We have two
 *  choices here: locking the nose wheel at high speed or gently reduce the
 *  steering effectiveness. We follow this latter way.
 *  
 *  The criteria is: given the pilot's steering input, calculate an angle
 *  of steering that produces the same centripetal acceleration at any speed
 *  above some limit speed V_THR. This result is achieved as follows.
 *  
 *  The radius of steering for the point between the two main wheels (that
 *  typically is also very close to the CM) is
 *  
 *  r = Q / tan(c->curNWDef)
 *  
 *  where Q=abs(c->rn.x - c->rm.x) is the distance between the nose and the
 *  main gears. For little angles, this formula becomes
 *  
 *  r = Q / c->curNWDef
 *  
 *  The centripetal acceleration of the CM for v < V_THR is
 *  
 *  a = v*v/r = v*v*k*c->curNWDef/Q   with   k=1.0
 *  
 *  For speeds above V_THR we reduce the steering angle by a factor "k"
 *  such that the resulting centripetal acceleration be the same we had at
 *  v=V_THR with the given pilot's input:
 *  
 *  k = (V_THR/v)^2
 */

#define V_THR units_KTtoFPS(10.0)

	if( v < V_THR )
		k = 1.0;
	else
		k = V_THR*V_THR / (v * v);
	//g->noseSteeringAngle = - k * (c->steerComm * c->cinfo->maxNWDef);
	g->noseSteeringAngle = - k * (c->rollComm * c->cinfo->maxNWDef);

/*
 *  muStatic and muKinetic are the friction mu coefficients of the left and
 *  right wheels, that varies depending on brakes on/off. Since the nose
 *  wheel does not brake, it always uses p->muStatic and p->muKinetic.
 */

	if (g->isBraking) {
		muStatic = p->muBStatic;
		muKinetic = p->muBKinetic;
	}
	else {
		muStatic = p->muStatic;
		muKinetic = p->muKinetic;
	}

	/*
	 * Compute brake force rate.
	 */

	muStatic = (1.0 - g->brakingFactor) * p->muStatic + g->brakingFactor * p->muBStatic;
	muKinetic = (1.0 - g->brakingFactor) * p->muKinetic + g->brakingFactor * p->muBKinetic;

/*
 *  A few theory about how oleo strut extension and extension rate are
 *  calculated:
 *  
 *  r = position of the gear strut attachment in AXYZ (ex. rn + Gn)
 *  q = [0,0,1] unity vector giving the oleo extension direction
 *  e = oleo extension (ranging from 0 to cnMax or cmMax for nose or main)
 *  M = AXYZtoSWC (for brevity)
 *  
 *  The wheel position in AXYZ will be ("==" means "definition"):
 *  
 *  w == r + eq
 *  
 *  Vector components can also conveniently be expressed in the SWC frame:
 *  
 *  r' == Mr  (components of r in SWC, NOT the position in SWC!)
 *  
 *  q' == Mq  (components of q in SWC)
 *  
 *  The wheel position in SWC requires to rotate the components of w and
 *  add the position of the CM in the SWC frame, Sg:
 *  
 *  w' == Mw + Sg
 *     = r' + eq' + Sg            (1)
 *  
 *  If the wheel is in contact with ground, the z component of the eq. 1
 *  must be zero, from which we obtain the oleo extension:
 *  
 *  e = - (r'.z + Sg.z) / q'.z     (2)
 *  
 *  Speeds in SWC can be easily calculated introducing the rotational speed
 *  vector applied to the CM with components in SWC:
 *  
 *  omega == [c->p, c->q, c->r]
 *  
 *  from which for any vector r in components AXYZ, its components in SWC
 *  are r' = Mr and its velocity in components SWC is r'_dot = omega x r'.
 *  We can now calculate the derivative of e, needed to calculate the
 *  damping. We obtain this value deriving (2):
 *  
 *  e_dot = - ([omega x r'].z + Cg.z) / q'.z - e  [omega x q'].z / q'.z
 *  
 *  Finally, the velocity of the wheel in SWC is the derivative of eq. 1:
 *  
 *  w'_dot = omega x r' + e_dot  q' + e * q'_dot + Cg
 *  
 *  We will now calculate e, e_dot and w'_dot for each wheel:
 */

	/* Angular velocity vector, components in SWC: */
	VSetPoint(&omega, c->p, c->q, c->r);

	/* Stroke unity vector and its derivative, components in SWC: */
	VSetPoint(&q, 0, 0, 1);
	VTransform_(&q, &AXYZtoSWC, &q);
	VCrossProd(&omega, &q, &q_dot);

	/* NOSE GEAR */
	ne = 0.0;  /* dummy */
	ne_dot = 0.0;  /* dummy */
	VSetPoint(&nw, 0.0, 0.0, 0.0);  /* dummy */
	VSetPoint(&nw_dot, 0.0, 0.0, 0.0);  /* dummy */
	if( g->noseAngle == GEARDOWN ){
		/*
		 *  Gear down and locked.
		 */

		/* Strut attachment position, components in SWC: */
		r = p->rn;
		r.z += p->Gn;
		VTransform_(&r, &AXYZtoSWC, &r);

		if( r.z < 1e-3 ){
			/* Wheel is certainly not in contact. */
			g->noseGroundContact = FALSE;
			ne = p->cnMax;  /* FIXME: not needed */
		} else {
			/* Possible contact with ground. Calculate oleo extension: */
			ne = (ground_z - r.z) / q.z;
			if( ne < 0.0 ){
				g->noseGroundContact = TRUE;
				return "nose gear smash";
			} else if( ne > p->cnMax ){
				/* Not in contact: */
				g->noseGroundContact = FALSE;
				ne = p->cnMax;
			} else {
				/* In contact: */
				/* Oleo extension rate: */
				VCrossProd(&omega, &r, &r_dot);
				ne_dot = - (r_dot.z + Cg.z) / q.z - ne * q_dot.z / q.z;

				/* Wheel position, aircraft frame: */
				nw.x = p->rn.x;
				nw.y = p->rn.y;
				nw.z = p->rn.z + p->Gn + ne;

				/* Wheel velocity vs. ground, components in SWC: */
				nw_dot.x = r_dot.x + ne_dot * q.x + ne * q_dot.x + Cg.x;
				nw_dot.y = r_dot.y + ne_dot * q.y + ne * q_dot.y + Cg.y;
				nw_dot.z = r_dot.z + ne_dot * q.z + ne * q_dot.z + Cg.z;

				if( ! g->noseGroundContact
				&& fabs(nw_dot.x) + fabs(nw_dot.y) > 1.0 && ne_dot <= 0 ){
					/* First contact. */
					sounds_playSound(c, sounds_Touchdown, FALSE);
				}
				g->noseGroundContact = TRUE;
			}
		}

	} else {
		/*
		 *  Gear up or not fully extended.
		 */
		g->noseGroundContact = FALSE;
	}


	/* RIGHT GEAR */
	re = 0.0;  /* dummy */
	re_dot = 0.0;  /* dummy */
	VSetPoint(&rw, 0.0, 0.0, 0.0);  /* dummy */
	VSetPoint(&rw_dot, 0.0, 0.0, 0.0);  /* dummy */
	if( g->leftAngle == GEARDOWN ){
		/*
		 *  Gear down and locked.
		 */

		/* Strut attachment position, components in SWC: */
		r = p->rm;
		r.z += p->Gm;
		VTransform_(&r, &AXYZtoSWC, &r);

		if( r.z < 1e-3 ){
			/* Wheel is certainly not in contact. */
			g->rightGroundContact = FALSE;
			re = p->cmMax;  /* FIXME: not needed */
		} else {
			/* Possible contact with ground. Calculate oleo extension: */
			re = (ground_z - r.z) / q.z;
			if( re < 0.0 ){
				g->rightGroundContact = TRUE;
				return "right gear smash";
			} else if( re > p->cmMax ){
				/* Not in contact: */
				g->rightGroundContact = FALSE;
				re = p->cmMax;
			} else {
				/* In contact: */
				/* Oleo extension rate: */
				VCrossProd(&omega, &r, &r_dot);
				re_dot = - (r_dot.z + Cg.z) / q.z - re * q_dot.z / q.z;

				/* Wheel position, aircraft frame: */
				rw.x = p->rm.x;
				rw.y = p->rm.y;
				rw.z = p->rm.z + p->Gm + re;

				/* Wheel velocity vs. ground, components in SWC: */
				rw_dot.x = r_dot.x + re_dot * q.x + re * q_dot.x + Cg.x;
				rw_dot.y = r_dot.y + re_dot * q.y + re * q_dot.y + Cg.y;
				rw_dot.z = r_dot.z + re_dot * q.z + re * q_dot.z + Cg.z;

				if( ! g->rightGroundContact
				&& fabs(rw_dot.x) + fabs(rw_dot.y) > 1.0 && re_dot < 0 ){
					/* First contact. */
					sounds_playSound(c, sounds_Touchdown, FALSE);
				}
				g->rightGroundContact = TRUE;
			}
		}

	} else {
		/*
		 *  Gear up or not fully extended.
		 */
		g->rightGroundContact = FALSE;
	}


	/* LEFT GEAR */
	le = 0.0;  /* dummy */
	le_dot = 0.0;  /* dummy */
	VSetPoint(&lw, 0.0, 0.0, 0.0);  /* dummy */
	VSetPoint(&lw_dot, 0.0, 0.0, 0.0);  /* dummy */
	if( g->rightAngle == GEARDOWN ){
		/*
		 *  Gear down and locked.
		 */

		/* Strut attachment position, components in SWC: */
		r = p->rm;
		r.y = -r.y;
		r.z += p->Gm;
		VTransform_(&r, &AXYZtoSWC, &r);

		if( r.z < 1e-3 ){
			/* Wheel is certainly not in contact. */
			g->leftGroundContact = FALSE;
			le = p->cmMax;  /* FIXME: not needed */
		} else {
			/* Possible contact with ground. Calculate oleo extension: */
			le = (ground_z - r.z) / q.z;
			if( le < 0.0 ){
				g->leftGroundContact = TRUE;
				return "left gear smash";
			} else if( le > p->cmMax ){
				/* Not in contact: */
				g->leftGroundContact = FALSE;
				le = p->cmMax;
			} else {
				/* In contact: */
				/* Oleo extension rate: */
				VCrossProd(&omega, &r, &r_dot);
				le_dot = - (r_dot.z + Cg.z) / q.z - le * q_dot.z / q.z;

				/* Wheel position, aircraft frame: */
				lw.x = p->rm.x;
				lw.y = -p->rm.y;
				lw.z = p->rm.z + p->Gm + le;

				/* Wheel velocity vs. ground, components in SWC: */
				lw_dot.x = r_dot.x + le_dot * q.x + le * q_dot.x + Cg.x;
				lw_dot.y = r_dot.y + le_dot * q.y + le * q_dot.y + Cg.y;
				lw_dot.z = r_dot.z + le_dot * q.z + le * q_dot.z + Cg.z;

				if( ! g->leftGroundContact
				&& fabs(lw_dot.x) + fabs(lw_dot.y) > 1.0 && le_dot < 0 ){
					/* First contact. */
					sounds_playSound(c, sounds_Touchdown, FALSE);
				}
				g->leftGroundContact = TRUE;
			}
		}

	} else {
		/*
		 *  Gear up or not fully extended.
		 */
		g->leftGroundContact = FALSE;
	}

#ifdef DEBUG
	printf("FIXME: ne=%.0f%%  re=%.0f%%  le=%.0f%%\n",
		100.0 * ne / p->cnMax, 100.0 * re / p->cmMax, 100.0 * le / p->cmMax);
	printf("FIXME: ne_dot=%f  re_dot=%f  le_dot=%f\n", ne_dot, re_dot, le_dot);
#endif

/* 
 *  Compute forces Fn,Fr,Fl (AXYZ) due to the static reaction of the wheels
 *  against the terreain. If the terrain were perfectly smooth and clean,
 *  this force would sustain the aircraft, but it would be impossible to
 *  brake and to steer.
 *  
 *  The spring constant p->Km and the damping factor p->Dm in the inventory
 *  refers to the two main landing gear right+left as a whole, that's why
 *  of the 0.5 factor appearing in front of these coefficients.
 *  
 *  To these forces we have to add also the longitudinal friction and lateral
 *  friction FnMu,FrMu,FlMu (AXYZ). These forces allow to brake and steer
 *  the aircraft.
 */

	/* NOSE GEAR: */

	VSetPoint(&Fn, 0.0, 0.0, 0.0);

	/* Add force Fn.z (AXYZ) due to spring and damper: */
	if( g->noseGroundContact ){
		Fn.z = spring_and_damper_force(p->Kn, p->Dn, p->cnMax, ne, ne_dot);
		if( Fn.z < - 1.0 /* FIXME */ * p->emptyWeight )
			return "nose gear collapsed under too high vertical load";
		if( Fn.z == 0.0 )
			g->noseGroundContact = FALSE;
	}

	/* Add friction and steering effects: */
	if( g->noseGroundContact ){
		VPoint f;
		double fm, deflection;
		
		/* Calculate terrain normal force f (SWC): */
		fm = Fn.z * Fn.z;
		VTransform_(&Fn, &AXYZtoSWC, &f);
		VSetPoint(&f, 0, 0, fm / f.z);

		/* Set actual static force Fn (AXYZ): */
		VReverseTransform_(&f, &AXYZtoSWC, &Fn);

		if( p->rn.x > p->rm.x ) /* tricycle gear */
			deflection = g->noseSteeringAngle;
		else /* bicycle */
			deflection = - g->noseSteeringAngle;

		skid = wheel_friction(deflection,
			nw_dot.x, nw_dot.y,
			-f.z,
			p->muStatic, p->muKinetic, MU_SKID, /* no brakes */
			&f, "nose");

		/* Convert f (SWC) in FnMu (AXYZ): */
		VReverseTransform_(&f, &AXYZtoSWC, &FnMu);

		if( skid ){
			sounds_playSound(c, sounds_Touchdown, 0);
			prompt_craft_print(c, "WARNING: nose wheel is skidding");
		}

		if( fabs(FnMu.y) > 1.0 * /* FIXME */ p->emptyWeight )
			return "nose gear broken under too high lateral force";

	} else {

		/* Nose wheel isn't in contact with ground: */

		VSetPoint(&FnMu, 0, 0, 0);
	}

	/* RIGHT GEAR: */

	VSetPoint(&Fr, 0.0, 0.0, 0.0);

	/* Add force Fr.z (AXYZ) due to spring and damper: */
	if( g->rightGroundContact ){
		Fr.z = spring_and_damper_force(0.5*p->Km, 0.5*p->Dm, p->cmMax, re, re_dot);
		if( Fr.z < - 1.5 /* FIXME */ * p->emptyWeight )
			return "right gear collapsed under too high vertical load";
		if( Fr.z == 0.0 )
			g->rightGroundContact = FALSE;
	}

	/* Add friction and braking: */
	if( g->rightGroundContact ){
		VPoint f;
		double fm;

		/* Calculate terrain normal force f (SWC): */
		fm = Fr.z * Fr.z;
		VTransform_(&Fr, &AXYZtoSWC, &f);
		VSetPoint(&f, 0, 0, fm / f.z);

		/* Set actual static force Fr (AXYZ): */
		VReverseTransform_(&f, &AXYZtoSWC, &Fr);

		skid = wheel_friction(0.0,
			rw_dot.x, rw_dot.y,
			-f.z,
			muStatic, muKinetic, MU_SKID,
			&f, "right");

		VReverseTransform_(&f, &AXYZtoSWC, &FrMu);

		if( skid ){
			sounds_playSound(c, sounds_Touchdown, 0);
			prompt_craft_print(c, "WARNING: right wheel is skidding");
		}

		if( fabs(FrMu.y) > 1.0 * /* FIXME */ p->emptyWeight )
			return "right gear broken under too high lateral force";

	} else {

		/* Right wheel isn't in contact with ground: */

		VSetPoint(&FrMu, 0, 0, 0);
	}

	/* LEFT GEAR: */

	VSetPoint(&Fl, 0.0, 0.0, 0.0);

	/* Add force Fl.z (AXYZ) due to spring and damper: */
	if( g->leftGroundContact ){
		Fl.z = spring_and_damper_force(0.5*p->Km, 0.5*p->Dm, p->cmMax, le, le_dot);
		if( Fl.z < - 1.5 /* FIXME */ * p->emptyWeight )
			return "left gear collapsed under too high vertical load";
		if( Fl.z == 0.0 )
			g->leftGroundContact = FALSE;
	}

	/* Add friction and braking: */
	if( g->leftGroundContact ){
		VPoint f;
		double fm;
		
		/* Calculate terrain normal force f (SWC): */
		fm = Fl.z * Fl.z;
		VTransform_(&Fl, &AXYZtoSWC, &f);
		VSetPoint(&f, 0, 0, fm / f.z);

		/* Set actual static force Fl (AXYZ): */
		VReverseTransform_(&f, &AXYZtoSWC, &Fl);

		skid = wheel_friction(0.0,
			lw_dot.x, lw_dot.y,
			-f.z,
			muStatic, muKinetic, MU_SKID,
			&f, "left");

		VReverseTransform_(&f, &AXYZtoSWC, &FlMu);

		if( skid ){
			sounds_playSound(c, sounds_Touchdown, 0);
			prompt_craft_print(c, "WARNING: left wheel is skidding");
		}

		if( fabs(FlMu.y) > 1.0 * /* FIXME */ p->emptyWeight )
			return "left gear broken under too high lateral force";

	} else {

		/* Left wheel isn't in contact with ground: */

		VSetPoint(&FlMu, 0, 0, 0);
	}

/*
 *  Here we sum up all the forces F and F*Mu at every wheel to get the total
 *  force F (AXYZ) applyed to the CM. The result is F (AXYZ).
 */

	F.x = Fn.x + Fl.x + Fr.x + FnMu.x + FlMu.x + FrMu.x;

	F.y = Fn.y + Fl.y + Fr.y + FnMu.y + FlMu.y + FrMu.y;

	F.z = Fn.z + Fl.z + Fr.z + FnMu.z + FlMu.z + FrMu.z;

/*
 *  Every wheel gives its contribute to the total moment M (AXYZ).
 */

	VCrossProd(&nw, &Fn,   &M);
	VCrossProd(&nw, &FnMu, &mt);     VAdd(&M, &mt, &M);
	VCrossProd(&lw, &Fl,   &mt);     VAdd(&M, &mt, &M);
	VCrossProd(&lw, &FlMu, &mt);     VAdd(&M, &mt, &M);
	VCrossProd(&rw, &Fr,   &mt);     VAdd(&M, &mt, &M);
	VCrossProd(&rw, &FrMu, &mt);     VAdd(&M, &mt, &M);

	*gearF = F;
	*gearM = M;

/*
 *  Since we are inside this function, it means we are very close to the
 *  ground. On a tricycle landing gear the tail will drag for pitch greater
 *  than 20 DEG.
 *  
 *  FIXME: that's not very realistic as we should use the parameter TailExtent
 *  of the inventory file to compute the actual maximum angle of rotation. The
 *  roll angle should also be considered, since also wing tips can drag.
 */

	if( (g->noseGroundContact || g->leftGroundContact || g->rightGroundContact)  /* some wheel still in contact */
	&& (p->rn.x > p->rm.x)               /* is bicycle */
	&& (c->curPitch > units_DEGtoRAD(20.0))    /* rotation above 20 DEG */
	) {
		return "rotation above 20 DEG, dragging tail to the runway";
	}
	
	return NULL;
}
