/*
** PURPOSE: Compute the force and torque produced by the tailcone,
**          given the vehicle state vector, and a  rudder (deltaR)
**          and elevator (deltaE) angle.
** AUTHOR:  Rob McEwen
** DATE:    99/7/1
** NOTES:1) This routine is similar to fins(), from the Odyssey
**          simulation.  The main differences are:
**          a) The distances from the body CM (Bo) to each of the 
**          four struts now vary as a function of deltaR and deltaE
**          since the whole tailcone moves wrto the body.
**          b) Lift and Drag from the ring-wing is accounted for.
**          c) The computation of lift and drag is first done in 
**          the rudder frame, since the control surfaces are fixed
**          in that frame.
**       2) All units are SI.
**       3) The notation for the kinematics (distances, velocities, etc.)
**          follows Kane's method.  It is precise, but unfortunately
**          cryptic.  It reads in the following way: v_Ao_B_C is the 
**          (translational) vector velocity of the point Ao with respect to
**          the reference frame B, coordinatized in the reference frame C.
**
**          r_Ao_Bo_C denotes a position vector r, from the point Ao to 
**          the point Bo, coordinatized in the frame C.  
**
**          Trq_Si_Eo_B is the moment of a force bound to point Si, about 
**          the point Eo, coordinatized in the frame B.  Trq_B is a torque,
**          coordinatized in frame B, with out regard to the creating force.
**
**          F_B is an unbound force coordinatized in frame B.
**
**       4) The struts and ring are rigidly attached to the tail-
**          cone body.
**       5) The tailcone orientation is determined by two 
**          successive body-fixed, right-handed, Euler Angle 
**          rotations, with elevator first.
**       6) Variables are passed in through the argument list.  Parameters
**          are passed in the array P.
**          
**       7) The upper vertical strut is #1, port horizontal is #2,
**          lower vertical is #3, and starboard horizontal is #4.
**
**       8) The last strut number (5) is the ring.
**
**          gcc -lm tailcone.c matrixMath.c
*/
#include <stdio.h>
#include <math.h>
#include "matrixMath.h"
#include "VehDyn.h"
#define SMALL  1.E-6
#define R2    1.41421356237310                /* Sqrt of 2 */

/*
** StrutType has to match the aggregate initialization 
** of T_S_R below.
*/
enum StrutType {Top, TopPort, Port, BottomPort, Bottom, 
                BottomStarboard, Starboard, TopStarboard, 
                Ring, NSTRUTS};
#if 0
enum StrutType {Top, Port, Bottom, Starboard, Ring, NSTRUTS};
#endif

void tailcone( double vel_Bo_N_B[3],  double om_B_N_B[3],
               double deltaR,         double deltaE,
               double TThrust,        double TTorque,
               double F_B[3],         double F_Sr_B[3],
               double betaR,          double betaE,
               double Trq_S_Bo_B[3],  double Trq_S_Eo_B[3], 
               double Trq_Sr_Bo_B[3], double *alphaR,
/*             double *alphaRud,      double *alphaElv, */
               double *P)
{
   double alpha[NSTRUTS], mag, mag2, CD, CL, Lift, Drag;
   int i, j;
   enum StrutType strut;
/*
** In this application, Si is the i^th
** strut, where the i is an index.  All vectors are of dimension 3 (x,y,z).
*/
   double r_Ro_S_E[NSTRUTS][3], 
          r_Ro_S_R[NSTRUTS][3],
          r_Eo_S_E[NSTRUTS][3],
          r_Eo_Ro_E[3],
          r_Bo_Eo_B[3],
          r_Eo_S_B[NSTRUTS][3],
          r_Bo_S_B[NSTRUTS][3],
          r_temp[NSTRUTS][3],
          vel_S_N_B[NSTRUTS][3],
          vel_S_N_R[NSTRUTS][3],
          vel_S_N_S[NSTRUTS][3],
          F_Si_S[NSTRUTS][3],
          F_Si_B[NSTRUTS][3],
          Trq_Si_Bo_B[NSTRUTS][3],
          Trq_Si_Eo_B[NSTRUTS][3],
          Trq_R[3],
          Trq_B[3];

   double T_B[3], T_R[3] = {0., 0., 0.};
   /*
   ** Direction Cosine Mat.'s 
   */
   double T_R_E[3][3], T_E_B[3][3], T_R_B[3][3], T_B_R[3][3];  
   double T_E_R[3][3], T_B_E[3][3], T_B_S[3][3], T_S_B[3][3];
/*
** Initialize the DC's that go from the rudder frame to the i th strut.  See
** Figure 14. on p.23 of my notes.  This defines the numbering order of the 
** struts.
*/
   double T_S_R[NSTRUTS][3][3] = 
          { 
	       { { 1.,      0.,       0. },   /* T_S1_R */
		 { 0.,      1.,       0. },
		 { 0.,      0.,       1. } },

	       { { 1.,      0.,       0. },   /* T_S2_R -pi/4 roll  */
		 { 0.,    1./R2,  -1./R2 },
		 { 0.,    1./R2,   1./R2 } },

	       { { 1.,      0.,       0. },   /* T_S3_R; -pi/2 roll */
		 { 0.,      0.,      -1. },
		 { 0.,      1.,       0. } },

	       { { 1.,      0.,       0. },   /* T_S4_R -3pi/4 roll */
		 { 0.,   -1./R2,  -1./R2 },
		 { 0.,    1./R2,  -1./R2 } },

	       { { 1.,      0.,       0. },   /* T_S5_R;  pi roll   */
		 { 0.,     -1.,       0. },
		 { 0.,      0.,      -1. } },

	       { { 1.,      0.,       0. },   /* T_S6_R  3pi/4 roll */
		 { 0.,   -1./R2,   1./R2 },
		 { 0.,   -1./R2,  -1./R2 } },

	       { { 1.,      0.,       0. },   /* T_S7_R;  pi/2 roll */
		 { 0.,      0.,       1. },
		 { 0.,     -1.,       0. } },

	       { { 1.,      0.,       0. },   /* T_S8_R   pi/4 roll */
		 { 0.,    1./R2,   1./R2 },
		 { 0.,   -1./R2,   1./R2 } },

	       { { 1.,      0.,       0. },   /* T_S9_R This will be*/
		 { 0.,      1.,       0. },   /* overwritten by the */
		 { 0.,      0.,       1. } }, /* ring calc.         */
          };
#ifdef TESTTC
   printf(" The Trans. from R to Si are:\n");
   for( i=0; i<NSTRUTS; i++ )
   {
       printMat(T_S_R[i]);
       printf(" \n");
   }
#endif
/*
** Unpack the vectors that go from Ro to each strut:
*/
   for( strut=Top; strut<NSTRUTS; strut++ )
   {
     if( strut != Ring )
     {
#ifdef TESTTC
       printf(" strut = %d\n", strut);
#endif
       for( j=0; j<3; j++ ) 
       {
	   r_Ro_S_R[strut][j] = P[ (r_Ro_S1_R0Ix + 3*strut) + j ];
       }
     }
     else
     {
#ifdef TESTTC
       printf(" Set Ring Params; strut = %d\n", strut);
#endif
       r_Ro_S_R[strut][0] = 0.;
       r_Ro_S_R[strut][1] = 0.;
       /*
       ** r_Ro_S_R is the distance from the rudder pivot to the 
       ** ring center, coordinatized in R.
       */
       r_Ro_S_R[strut][2] = P[xRIx] - P[xPRIx] - P[xPIx];
     }
   }
   r_Eo_Ro_E[0] = P[xPRIx];
   r_Eo_Ro_E[1] = 0.;
   r_Eo_Ro_E[2] = 0.;
   r_Bo_Eo_B[0] = P[xPIx];
   r_Bo_Eo_B[1] = 0.;
   r_Bo_Eo_B[2] = 0.;
#ifdef TESTTC
   for( i=0; i< NSTRUTS-1; i++ )
   {
       printf(" r_Ro_S_R[%i] = \n",i);
       for( j=0; j<3; j++ )
       {
	   printf(" %10f\n", P[ (r_Ro_S1_R0Ix + 3*i) + j ]);
       }
   }
   if( Ring < NSTRUTS )
   {
     printf(" r_Ro_S_R[%i] = \n",i);
     for( j=0; j<3; j++ )
     {
       printf(" %10f\n", r_Ro_S_R[Ring][j]);
     }
     
   }
#endif

/*                 
** Form the Direction Cosine Matrices T_B/E and T_E/R.  This notation means
** that T_A_B is a DC matrix that takes a vector coordinatized in the B frame
** and recoordinatizes it in the A frame.
*/
   TPitch(  T_E_B, deltaE);
   TYaw  (  T_R_E, deltaR);
   TTMult(  T_R_B, T_R_E, T_E_B );
   TTransp( T_B_E, T_E_B );
   TTransp( T_E_R, T_R_E );
   TTransp( T_B_R, T_R_B );      /* To be used in the TTorque computation. */

#ifdef TESTTC
   printf(" The Transformation from E to R is:\n");
   printMat(T_R_E);
   printf(" \n");
   printf(" The Transformation from B to R is:\n");
   printMat(T_R_B);
   printf(" \n");
   printf(" The Transformation from R to B is:\n");
   printMat(T_B_R);
   printf(" \n");
#endif
/*
** First zero the outputs:
*/
   for( i=0; i<DIM; i++ )
   {
       F_B[i]        = 0.0;
       Trq_S_Bo_B[i] = 0.0;
       Trq_S_Eo_B[i] = 0.0;
   }
/*
** Return if there is no translational motion.  Otherwise, Sim-U-Wonder will
** divide by zero.  I'll ignore pure rotational motion for this situation.
*/
   if( Vnorm(vel_Bo_N_B) < SMALL ) return;
/*
** Outer loop, where i is the strut number.
*/
   for( i=0; i<NSTRUTS; i++ )
   {
       /*
       ** Find the distance from the CM (Bo) to the i^th strut, r_Bo_S_B[i],
       ** which depends on deltaR and deltaE. First, find the distance from 
       ** Eo to Si coord. in E, denoted r_Eo_S_E[i].  See  p.22, Eqn. 86.
       ** The vectors r_Ro_S_R[i] are constant for each i.
       */
       TVMult( r_Ro_S_E[i], T_E_R,     r_Ro_S_R[i] );
       Vadd  ( r_Eo_S_E[i], r_Eo_Ro_E, r_Ro_S_E[i] );
       /*
       ** Use r_Eo_S_E[i] to find the distance from Bo to Si, coord. in B,
       ** denoted r_Bo_S_B[i]. See  p.22, Eqn. 88.
       */
       TVMult( r_Eo_S_B[i], T_B_E,     r_Eo_S_E[i] );
       Vadd  ( r_Bo_S_B[i], r_Bo_Eo_B, r_Eo_S_B[i] );
       /*
       ** Now, find the velocity of each strut wrto the fluid, which is
       ** assumed inertially fixed here. Also, approximate 
       ** deltaRdot = deltaEdot = 0.
       */
       Vcross( r_temp[i],    om_B_N_B,  r_Bo_S_B[i] );
       Vadd  ( vel_S_N_B[i], r_temp[i], vel_Bo_N_B  );
       /*
       ** Recoordinatize the velocity into the Strut_i Frame.
       */ 
       TVMult( vel_S_N_R[i], T_R_B,    vel_S_N_B[i] );
       TVMult( vel_S_N_S[i], T_S_R[i], vel_S_N_R[i] );
       /*
       ** Now, recompute T_S_R[Ring], as shown in Eqn.s 96-98 of my notes.
       */
       if( i == Ring )
       {
         /*
	 ** Beware that this mag is the second and third elements.
	 */
         mag2 = pow( vel_S_N_R[i][1], 2. ) + pow( vel_S_N_R[i][2], 2. );
         mag  = sqrt( mag2 );
	 /*
	 ** For the case where the velocity of Si is along the r1 axis, 
	 ** Si and R are coincident and we will use the (zero angle) default 
	 ** value of T_S_R[Ring].  This will fail for reverse motion.
	 */
         if( mag < SMALL ) 
         {
	   T_S_R[i][0][0] = 1.; T_S_R[i][0][1] = 0.; T_S_R[i][0][2] = 0.;
	   T_S_R[i][1][0] = 0.; T_S_R[i][1][1] = 1.; T_S_R[i][1][2] = 0.;
	   T_S_R[i][2][0] = 0.; T_S_R[i][2][1] = 0.; T_S_R[i][2][2] = 1.;
	 }
	 else
         {
	   /*
	   ** First row is r1 in R:
	   */
	   T_S_R[i][0][0] = 1.;
	   T_S_R[i][0][1] = 0.;
	   T_S_R[i][0][2] = 0.;
	   /*
	   ** Second row
	   */
	   T_S_R[i][1][0] = 0.;
	   T_S_R[i][1][1] = vel_S_N_R[i][1]/mag;
	   T_S_R[i][1][2] = vel_S_N_R[i][2]/mag;
	   /*
	   ** Third row is r1 X vSi/norm().
	   */
	   T_S_R[i][2][0] = 0.;
	   T_S_R[i][2][1] =-vel_S_N_R[i][2]/mag;
	   T_S_R[i][2][2] = vel_S_N_R[i][1]/mag;
	 }
#ifdef TESTTC
	 printf(" The Transformation from R to S%d is:\n", i);
	 printMat(T_S_R[i]);
	 printf(" \n");
#endif
	 /*
	 ** Finally, compute the velocity of the Ring Center
	 */
         TVMult( vel_S_N_S[i], T_S_R[i], vel_S_N_R[i] );
       }
#ifdef TESTTC
       printf(" The velocity of strut %d, in S%d, is [%f %f %f]'\n", 
              i, i, vel_S_N_S[i][0], vel_S_N_S[i][1], vel_S_N_S[i][2]);
#endif
       /*
       ** Compute the angle of attack.  The i_th strut by definition lies along
       ** the minus z axis of the i_th strut frame.  mag is the magnitude of
       ** vel_S_N_S[i] projected into the x-y plane of the strut frame.   
       **
       ** The minus sign below is because the velocity is coord in the Si 
       ** frame.  See my Altex notes, Sec. 2, p. 24.
       **
       ** I will ASSUME that the angle of attack, alpha[i], depends only on
       ** the component of the flow that is parallel to the chord.
       ** That is, any flow component along the z axis of the strut frame 
       ** doesn't contribute to the lift.
       */ 
       mag2 = pow( vel_S_N_S[i][0], 2. ) + pow( vel_S_N_S[i][1], 2. );
       mag  = sqrt( mag2 );
       if( mag > SMALL )
       {
	   alpha[i] = -atan2( vel_S_N_S[i][1], vel_S_N_S[i][0] );
       }
       else alpha[i] = 0.0;
#ifdef TESTTC
       printf(" The angle of attack on strut %d is %f\n", i, alpha[i]);
#endif
       /*
       ** Find the lift in the L/D (lift/drag) frame.  This frame has it's
       ** z axis aligned with the Strut Frame, but is rotated so that the
       ** dot product of it's x unit vector with the inertial velocity of 
       ** the point Si (vel_S_N_S[i]) is minimum.  Said simply, the x axis of
       ** the L/D frame is in the direction of the fluid flow at that point.
       ** Bear in mind that the L/D frame at any strut is not likely 
       ** to be oriented the same way as at the other struts.  See page 24,
       ** Figure 15, Sec. 2 of my Altex notes.
       **
       ** I will ASSUME that when the angle of attack is greater than the
       ** stall angle, there is no lift, BUT equation 124 on p.306 of PNA,
       ** 3rd Edition, still holds in this situation.  This is different
       ** than fins(), which assumes that the CL term in 124 is zero when
       ** the surface is stalled, and hence the drag is zero.
       **
       ** The vectored thrust, and reaction torque, are included with the
       ** Ring computation.
       */
       if( i != Ring )
       {
	 CL = P[dClIx]*alpha[i] + P[Cl2Ix]*alpha[i]*fabs(alpha[i]); 
	 CD = P[Cd0Ix] + P[Cd2Ix]*CL*CL;          /* Coeff of drag */
	 if( fabs(alpha[i]) > P[StallIx] ) 
	   Lift = 0.;
	 else 
	   Lift = P[halfRhoSIx] * mag2 * CL;      
	 Drag  = P[halfRhoSIx] * mag2 * CD;
	 /*
	 ** F_Si_S[i] is the force exerted on (a point on) the ith strut, 
	 ** coordinatized in the Si frame.  
	 */
	 F_Si_S[i][0] = -Drag*cos(alpha[i]) + Lift*sin(alpha[i]);
	 F_Si_S[i][1] =  Drag*sin(alpha[i]) + Lift*cos(alpha[i]);
	 F_Si_S[i][2] = 0.;
       }
       else    /* i == Ring */
       {
	 CL = P[dClRIx]*alpha[i] + P[Cl2RIx]*alpha[i]*fabs(alpha[i]); 
	 CD = P[Cd0RIx] + P[Cd2RIx]*CL*CL;          /* Coeff of drag */
	 if( fabs(alpha[i]) > P[StallIx] ) 
  	   Lift = 0.;
	 else 
	   Lift = P[halfRhoSRIx] * mag2 * CL;      
	 Drag  = P[halfRhoSRIx] * mag2 * CD;
	 /*
	 ** F_Si_S[i] is the force exerted on (a point on) the ith strut, 
	 ** coordinatized in the Si frame.  
	 */
	 F_Si_S[i][0] = -Drag*cos(alpha[i]) + Lift*sin(alpha[i]) + TThrust;
	 F_Si_S[i][1] =  Drag*sin(alpha[i]) + Lift*cos(alpha[i]);
	 F_Si_S[i][2] = 0.;
#ifdef TESTTC
         printf(" Trq_B = \n");
         for(j=0; j<3; j++) printf("    %10f\n", Trq_B[j]);
#endif
       }
#ifdef TESTTC
       printf(" Strut %d; Lift = %f\n", i, Lift);
       printf(" Strut %d; Drag = %f\n", i, Drag);
       printf(" The Force of strut %d, in S%d, is [%f %f %f]'\n", 
              i, i, F_Si_S[i][0], F_Si_S[i][1], F_Si_S[i][2]);
#endif
       /*
       ** Recoordinatize in the Body Frame.
       */
       TTMult ( T_S_B,     T_S_R[i], T_R_B    );
       TTransp( T_B_S,     T_S_B              );
       TVMult ( F_Si_B[i], T_B_S,    F_Si_S[i] );
       /*
       ** Compute the torque from each strut about the CM, Bo, and about
       ** the pivot point, Eo.
       **
       ** I'm not going to separately compute the torque about Ro, Trq_Si_Ro.
       ** Later, I will approximate Trq_Si_Ro as Trq_Si_Eo.  The difference in 
       ** location of Eo and Ro is on the order of an inch.  If the force
       ** on the rudder actuator ever does become an issue, I will add code to 
       ** explicitly compute Trq_Si_Ro.
       */
       Vcross( Trq_Si_Bo_B[i], r_Bo_S_B[i], F_Si_B[i] );
       Vcross( Trq_Si_Eo_B[i], r_Eo_S_B[i], F_Si_B[i] );
       /*
       ** Add each contribution to the total. Trq_S_Bo_B is the total torque
       ** of the struts about the point Bo (CM) coord in B.  Trq_Si_Bo_B[i] is
       ** the contribution to that torque from the i th strut.
       */
       Vadd( F_B,        F_B,        F_Si_B[i]  );
       Vadd( Trq_S_Bo_B, Trq_S_Bo_B, Trq_Si_Bo_B[i] );
       Vadd( Trq_S_Eo_B, Trq_S_Eo_B, Trq_Si_Eo_B[i] );

   } /* End loop indexed on strut # */
   /*
   ** Compute the vectored thrust in B, and the resultant torque about Bo, 
   ** alone. This has already been included in F_B and Trq_Si_Bo_B above.
   ** We'll reuse F_Si_S[Ring] for this computation.  Also, We'll multiply 
   ** T_B_R * F_Si_S, which is notationally inconsistant, but correct, 
   ** because in this case the frame S = R.
   */
   F_Si_S[Ring][0] = TThrust;
   F_Si_S[Ring][1] = 0.;
   F_Si_S[Ring][2] = 0.;
   TVMult( F_Si_B[Ring], T_B_R,          F_Si_S[Ring] );
   Vcross( Trq_Sr_Bo_B,  r_Bo_S_B[Ring], F_Si_B[Ring] );
   /*
   ** Copy the thruster force at the ring center, coordinatized in B, to the
   ** output variable F_Sr_B,  
   */
   for( i=0; i<3; i++ ) F_Sr_B[i] = F_Si_B[Ring][i];
   /*
   ** Find the thruster reaction torque in the Rudder Frame first (easy).
   ** Then put it in the B frame.  Positive reaction torque implies that
   ** the propeller spins ccw, when viewed from behind the vehicle, 
   ** looking forward.
   */
   Trq_R[0] = TTorque;                 /* Other components are always 0 */
   TVMult( Trq_B, T_B_R, Trq_R );
   /*
   ** Add in the thruster reaction torque.
   */
   Vadd( Trq_S_Bo_B, Trq_S_Bo_B, Trq_B );
   /*
   ** Compute the beta angles at the ring center.  See Fig.3, p.7 of my notes.
   */
   betaR   = atan2( vel_S_N_B[Ring][1], vel_S_N_B[Ring][0]);
   betaE   = atan2( vel_S_N_B[Ring][2], vel_S_N_B[Ring][0]);
   /*
   ** Copy out the angle-of-attack at the ring center.  The ring reference
   ** frame is defined so that alpha[Ring] is always negative; the minus sign
   ** here is just to make it easier to look at, when plotted!
   */
   *alphaR   = -alpha[Ring];
   /*
   ** Now, compute the angle of attack, projected into the R xy plane, for
   ** the rudder.  Project it into the R xz plane for the elevator.  See
   ** my notes, p.27, Eqn. 105 & 107.  
   **
   ** The angle of attack is defined to be positive as shown in Figs. 17 & 18.
   ** (Same as Odyssey).
   **
   ** Recall that vel_S_N_R[Ring] is the velocity of the center of the ring
   ** (not Ro) wrto N, coordinatized in R.
   */
#ifdef TESTTC
   *alphaRud = -atan2( vel_S_N_R[Ring][1], vel_S_N_R[Ring][0] );
   *alphaElv =  atan2( vel_S_N_R[Ring][2], vel_S_N_R[Ring][0] );
#endif
}          

#ifdef TESTTC
/*
** Unit test this thing!
**
** gcc -lm -I/u/rob/altex/sim -DTESTTC tailcone.c matrixMath.c -o tailcone
*/
#define FTOM .3048
#define PI 3.1415926535898
void main( void )
{
  
  double P[NPARAMS];
  double vel_Bo_N_B[3]; double om_B_N_B[3];
  double deltaR;        double deltaE;
  double TThrust = 0.;  double TTorque = 0.;
  double F_B[3];        double F_Sr_B[3]; 
  double betaR;         double betaE;
  double Trq_S_Bo_B[3]; double Trq_S_Eo_B[3];
  double Trq_Sr_Bo_B[3];double alphaR;
  double alphaRud;      double alphaElv;

  int i;

  /*
  ** The following parameter values are based on the Odyssey sim.
  */
  const double rho = 1.025e3;     /* kg/m^3 density of seawater */
  double r1[4] = {0.0, -1.069, 0.012, -0.5 * FTOM}; /* location on the body */
  double r2[4] = {0.0, -1.069, -0.5 * FTOM, 0.0};
  double r3[4] = {0.0, -1.069 , 0.012, 0.5 * FTOM};
  double r4[4] = {0.0, -1.069, 0.5 * FTOM, 0.0};
  double radius = .1524;
  static const double cdo = 0.0065;         /* Cd from NACA 0015 section */
  static const double ec = 0.90;            /* "Oswald" efficiency factor */
  double aspect_ratio = 4.26;               /* effective aspect ratio */
  double S            = 0.0299;   /* fin area (m^2) */
  double CDc          = 0.723;    /* crossflow Cd, fsh changed from 0.1 */
  double dCL          = 3.392 ;   /* lift coefficient slope */
  
  P[xPIx]  = -1.0691;           /*Dist from CM to Elev. pivot*/
  P[xRIx]  = -1.0691;           /*Dist from CM to Ring Center*/
  P[xPRIx] = 0.;                /*Dist Elv to Rud*/
/*
** r_Ro_Si_R is the distance from the rudder pivot point, Ro, to the CP
** of the i-th strut, Si, coordinatized in the rudder frame R.
*/
  P[ r_Ro_S1_R0Ix ] = 0.;
  P[ r_Ro_S1_R1Ix ] = 0.;
  P[ r_Ro_S1_R2Ix ] = -radius;

  P[ r_Ro_S2_R0Ix ] = 0.;
  P[ r_Ro_S2_R1Ix ] = -radius/R2;
  P[ r_Ro_S2_R2Ix ] = -radius/R2;

  P[ r_Ro_S3_R0Ix ] = 0.;
  P[ r_Ro_S3_R1Ix ] = -radius;
  P[ r_Ro_S3_R2Ix ] = 0.;

  P[ r_Ro_S4_R0Ix ] = 0;
  P[ r_Ro_S4_R1Ix ] = -radius/R2;
  P[ r_Ro_S4_R2Ix ] =  radius/R2;

  P[ r_Ro_S5_R0Ix ] = 0.;
  P[ r_Ro_S5_R1Ix ] = 0.;
  P[ r_Ro_S5_R2Ix ] =  radius;

  P[ r_Ro_S6_R0Ix ] = 0.;
  P[ r_Ro_S6_R1Ix ] =  radius/R2;
  P[ r_Ro_S6_R2Ix ] =  radius/R2;

  P[ r_Ro_S7_R0Ix ] = 0.;
  P[ r_Ro_S7_R1Ix ] =  radius/R2;
  P[ r_Ro_S7_R2Ix ] = 0.;

  P[ r_Ro_S8_R0Ix ] = 0;
  P[ r_Ro_S8_R1Ix ] =  radius/R2;
  P[ r_Ro_S8_R2Ix ] = -radius/R2;

  P[StallIx] = 20.*PI/180.;

  P[dClIx] = dCL;
  P[Cl2Ix] = CDc/aspect_ratio;
  P[Cd0Ix] = cdo;
  P[Cd2Ix] = 1./PI/aspect_ratio/ec;        /* 1/pi/Ar/Ef */
  P[halfRhoSIx] = .5*rho*S;

  P[dClRIx] = dCL;
  P[Cl2RIx] = CDc/aspect_ratio;
  P[Cd0RIx] = cdo;
  P[Cd2RIx] = 1./PI/aspect_ratio/ec;       /* 1/pi/Ar/Ef */
  P[halfRhoSRIx] = .5*rho*2.*S;

  for( i=0; i<3; i++ )
  {
    vel_Bo_N_B[i] = 0.;
    om_B_N_B[i]   = 0.;
  }
  /*
  ** Set the inputs:
  */
  vel_Bo_N_B[0] = 1.5433;  /* 3.0 knots */
  /*vel_Bo_N_B[1] = .15433;*/
  deltaR = .1;
  deltaE = 0.;
  P[halfRhoSIx] = .5*rho*0.;               /* ZERO the strut contribution! */
  /*P[halfRhoSRIx] = .5*rho*0.;*/          /* ZERO the ring contribution! */    
#if 0
  /*
  ** Test vectored thrust and torque
  */
  P[halfRhoSIx]  = .5*rho*0.;              /* ZERO the strut contribution! */
  P[halfRhoSRIx] = .5*rho*0.;              /* ZERO the ring contribution! */    
  TThrust = 1.;
  TTorque = 0.;
#endif
  printf(" The tailcone angle is: deltaR = %f, deltaE = %f\n", deltaR, deltaE);
  tailcone(  vel_Bo_N_B,     om_B_N_B,
               deltaR,        deltaE,
               TThrust,       TTorque,
                 F_B,         F_Sr_B,
                betaR,         betaE,
              Trq_S_Bo_B,    Trq_S_Eo_B,   
              Trq_Sr_Bo_B,     &alphaR,
              &alphaRud,      &alphaElv,
                   P      );
  printf(" F_B = \n");
  for(i=0; i<3; i++) printf("    %10f\n", F_B[i]);
  printf(" Trq_S_Bo_B = \n");
  for(i=0; i<3; i++) printf("    %10f\n", Trq_S_Bo_B[i]);
  printf(" Trq_Sr_Bo_B = \n");
  for(i=0; i<3; i++) printf("    %10f\n", Trq_Sr_Bo_B[i]);
  printf(" Trq_S_Eo_B = \n");
  for(i=0; i<3; i++) printf("    %10f\n", Trq_S_Eo_B[i]);

  printf(" betaR  = %f Radians\n", betaR);
  printf(" betaE  = %f Radians\n", betaE);
  printf(" alphaR = %f Radians\n", alphaR);
  printf(" alphaRud = %f Radians\n", alphaRud);
  printf(" alphaElv = %f Radians\n", alphaElv);
}


#endif