/*  
***************************************************************************
**
** PURPOSE: Sway/Yaw Dynamics for the Altex Vehicle.
** AUTHOR:  Rob McEwen
** DATE:    99/2/23
**
** NOTES: 
** 1) All units are in SI (mks & radians) unless otherwise noted.
**
***************************************************************************
*/

#define S_FUNCTION_NAME VehDyn
#define S_FUNCTION_LEVEL 2

#include "simstruc.h"
#include "math.h"
#include "VehDyn.h"
#include "matrixMath.h"
#define NNAMES 1
#define N_OUTPUTS 24
#define N_INPUTS 5
#define N_STATES 12
#define PI 3.1415926535898

#define GetParam(num) (ssGetSFcnParam(S,num))
#define U(element) (*uPtrs[element])  /* Pointer to Input Port0 */


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);

char *name[] =

           {  "MassIx",
	      "IxxIx",
	      "IyyIx",
	      "IzzIx",
	      "IxyIx",
	      "IyzIx",
	      "IzxIx",        
              "WIx",
              "xGIx",
              "yGIx",
              "zGIx",
              "BIx",
              "xBIx",
              "yBIx",
              "zBIx",
              "MSurgeIx",
              "velIx",
              "XrvIx",
              "XrrIx",
              "XuuIx",
              "XvvIx",
              "XqqIx",
              "XwqIx",
              "XwwIx",
              "YvIx",
              "YrIx",
              "YwpIx",
              "YrabsrIx",
              "YvabsrIx",
              "YvabsvIx",
              "ZqIx",
              "ZwIx",
              "ZvpIx",
              "ZqabsqIx",
              "ZwabswIx",
              "KpabspIx",
              "MqIx",
              "MwIx",
              "MprIx",
              "MqabsqIx",
              "MwabswIx",
              "NvIx",
              "NrIx",
              "NpqIx",
              "NrabsvIx",
              "NvabsvIx",
              "NrabsrIx",
              "DecoupleIx",
              "xRIx",         /* Vehicle CG to ring CP; < 0 in Body Coord     */
              "xPIx",         /* Veh CG to elevator pivot; < 0 in Body Coord  */
              "xPRIx",        /* Dist frm Elev pivot to rudder pivot, < 0     */
              "r_Ro_S1_R0Ix", /* Do not insert any other params in this block;*/
              "r_Ro_S1_R1Ix", /* these are assumed by the code to be in this  */
              "r_Ro_S1_R2Ix", /* order.                                       */
              "r_Ro_S2_R0Ix",
              "r_Ro_S2_R1Ix",
              "r_Ro_S2_R2Ix",
              "r_Ro_S3_R0Ix",
              "r_Ro_S3_R1Ix",
              "r_Ro_S3_R2Ix",
              "r_Ro_S4_R0Ix",
              "r_Ro_S4_R1Ix",
              "r_Ro_S4_R2Ix",
              "r_Ro_S5_R0Ix",
              "r_Ro_S5_R1Ix",
              "r_Ro_S5_R2Ix",
              "r_Ro_S6_R0Ix",
              "r_Ro_S6_R1Ix",
              "r_Ro_S6_R2Ix",
              "r_Ro_S7_R0Ix",
              "r_Ro_S7_R1Ix",
              "r_Ro_S7_R2Ix",
              "r_Ro_S8_R0Ix",
              "r_Ro_S8_R1Ix",
              "r_Ro_S8_R2Ix",
              "aRIx",
              "rR1Ix",         /* See notes, 99/4/20 p.15, Eqns. 74 & 75.*/
              "rR2Ix",
              "xRRIx",         /* = xR as defined on p. 13. */
              "dClIx",
              "Cl2Ix",         /* Cdc/Ar */
              "Cd0Ix",
              "Cd2Ix",         /* 1/pi/Ar/Ef */
              "halfRhoSIx",    /* .5*rho*ControlArea */
              "dClRIx",
              "Cl2RIx",        /* CdcR/ArR */
              "Cd0RIx",
              "Cd2RIx",        /* 1/pi/ArR/EfR */
              "halfRhoSRIx",   /* .5*rho*RingArea */
              "StallIx",       /* Strut & Ring stall angle */
              "u0",
              "v0",
              "w0",
              "p0",
              "q0",
              "r0",
              "x0",
              "y0",
              "z0",
              "phi0",
              "theta0",
              "psi0"
          };
/*
** Constant Parameters.  See run.m for comments and values.
*/
static real_T GenMassInv[DIM_M][DIM_M], P[NPARAMS];
/*
** Variables:
*/                      
static real_T beta=0.0, AofA=0.0, TrqNTC=0.0, TrqP=0.0, FYR=0.0;
static real_T FRA=0.0, FYT=0.0, FXR=0.0, FXT=0.0;
static double F_B[3], F_Sr_B[3], Trq_S_Bo_B[3], Trq_S_Eo_B[3];
static double Trq_Sr_Bo_B[3], alphaR, alphaRud, alphaElv;
/*static double betaE[1]={0.0}, betaRtc=0.0, betaR[1]={0.0};*/
static double betaE=0.0, betaRtc=0.0, betaR=0.0;

#define MDL_CHECK_PARAMETERS
#if defined(MDL_CHECK_PARAMETERS) && defined(MATLAB_MEX_FILE)
  /* Function: mdlCheckParameters ==========================================
   * Abstract:
   *    Validate our parameters to verify they are okay.
   */
  static void mdlCheckParameters(SimStruct *S)
  {
      /* Check 1st parameter: Lower Bound (LB) parameter */
      {
          if (!mxIsDouble(GetParam(0)) ||
              mxGetNumberOfElements(GetParam(0)) != NPARAMS) {
              ssSetErrorStatus(S,"CDynVec must have dimension NPARAMS");
              return;
          }
      }
  }
#endif /* MDL_CHECK_PARAMETERS */

/* Function: mdlInitializeSizes ===============================================
 * Abstract:
 *    The sizes information is used by Simulink to determine the S-function
 *    block's characteristics (number of inputs, outputs, states, etc.).
 */
static void mdlInitializeSizes(SimStruct *S)
{
    ssSetNumSFcnParams(S, NNAMES);  /* Number of expected parameters */
#if defined(MATLAB_MEX_FILE)
    if (ssGetNumSFcnParams(S) == ssGetSFcnParamsCount(S)) {
        mdlCheckParameters(S);
        if (ssGetErrorStatus(S) != NULL) {
            return;
        }
    } else {
        return; /* Parameter mismatch will be reported by Simulink */
    }
#endif
    ssSetNumContStates(S, N_STATES);
    ssSetNumDiscStates(S, 0);

    if (!ssSetNumInputPorts(S, 1)) return;
    /*
    ** S is the data structure, 0 is the input port number, and the third
    ** argument is the port width.
    */
    ssSetInputPortWidth(S, 0, N_INPUTS);
    /*ssSetInputPortWidth(S, 0, DYNAMICALLY_SIZED);*/
    /*ssSetInputPortDirectFeedThrough(S, 0, 1);  */

    if (!ssSetNumOutputPorts(S, 1)) return;
    ssSetOutputPortWidth(S, 0, N_OUTPUTS);

    ssSetNumSampleTimes(S, 1);
    ssSetNumRWork(S, 0);
    ssSetNumIWork(S, 0);
    ssSetNumPWork(S, 0);
    ssSetNumModes(S, 0);
    ssSetNumNonsampledZCs(S, 0);

    /* Take care when specifying exception free code - see sfuntmpl.doc */
    /*    ssSetOptions(S, SS_OPTION_EXCEPTION_FREE_CODE);*/
}
/* Function: mdlInitializeSampleTimes =========================================
 * Abstract:
 *    Specifiy that we have a continuous sample time.
 */
static void mdlInitializeSampleTimes(SimStruct *S)
{
    ssSetSampleTime(S, 0, CONTINUOUS_SAMPLE_TIME);
    ssSetOffsetTime(S, 0, 0.0);
}



#define MDL_INITIALIZE_CONDITIONS
/* Function: mdlInitializeConditions =========================================
 * Abstract:
 *    Initialize all states to zero.
 */
static void mdlInitializeConditions(SimStruct *S)
{
    real_T *x0 = ssGetContStates(S);
    real_T *y  = ssGetOutputPortRealSignal(S,0);
    int_T  lp;
    int_T  index, i, j;
/*
**  Get Parameters from the stack, going through the Simulink dialog box.
*/
    printf("\n\n Simulation Initialization:\n");
    index = MIx;
    for( i=0; i<DIM_M; i++)
    {
      for( j=0; j<DIM_M; j++)
      {
	GenMassInv[i][j] = *(mxGetPr(GetParam(0)) + index++);
        printf(" GenMassInv [%d][%d] = %e\n", i, j, GenMassInv[i][j]);
      }
    }
/*
**  Load the parameter array P.  P can be most easily indexed by the 
**  enumerated values defined at the top of this file.
*/
    /*for( i=0; i<NPARAMS; i++)*/
    for( i=massIx; i<NPARAMS; i++)
    {
      P[i] = *(mxGetPr(GetParam(0)) + i);
      printf(" P[%s] = %e;\n", name[i-massIx], P[i]);
    }
#if 0
/*
**  Initial state vector is zero, except for the forward speed.
*/
    *x0 = P[velIx];
    for (lp=1;lp<N_STATES;lp++) { 
        x0[lp] = 0.0; 
    }
#endif
/*
**  Initialize state vector at t = 0:
*/
    for ( lp=0; lp<N_STATES; lp++) { 
        x0[lp] = P[ lp + u0 ]; 
        /*printf(" x0[%d] = %f\n", lp, x0[lp]);*/
    }
/*
**  Initial output vector is zero.
*/
    for (lp=0;lp<N_OUTPUTS;lp++) { 
        *y++=0.0; 
    }
}

static real_T T1, T2, T3, T4, T5, T6, FXTC;

/* Function: mdlOutputs =======================================================
 * Abstract:
 *      y = g(x,u)
 */
static void mdlOutputs(SimStruct *S, int_T tid)
{
    real_T            *y    = ssGetOutputPortRealSignal(S,0);
    real_T            *x    = ssGetContStates(S);
    InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);
/*
**  Compute the total force of the control surfaces, resolved on to the
**  body y axis, with out including the vectored thrust.
*/
    FYR = F_B[1] - F_Sr_B[1];
/*
**  Now pass the following items back out to Simulink:
*/

    y[0] = x[1];         /* Drift rate, v */
    y[1] = x[5];         /* Yaw rate, r */
    y[2] = beta;         /* Drift angle at CG */
    y[3] = betaR;        /* Drift angle at Ring CP */
    y[4] = alphaR;       /* Angle of attack on the ring */
    y[5] = Trq_S_Bo_B[2];/* TrqNT: Trq exerted by the tailc. about the veh. CG */
    y[6] = Trq_S_Eo_B[2];/* TrqP Trq exerted by the tailc. about the pivot pnt */
    y[7] = x[6];         /* x position in the Earth Fixed Frame */
    y[8] = x[7];         /* y position in the Earth Fixed Frame */
    y[9] = x[11];        /* yaw angle wrto the Earth Fixed Frame */
    y[10] = FYR;         /* FYR; Rudder force resolved to Body y Axis */
    y[11] = F_Sr_B[1];   /* FYT; Thrust resolved to Body y Axis */
    y[12] = FRA;         /* Force exerted by the tailc. on the rudder actuator*/
    y[13] = U(0);        /* BEWARE: U is a #define above.  This is deltaR */
    y[14] = x[0];        /* Forward speed u */

    y[15] = x[2];        /* Heave rate w    */
    y[16] = x[3];        /* Roll rate p     */
    y[17] = x[4];        /* Pitch rate q    */
    y[18] = x[8];        /* z position in Earth Fixed Frame */
    y[19] = x[9];        /* Roll            */
    y[20] = x[10];       /* Pitch           */
    y[21] = U(1);        /* deltaE          */
    y[22] = alphaRud;    /* Ring angle-of-attack projected into the Rudder xy */
    y[23] = alphaElv;    /* Ring angle-of-attack projected into the Rudder xz */

#if 0

    y[0] = x[0];         /* Forward speed u */
    y[1] = x[1];         /* Drift rate, v */
    y[2] = x[5];         /* Yaw rate, r */
    y[3] = x[6];         /* x position in the Earth Fixed Frame */
    y[4] = x[7];         /* y position in the Earth Fixed Frame */
    y[5] = x[11];        /* yaw angle wrto the Earth Fixed Frame */
    y[6] = beta;         /* Drift angle at CG */
    y[7] = betaR;        /* Drift angle at Ring CP */
    y[8] = alphaR;       /* Angle of attack on the ring */
    y[9] = Trq_S_Bo_B[2];/* TrqNT: Trq exerted by the tailc. about the veh. CG */
    y[10]= Trq_S_Eo_B[2];/* TrqP Trq exerted by the tailc. about the pivot pnt */
    y[11] = FYR;         /* FYR; Rudder force resolved to Body y Axis */
    y[12] = F_Sr_B[1];   /* FYT; Thrust resolved to Body y Axis */
    y[13] = FRA;         /* Force exerted by the tailc. on the rudder actuator*/
    y[14] = U(0);        /* BEWARE: U is a #define above.  This is deltaR */
#endif
}


#define MDL_DERIVATIVES
/* Function: mdlDerivatives =================================================
 */
static void mdlDerivatives(SimStruct *S)
{
    real_T *dx   = ssGetdX(S);
    real_T *x    = ssGetContStates(S);
    real_T FY, FX, FZ, u, u1, v, w, p, q, r, FYTC, CL, CD, Lift, Drag;
    real_T magVelR, magVelR2, Tp, TrqNT, TrqNR, psi, deltaR;
    real_T k1, k2, num, sinGamR, rudActDist, theta, phi, TrqK, TrqM, TrqN;
    double vel_Bo_N_B[3],vel_Bo_N_N[3], om_B_N_B[3], Edot_Om[3][3];
    double Edot[3], deltaE = 0., xN, yN, zN, F_Bo[3], Trq_B[3];
    double TThrust, TTorque, T_N_B[3][3];
    double ds4, dc4, ds5, dc5, ds6, dc6, dt5, ds5dc6, ds5ds6;
    int i, j;

    InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);
/*
**  Conveniently rename some things.  (u, v, w) are the elements of the
**  translational velocity vector vel_Bo_N_B, which is the velocity of
**  the center-of-mass Bo with respect to the inertial frame N, 
**  coordinatized in the body frame B.
**
**  (p, q, r) are elements of the angular velocity vector om_B_N_B, which
**  denotes the velocity of the body frame B wrto the inertial frame N, 
**  coordinatized in the body frame.
**
**  (xN, yN, zN) are the elements of the displacement vector r_Bo_No_N,
**  which is the position of the c.m. Bo wrto the origin of the inertially
**  fixed frame N, coordinatized in the inertial frame N.
**
**  The remaining 3 states are the usual Euler angles.  See below for a
**  description.
*/
    u      = x[0];          
    v      = x[1];
    w      = x[2];
    p      = x[3];
    q      = x[4];
    r      = x[5];
    xN     = x[6];
    yN     = x[7];
    zN     = x[8];
    phi    = x[9];
    theta  = x[10];
    psi    = x[11];
    deltaR = U(0);          /* Careful; U is a #define, above, not an array */
    deltaE = U(1);          /* Careful; U is a #define, above, not an array */
    Tp     = U(2);
/*
**  First define some convenient short variable names:
*/
    ds4 = sin(phi);
    dc4 = cos(phi);
    ds5 = sin(theta);
    dc5 = cos(theta);
    ds6 = sin(psi);
    dc6 = cos(psi);
    dt5 = tan(theta);
    ds5dc6 = ds5 * dc6;
    ds5ds6 = ds5 * ds6;
/*
**  If the Surge - Sway/Yaw "decouple" flag is true, set u to a 
**  constant value P[velIx].
*/
    if( P[DecoupleIx] ) u = P[velIx];
/*
**  Find the magnitude of the velocity.
*/
    magVelR2 = u*u + (v + P[xRIx]*r)*(v + P[xRIx]*r);
    magVelR  = sqrt(magVelR2);
/*
**  Compute the rudder force resolved onto the body x and y axes, FXR and FYR.
**  For the betaR calculation, assume that the tailcone pivot point and the 
**  ring CP are coincident, since the CM-Pivot lever arm is about 6 ft., and 
**  the Pivot-Ring CP is a few inches, and deltaR < 15 Deg.
*/
    beta  = atan(v/u);                          /* Drift angle at the CG */
#if 0
    betaR = asin( (v + P[xRIx]*r)/magVelR );    /* Drift angle at the rudder */
    AofA  = deltaR - betaR;                     /* Ang of attack on the rudder*/
    CL    = P[dClIx]*AofA + P[Cl2Ix]*AofA*fabs(AofA); /* Coeff of lift */
    CD    = P[Cd0Ix] + P[Cd2Ix]*CL*CL;          /* Coeff of drag */
    Lift  = P[halfRhoSIx] * magVelR2 * CL;      /* In the Lift/Drag frame */
    Drag  = P[halfRhoSIx] * magVelR2 * CD;
    FXR   = -Drag*cos(betaR) - Lift*sin(betaR); /* Cvrt frm L/D to B */
    FYR   = -Drag*sin(betaR) + Lift*cos(betaR); /* Cvrt frm L/D to B */
/*
**  Compute the vectored thrust body y component, FYT.  Add it to the rudder
**  component to get the total actuation force.
*/
    FXT  = Tp*cos(deltaR);
    FYT  = Tp*sin(deltaR);
    FXTC = FXR + FXT;                         /* Total tailcone force */
    FYTC = FYR + FYT;
/*
**  Do the same for torque about the vehicle z axis (N component).
**  Here, the Pivot-Ring CP is NOT assumed zero.
*/
    TrqNR = P[xRIx] * FYR;                    /* Torque about CG due to rudder*/
    TrqNT = P[xPIx] * FYT;                    /* Torque about CG due to thrust*/
    TrqNTC= TrqNR + TrqNT;                    /* Total tailcone torque */
    TrqP  = (P[xRIx] - P[xPIx]) * cos(deltaR) * FYR; /* Trq about pivot point */
#endif
/*
**  Compute the forces and torques imparted to the vehicle from the control
**  surfaces (the tailcone).  First, it will be convenient to combine the 
**  individual states into vector form:
*/
    vel_Bo_N_B[0] = u;
    vel_Bo_N_B[1] = v;
    vel_Bo_N_B[2] = w;
    om_B_N_B[0]   = p;
    om_B_N_B[1]   = q;
    om_B_N_B[2]   = r;
    TThrust       = Tp;
    TTorque       = 0.;

    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                          );

/*
**  Rename the torque about the pivot point for the actuator calc. later
*/
    TrqP  = Trq_S_Eo_B[2];                    /* Trq about pivot point */

#if 0
/*
**  Find the force acting on the rudder actuator, FRA, for the 2 actuator case.
**  The Pivot-Ring CP is NOT assumed zero.  This is for the simple case of
**  the rudder actuator base fixed in the body, and the actuator axis 
**  parallel to the vehicle center-line when deltaR = 0.  See Eqn. 35 on p.6
**  of my notes.
*/
    k1 = sin(deltaR) + P[xRIx]/P[aRIx];
    k2 = cos(deltaR) - 1.0;
    num = sin(deltaR)*k1 + cos(deltaR)*k2;
    sinGamR = sqrt(1.0 - num*num/(k1*k1 + k2*k2));
    FRA = -TrqP/P[aRIx]/sinGamR;
#endif

/*
**  Now find the FRA derived in my notes, p. 14, 99/4/20, Eqn. 70.  This has
**  similar geometry to the above except the actuator axis is not necessarily 
**  parallel to the center-line, and the lever arm is not necessarily 
**  perpendicular at deltaR = 0.  See Eqns. 74-77.
**
**  The torque, as derived in Eqn. 63, p.14, is the force the actuator must
**  supply in the direction defined in the diagram.  We would rather see the
**  the load on the actuator, where minus indicates tension, so the sign of
**  Eqn (1) below is flipped.  Eqn. 1a is an approximation of 1.
*/
    k1  = P[rR1Ix]*cos(deltaR) - P[rR2Ix]*sin(deltaR);
    k2  = P[rR1Ix]*sin(deltaR) + P[rR2Ix]*cos(deltaR);
    rudActDist = sqrt( pow(k1 - P[xRRIx], 2) + pow(k2 - P[aRIx], 2) );  
    FRA = -TrqP/P[aRIx]/sin(PI/2. - deltaR);             /* 1a*/
    FRA = -TrqP*rudActDist/( k2*P[xRRIx] - k1*P[aRIx] ); /* 1 */
/*
**  Compute the right-hand side of the Surge EOM.  See notes, p16, 99/3/25.
**  Here u is a state even if the Sway/Yaw EOMS are decoupled, and so 
**  u must not be set to P[velIx].  F_B[0] = FXTC.
*/
 u1 = x[0];

 FX = - ( P[WIx] - P[BIx] )*ds5
      + P[massIx]*( v*r - w*q + P[xGIx]*(q*q + r*r) 
                   - P[yGIx]*p*q - P[zGIx]*p*r      ) 
      + P[XvvIx]*v*v + P[XwwIx]*w*w + P[XrvIx]*r*v + P[XwqIx]*w*q
      + P[XrrIx]*r*r + P[XqqIx]*q*q + P[XuuIx]*u1*fabs(u1) 
      + F_B[0];
#if 0
    FX = P[massIx]*v*r + P[massIx]*P[xGIx]*r*r + P[XvvIx]*v*v + 
         P[XrvIx]*r*v  + P[XrrIx]*r*r + P[XuuIx]*u1*fabs(u1) + F_B[0];
#endif
/*
**  Compute the total roll moment.  See sim.c and Gertler & Hagen, "Standard
**  EOM for Submarine Simulations".
*/
    TrqK = - ( P[zGIx]*P[WIx] - P[zBIx]*P[BIx] )*dc5*ds4
           + ( P[yGIx]*P[WIx] - P[yBIx]*P[BIx] )*dc5*dc4
           - P[massIx]*( P[yGIx]*(v*p - u*q) + P[zGIx]*(w*p - u*r))
           - ( P[IzzIx] - P[IyyIx] )*q*r
           + P[IzxIx]*p*q - P[IxyIx]*p*r + P[IyzIx]*q*q - P[IxyIx]*r*r
           + P[KpabspIx]*p*fabs(p);
           
#if 0

  rhs[4] = -(Zgp*Wp - Zbp*Bp)*cp5*sp4    /* ROLL MOMENTS */
           + (Ygp*Wp - Ybp*Bp)*cp5*cp4
         + Kpabp*pabp
         - (Izz - Iyy)*qr
         + Izx*pq - Ixy*pr + Iyz*qq -Iyz*rr
         - mass*(Ygp*(vp-uq) + Zgp*(wp-ur))
#endif
/*
**  Compute the right-hand side of the Sway/Yaw EOM.  See notes, p11, 99/2/23.
**  This is the sum of all forces and torques which are functions of the state.
**  F_B[1] = FYTC, and Trq_S_Bo_B[2] = TrqNTC.
*/
    FY   =   ( P[WIx] - P[BIx] )*ds4*dc5
	   + P[massIx]*( w*p - u*r + P[yGIx]*(p*p + r*r) 
			- P[xGIx]*p*q - P[zGIx]*q*r      ) 
	   + P[YvIx]*u*v + P[YrIx]*u*r + P[YrabsrIx]*r*fabs(r) + P[YwpIx]*w*p
	   + P[YvabsvIx]*v*fabs(v) + P[YvabsrIx]*v*fabs(r) 
	   + F_B[1];

    TrqN = + ( P[xGIx]*P[WIx] - P[xBIx]*P[BIx] )*dc5*ds4
           + ( P[yGIx]*P[WIx] - P[yBIx]*P[BIx] )*ds5
           - P[massIx]*( P[xGIx]*(u*r - w*p) + P[yGIx]*(w*q - v*r))
           + ( P[IxxIx] - P[IyyIx] )*p*q
           + P[IxyIx]*( p*p - q*q ) + P[IyzIx]*p*r - P[IzxIx]*r*q
           + P[NvIx]*u*v + P[NrIx]*u*r + P[NpqIx]*p*q
           + P[NvabsvIx]*v*fabs(v) + P[NrabsrIx]*r*fabs(r) 
           + P[NrabsvIx]*r*fabs(v)
           + Trq_S_Bo_B[2];
#if 0
    FY   = -P[massIx]*u*r + P[YvIx]*u*v + P[YrIx]*u*r + 
            P[YvabsvIx]*v*fabs(v) + P[YvabsrIx]*v*fabs(r) + F_B[1];
    TrqN = -P[xGIx]*P[massIx]*u*r + P[NvIx]*u*v + 
            P[NrIx]*u*r + P[NrabsvIx]*r*fabs(v) +
            P[NvabsvIx]*v*fabs(v) + P[NrabsrIx]*r*fabs(r) + Trq_S_Bo_B[2];

  rhs[2] = (Wp-Bp)*cp5*sp4 + Yvabv*vabv
      + Yuv*uv + Yur*ur + Yrabr*rabr + Ywp*wp
      + mass*(wp - ur + Ygp*(rr+pp) -Zgp*qr - Xgp*pq)

#endif
/*
**  Finally compute the Heave/Pitch EOMs.  These are from G&H, and from
**  Odyssey/sim.c.
*/
    FZ   =   ( P[WIx] - P[BIx] )*dc4*dc5
	   + P[massIx]*( u*q - v*p + P[zGIx]*(p*p + q*q) 
			- P[xGIx]*p*r - P[yGIx]*q*r      ) 
	   + P[ZqIx]*u*q + P[ZwIx]*u*w + P[ZvpIx]*v*p 
	   + P[ZwabswIx]*w*fabs(w) + P[ZqabsqIx]*q*fabs(q) 
	   + F_B[2];
/*+ P[YvabsrIx]*v*fabs(r) */
    TrqM = - ( P[xGIx]*P[WIx] - P[xBIx]*P[BIx] )*dc5*dc4
           - ( P[zGIx]*P[WIx] - P[zBIx]*P[BIx] )*ds5
           - P[massIx]*( P[zGIx]*(w*q - v*r) + P[xGIx]*(u*q - v*p))
           + ( P[IzzIx] - P[IxxIx] )*p*r
           + P[IzxIx]*( r*r - p*p ) - P[IyzIx]*p*q + P[IxyIx]*r*q
           + P[MqIx]*u*q + P[MwIx]*u*w + P[MprIx]*p*r
           + P[MwabswIx]*w*fabs(w) + P[MqabsqIx]*q*fabs(q) 
           + Trq_S_Bo_B[1];
/*
**  Now, stack up the forces and torques into vectors:
**
**  F_Bo is the resultant force acting at Bo, the vehicle CM.  Trq_B is
**  the total external about the vehicle CM.  Both are coordinatized in 
**  the vehicle frame, B.
*/
    F_Bo[0]  = FX;
    F_Bo[1]  = FY;
    F_Bo[2]  = FZ;
    Trq_B[0] = TrqK;
    Trq_B[1] = TrqM;
    Trq_B[2] = TrqN;
/*
**  Multiply by the mass matrix inverse to get accelerations.  I've hoked up
**  the following multiply (beware j<DIM_M/2) to accomodate separate F and
**  Trq vectors, rather than one vector of dim 6.
*/
    /* xdot=f(x,u) */
   for( i=0; i<DIM_M; i++ )
   {
     dx[i] = 0.0;
     for( j=0; j<DIM_M/2; j++ ) 
     {
       dx[i] += GenMassInv[i][j]  *F_Bo[j];
       dx[i] += GenMassInv[i][j+3]*Trq_B[j];
     }
   }
#if 0
    dx[0] = GenMassInv[1][1]*FX;                             /* u dot */
    dx[1] = GenMassInv[1][1]*FY  +  GenMassInv[1][5]*TrqN;   /* v dot */
    dx[2] = 0.;
    dx[3] = 0.;
    dx[4] = 0.;
    dx[5] = GenMassInv[5][1]*FY  +  GenMassInv[5][5]*TrqN;   /* r dot */
/*
**  Kinematics.  The first 2 eqns transform v_B_N_B to v_B_N_N, and then
**  add a current, U(3) and U(4), which are coord. in the Earth-Fixed frame.
*/
    dx[6] = cos(psi)*u - sin(psi)*v + U(3);  /* x dot in Earth Fixed */
    dx[7] = sin(psi)*u + cos(psi)*v + U(4);  /* y dot in Earth Fixed */
    dx[8] = 0.;
    dx[9] = 0.;
    dx[10]= 0.;
    dx[11]= r;                               /* yaw dot wrto E. Fixed*/
#endif
/*
**  KINEMATICS:  
**
**  Form the direction cosine matrix that takes a vector in the body frame
**  B into the inertial reference frame, N.  This is a 3-2-1 Euler (body-fixed)
**  right-handed rotation sequence.  All frames are initially coincident.
*/
    T_N_B[0][0] =  dc5 * dc6;
    T_N_B[0][1] = -dc4 * ds6 + ds4 * ds5dc6;
    T_N_B[0][2] =  ds4 * ds6 + dc4 * ds5dc6;

    T_N_B[1][0] =  dc5 * ds6;
    T_N_B[1][1] =  dc4 * dc6 + ds4 * ds5ds6;
    T_N_B[1][2] = -ds4 * dc6 + dc4 * ds5ds6;

    T_N_B[2][0] = -ds5;
    T_N_B[2][1] =  dc5 * ds4;
    T_N_B[2][2] =  dc5 * dc4;
/*
**  Form the matrix that produces the Euler rates from the angular velocity 
**  vector.  This is NOT a unitary direction cosine matrix.
*/
    Edot_Om[0][0] =  1.0;
    Edot_Om[0][1] =  ds4 * dt5;
    Edot_Om[0][2] =  dc4 * dt5;

    Edot_Om[1][0] =  0.0;
    Edot_Om[1][1] =  dc4;
    Edot_Om[1][2] = -ds4;

    Edot_Om[2][0] =  0.0;
    Edot_Om[2][1] =  ds4/dc5;
    Edot_Om[2][2] =  dc4/dc5;
/*
**  Recoordinatize the translational velocity from the body frame to the 
**  inertal frame.
*/
    TVMult( vel_Bo_N_N, T_N_B, vel_Bo_N_B );
/*
**  Find the Euler rates.
*/
    TVMult( Edot, Edot_Om, om_B_N_B );
/*
**  Output these to the integrator.
*/
    for( i=0; i<3; i++ )
    {
      dx[i+6] = vel_Bo_N_N[i];
      dx[i+9] = Edot[i];
    }
/*
**  Add in motion due to ocean current
*/
    dx[6] += U(3);  /* x dot in Earth Fixed */
    dx[7] += U(4);  /* y dot in Earth Fixed */

#if 0
    for( i=0; i<12; i++ ) printf(" x[%d] = %f\n", i, x[i] );       /*zz*/
    printf("\n");
#endif

}
/* Function: mdlTerminate =====================================================
 * Abstract:
 *    No termination needed, but we are required to have this routine.
 */
static void mdlTerminate(SimStruct *S)
{
}

#ifdef  MATLAB_MEX_FILE    /* Is this file being compiled as a MEX-file? */
#include "simulink.c"      /* MEX-file interface mechanism */
#else
#include "cg_sfun.h"       /* Code generation registration function */
#endif