/***************************************************************************

  TITLE:	uiuc_aero	
		
----------------------------------------------------------------------------

  FUNCTION:	aerodynamics, engine and gear model

----------------------------------------------------------------------------

  MODULE STATUS:	developmental

----------------------------------------------------------------------------

  GENEALOGY:		Equations based on Part 1 of Roskam's S&C text

----------------------------------------------------------------------------

  DESIGNED BY:		Bipin Sehgal	
		
  CODED BY:		Bipin Sehgal
		
  MAINTAINED BY:	Bipin Sehgal

----------------------------------------------------------------------------

  MODIFICATION HISTORY:
		
  DATE		PURPOSE												BY
  3/17/00   Initial test release  
  

----------------------------------------------------------------------------

  CALLED BY:

----------------------------------------------------------------------------

  CALLS TO:

----------------------------------------------------------------------------

  INPUTS:	

----------------------------------------------------------------------------

  OUTPUTS:

--------------------------------------------------------------------------*/


#include <math.h>
#include "ls_types.h"
#include "ls_generic.h"
#include "ls_constants.h"
#include "ls_cockpit.h"
#include <FDM/UIUCModel/uiuc_wrapper.h>


void uiuc_aero( SCALAR dt, int Initialize ) 
{
    static int init = 0;

    if (init==0)
    {
      init = -1; 
      uiuc_init_aeromodel();
    }

    uiuc_force_moment(dt);
}


void uiuc_engine( SCALAR dt, int Initialize ) 
{
    uiuc_engine_routine();
}

/* ***********************************************************************
 * Gear model. Exact copy of C172_gear.c. Additional gear models will be
 * added later and the choice of the gear model could be specified at
 * runtime.
 * ***********************************************************************/
static void sub3( DATA v1[],  DATA v2[], DATA result[] )
{
    result[0] = v1[0] - v2[0];
    result[1] = v1[1] - v2[1];
    result[2] = v1[2] - v2[2];
}

static void add3( DATA v1[],  DATA v2[], DATA result[] )
{
    result[0] = v1[0] + v2[0];
    result[1] = v1[1] + v2[1];
    result[2] = v1[2] + v2[2];
}

static void cross3( DATA v1[],  DATA v2[], DATA result[] )
{
    result[0] = v1[1]*v2[2] - v1[2]*v2[1];
    result[1] = v1[2]*v2[0] - v1[0]*v2[2];
    result[2] = v1[0]*v2[1] - v1[1]*v2[0];
}

static void multtrans3x3by3( DATA m[][3], DATA v[], DATA result[] )
{
    result[0] = m[0][0]*v[0] + m[1][0]*v[1] + m[2][0]*v[2];
    result[1] = m[0][1]*v[0] + m[1][1]*v[1] + m[2][1]*v[2];
    result[2] = m[0][2]*v[0] + m[1][2]*v[1] + m[2][2]*v[2];
}

static void mult3x3by3( DATA m[][3], DATA v[], DATA result[] )
{
    result[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2];
    result[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2];
    result[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2];
}

static void clear3( DATA v[] )
{
    v[0] = 0.; v[1] = 0.; v[2] = 0.;
}

void uiuc_gear()
{
char rcsid[] = "$Id$";

  /*
   * Aircraft specific initializations and data goes here
   */
   
#define NUM_WHEELS 3

    static int num_wheels = NUM_WHEELS;		    /* number of wheels  */
    static DATA d_wheel_rp_body_v[NUM_WHEELS][3] =  /* X, Y, Z locations */
    {
	{ 10.,  0., 4. },				/* in feet */
	{ -1.,  3., 4. }, 
	{ -1., -3., 4. }
    };
    static DATA spring_constant[NUM_WHEELS] =	    /* springiness, lbs/ft */
	{ 1500., 5000., 5000. };
    static DATA spring_damping[NUM_WHEELS] =	    /* damping, lbs/ft/sec */
	{ 100.,  150.,  150. };		
    static DATA percent_brake[NUM_WHEELS] =	    /* percent applied braking */
	{ 0.,  0.,  0. };			    /* 0 = none, 1 = full */
    static DATA caster_angle_rad[NUM_WHEELS] =	    /* steerable tires - in */
	{ 0., 0., 0.};				    /* radians, +CW */	
  /*
   * End of aircraft specific code
   */
    
  /*
   * Constants & coefficients for tyres on tarmac - ref [1]
   */
   
    /* skid function looks like:
     * 
     *           mu  ^
     *               |
     *       max_mu  |       +		
     *               |      /|		
     *   sliding_mu  |     / +------	
     *               |    /		
     *               |   /		
     *               +--+------------------------> 
     *               |  |    |      sideward V
     *               0 bkout skid
     *	               V     V
     */
  
  
    static DATA sliding_mu   = 0.5;	
    static DATA rolling_mu   = 0.01;	
    static DATA max_brake_mu = 0.6;	
    static DATA max_mu	     = 0.8;	
    static DATA bkout_v	     = 0.1;
    static DATA skid_v       = 1.0;
  /*
   * Local data variables
   */
   
    DATA d_wheel_cg_body_v[3];		/* wheel offset from cg,  X-Y-Z	*/
    DATA d_wheel_cg_local_v[3];		/* wheel offset from cg,  N-E-D	*/
    DATA d_wheel_rwy_local_v[3];	/* wheel offset from rwy, N-E-U */
    DATA v_wheel_body_v[3];		/* wheel velocity,	  X-Y-Z	*/
    DATA v_wheel_local_v[3];		/* wheel velocity,	  N-E-D	*/
    DATA f_wheel_local_v[3];		/* wheel reaction force,  N-E-D	*/
    DATA temp3a[3], temp3b[3], tempF[3], tempM[3];	
    DATA reaction_normal_force;		/* wheel normal (to rwy) force	*/
    DATA cos_wheel_hdg_angle, sin_wheel_hdg_angle;	/* temp storage */
    DATA v_wheel_forward, v_wheel_sideward,  abs_v_wheel_sideward;
    DATA forward_mu, sideward_mu;	/* friction coefficients	*/
    DATA beta_mu;			/* breakout friction slope	*/
    DATA forward_wheel_force, sideward_wheel_force;

    int i;				/* per wheel loop counter */
  
  /*
   * Execution starts here
   */
   
    beta_mu = max_mu/(skid_v-bkout_v);
    clear3( F_gear_v );		/* Initialize sum of forces...	*/
    clear3( M_gear_v );		/* ...and moments		*/
    
  /*
   * Put aircraft specific executable code here
   */
   
    percent_brake[1] = 0.; /* replace with cockpit brake handle connection code */
    percent_brake[2] = percent_brake[1];
    
    caster_angle_rad[0] = 0.03*Rudder_pedal;
    
    for (i=0;i<num_wheels;i++)	    /* Loop for each wheel */
    {
	/*========================================*/
	/* Calculate wheel position w.r.t. runway */
	/*========================================*/
	
	    /* First calculate wheel location w.r.t. cg in body (X-Y-Z) axes... */
	
	sub3( d_wheel_rp_body_v[i], D_cg_rp_body_v, d_wheel_cg_body_v );
	
	    /* then converting to local (North-East-Down) axes... */
	
	multtrans3x3by3( T_local_to_body_m,  d_wheel_cg_body_v, d_wheel_cg_local_v );
	
	    /* Runway axes correction - third element is Altitude, not (-)Z... */
	
	d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* since altitude = -Z */
	
	    /* Add wheel offset to cg location in local axes */
	
	add3( d_wheel_cg_local_v, D_cg_rwy_local_v, d_wheel_rwy_local_v );
	
	    /* remove Runway axes correction so right hand rule applies */
	
	d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* now Z positive down */
	
	/*============================*/
	/* Calculate wheel velocities */
	/*============================*/
	
	    /* contribution due to angular rates */
	    
	cross3( Omega_body_v, d_wheel_cg_body_v, temp3a );
	
	    /* transform into local axes */
	  
	multtrans3x3by3( T_local_to_body_m, temp3a, temp3b );

	    /* plus contribution due to cg velocities */

	add3( temp3b, V_local_rel_ground_v, v_wheel_local_v );
	
	
	/*===========================================*/
	/* Calculate forces & moments for this wheel */
	/*===========================================*/
	
	    /* Add any anticipation, or frame lead/prediction, here... */
	    
		    /* no lead used at present */
		
	    /* Calculate sideward and forward velocities of the wheel 
		    in the runway plane					*/
	    
	cos_wheel_hdg_angle = cos(caster_angle_rad[i] + Psi);
	sin_wheel_hdg_angle = sin(caster_angle_rad[i] + Psi);
	
	v_wheel_forward  = v_wheel_local_v[0]*cos_wheel_hdg_angle
			 + v_wheel_local_v[1]*sin_wheel_hdg_angle;
	v_wheel_sideward = v_wheel_local_v[1]*cos_wheel_hdg_angle
			 - v_wheel_local_v[0]*sin_wheel_hdg_angle;

	    /* Calculate normal load force (simple spring constant) */
	
	reaction_normal_force = 0.;
	if( d_wheel_rwy_local_v[2] < 0. ) 
	{
	    reaction_normal_force = spring_constant[i]*d_wheel_rwy_local_v[2]
				  - v_wheel_local_v[2]*spring_damping[i];
	    if (reaction_normal_force > 0.) reaction_normal_force = 0.;
		/* to prevent damping component from swamping spring component */
	}
	
	    /* Calculate friction coefficients */
	    
	forward_mu = (max_brake_mu - rolling_mu)*percent_brake[i] + rolling_mu;
	abs_v_wheel_sideward = sqrt(v_wheel_sideward*v_wheel_sideward);
	sideward_mu = sliding_mu;
	if (abs_v_wheel_sideward < skid_v) 
	    sideward_mu = (abs_v_wheel_sideward - bkout_v)*beta_mu;
	if (abs_v_wheel_sideward < bkout_v) sideward_mu = 0.;

	    /* Calculate foreward and sideward reaction forces */
	    
	forward_wheel_force  =   forward_mu*reaction_normal_force;
	sideward_wheel_force =  sideward_mu*reaction_normal_force;
	if(v_wheel_forward < 0.) forward_wheel_force = -forward_wheel_force;
	if(v_wheel_sideward < 0.) sideward_wheel_force = -sideward_wheel_force;
	
	    /* Rotate into local (N-E-D) axes */
	
	f_wheel_local_v[0] = forward_wheel_force*cos_wheel_hdg_angle
			  - sideward_wheel_force*sin_wheel_hdg_angle;
	f_wheel_local_v[1] = forward_wheel_force*sin_wheel_hdg_angle
			  + sideward_wheel_force*cos_wheel_hdg_angle;
	f_wheel_local_v[2] = reaction_normal_force;	  
	   
	    /* Convert reaction force from local (N-E-D) axes to body (X-Y-Z) */
	
	mult3x3by3( T_local_to_body_m, f_wheel_local_v, tempF );
	
	    /* Calculate moments from force and offsets in body axes */

	cross3( d_wheel_cg_body_v, tempF, tempM );
	
	/* Sum forces and moments across all wheels */
	
	add3( tempF, F_gear_v, F_gear_v );
	add3( tempM, M_gear_v, M_gear_v );
	
    }
}