/*
** 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 vector r, from the point Ao to the point Bo, 
**          coordinatized in the frame C.  
**
**          Trq_Si_Eo_B is the torque of point Si about the point Eo, 
**          coordinatized in the frame B.
**
**          F_B is a 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;
**          constants are global.
**       7) The lower vertical strut is #1, port horizontal is #2,
**          upper vertical is #3, and starboard horizontal is #4.
**
**          gcc -lm tailcone.c matrixMath.c
*/
#include <stdio.h>
#include <math.h>
#include "matrixMath.h"
#include "VehDyn.h"
#define NSTRUTS 4
#define SMALL .001
void tailcone( double vel_Bo_N_B[3], double om_B_N_B[3],
               double deltaR,        double deltaE,
               double F_B[3],        double Trq_S_Bo_B[3],
               double Trq_S_Eo_B[3], double *P)
{
   double alpha[NSTRUTS], mag, mag2, CD, CL, Lift, Drag;
   int i, j;
/*
** 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_S_S[NSTRUTS][3],
          F_S_B[NSTRUTS][3],
          Trq_Si_Bo_B[NSTRUTS][3],
          Trq_Si_Eo_B[NSTRUTS][3];

   double T_R_E[3][3], T_E_B[3][3], T_R_B[3][3];  /* Direction Cosine Mat.'s */
   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 1. on p.23 of my notes.
*/
   double T_S_R[NSTRUTS][3][3] = { 
				   { { 1., 0., 0. },   /* T_S1_R;  pi roll   */
				     { 0.,-1., 0. },
				     { 0., 0.,-1. } },
				   { { 1., 0., 0. },   /* T_S2_R; -pi/2 roll */
				     { 0., 0.,-1. },
				     { 0., 1., 0. } },
                                   { { 1., 0., 0. },   /* T_S3_R */
				     { 0., 1., 0. },
				     { 0., 0., 1. } },
				   { { 1., 0., 0. },   /* T_S4_R;  pi/2 roll */
 				     { 0., 0., 1. },
				     { 0.,-1., 0. } } 
                                 };
#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 Bo to each strut:
*/
   for( i=0; i< NSTRUTS; i++ )
   {
       for( j=0; j<3; j++ )
       {
	   r_Ro_S_R[i][j] = P[ (r_Ro_S1_R0Ix + 3*i) + j ];
       }
   }
   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; i++ )
   {
       printf(" r_Ro_S_R[%i] = \n",i);
       for( j=0; j<3; j++ )
       {
	   printf(" %f\n", P[ (r_Ro_S1_R0Ix + 3*i) + 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);
   TRoll (  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 );
/*
** 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;
   }
/*
** 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] );
       /*
       ** 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.   
       **
       ** I will ASSUME that the angle of attack, alpha[i], depends only on
       ** the component of the flow that is perpendicular to the leading edge.
       ** That is, any 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;
       /*
       ** 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.
       **
       ** 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.
       */
       CL    = P[dClIx]*alpha[i] + P[Cl2Ix]*alpha[i]*fabs(alpha[i]); 
       CD    = P[Cd0Ix] + P[Cd2Ix]*CL*CL;          /* Coeff of drag */
       if( alpha[i] > P[StallIx] ) 
         Lift = 0.;
       else 
         Lift = P[halfRhoSIx] * mag2 * CL;      
       Drag  = P[halfRhoSIx] * mag2 * CD;
       /*
       ** F_S_S[i] is the force exerted by the ith strut, coordinatized in
       ** the Si frame.  
       */
       F_S_S[i][0] = -Drag*cos(alpha[i]) - Lift*sin(alpha[i]);
       F_S_S[i][1] = -Drag*sin(alpha[i]) + Lift*cos(alpha[i]);
       F_S_S[i][2] = 0.;
       /*
       ** 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_S_B[i], T_B_S,    F_S_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, add code to 
       ** explicitly compute Trq_Si_Ro.
       */
       Vcross( Trq_Si_Bo_B[i], r_Bo_S_B[i], F_S_B[i] );
       Vcross( Trq_Si_Eo_B[i], r_Eo_S_B[i], F_S_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_S_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 # */
}          

#ifdef TESTTC
/*
** Unit test this thing!
**
** gcc -lm -DTESTTC tailcone.c matrixMath.c -o tailcone
*/
void main( void )
{
    double P[NPARAMS];
    double vel_Bo_N_B[3]; double om_B_N_B[3];
    double deltaR;        double deltaE;
    double F_B[3];        double Trq_S_Bo_B[3];
    double Trq_S_Eo_B[3];

    int i, j;
/*
** The following values are from the sim.
*/
 P[0] = 6.988120e-04;
 P[1] = 0.000000e+00;
 P[2] = 0.000000e+00;
 P[3] = 7.833945e-04;
 P[4] = 7.950000e+02;
 P[5] = 8.548530e+02;
 P[6] = 1.543333e+00;
 P[7] = -1.885500e+00;
 P[8] = -1.809300e+00;
 P[9] = 2.540000e-02;
 P[10] = -7.620000e-02;
 P[11] = 0.000000e+00;
 P[12] = 1.524000e-01;
 P[13] = -7.620000e-02;
 P[14] = -1.524000e-01;
 P[15] = 0.000000e+00;
 P[16] = -7.620000e-02;
 P[17] = 0.000000e+00;
 P[18] = -1.524000e-01;
 P[19] = -7.620000e-02;
 P[20] = 1.524000e-01;
 P[21] = 0.000000e+00;
 P[22] = 6.350000e-02;
 P[23] = 2.540000e-02;
 P[24] = 6.350000e-02;
 P[25] = 2.921000e-01;
 P[26] = 3.392000e+00;
 P[27] = 1.697183e-01;
 P[28] = 6.500000e-03;
 P[29] = 8.302292e-02;
 P[30] = 6.147440e+01;
 P[31] = 5.235988e-01;
 P[32] = 1.269655e+02;
 P[33] = -2.867463e+01;
 P[34] = -2.143193e+01;
 P[35] = 8.843359e+01;
 P[36] = -9.486874e+01;
 P[37] = 5.088000e+01;
 P[38] = 0.000000e+00;
 P[39] = -5.578282e+02;
 P[40] = 0.000000e+00;
 P[41] = -6.320250e+02;
 P[42] = -1.498973e+02;
 P[43] = 0.000000e+00;
 P[44] = 2.734800e+02;
 P[45] = -9.839755e+02;
 P[46] = 0.000000e+00;

 for( i=0; i<3; i++ )
 {
     vel_Bo_N_B[i] = 0.;
     om_B_N_B[i]   = 0.;
 }
 vel_Bo_N_B[0] = 1.5433;  /* 3.0 knots */
 deltaR = 0.;
 deltaE = 0.;

 tailcone(  vel_Bo_N_B,     om_B_N_B,
                deltaR,     deltaE,
                F_B,        Trq_S_Bo_B,
                Trq_S_Eo_B, P);
 printf(" F_B = \n");
 for(i=0; i<3; i++) printf("    %f\n", F_B[i]);
 printf(" Trq_S_Bo_B = \n");
 for(i=0; i<3; i++) printf("    %f\n", Trq_S_Bo_B[i]);
 printf(" Trq_S_Eo_B = \n");
 for(i=0; i<3; i++) printf("    %f\n", Trq_S_Eo_B[i]);
}


#endif