/*  
***************************************************************************
**
** 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"
#define NNAMES 1
#define N_OUTPUTS 15
#define N_INPUTS 5
#define N_STATES 6
#define PI 3.1415926535898

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

/*
** Constant Parameters.  See run.m for comments and values.
*/
static real_T MSwayInv[DIM_M][DIM_M], P[NPARAMS];
/*
** Variables:
*/                      
static real_T beta=0.0, betaR=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;

#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++)
      {
	MSwayInv[i][j] = *(mxGetPr(GetParam(0)) + index++);
        printf(" MSwayInv [%d][%d] = %e\n", i, j, MSwayInv[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=massIx; i<NPARAMS; i++)*/
    for( i=0; i<NPARAMS; i++)  
    {
      P[i] = *(mxGetPr(GetParam(0)) + i);
      printf(" P[%d] = %e;\n", i, P[i]);
    }
/*
**  Initial state vector is zero, except for the forward speed.
*/
    *x0 = P[velIx];
    for (lp=1;lp<N_STATES;lp++) { 
        x0[lp] = 0.0; 
    }
/*
**  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);
 
    y[0] = x[1];     /* Drift rate, v */
    y[1] = x[2];     /* Yaw rate, r */
    y[2] = beta;     /* Drift angle at CG */
    y[3] = betaR;    /* Drift angle at Ring CP */
    y[4] = AofA;     /* Angle of attack on the ring */
    y[5] = TrqNTC;   /* Torque exerted by the tailcone about the vehicle CG */
    y[6] = TrqP;     /* Torque exerted by the tailcone about the pivot point */
    y[7] = x[3];     /* x position in the Earth Fixed Frame */
    y[8] = x[4];     /* y position in the Earth Fixed Frame */
    y[9] = x[5];     /* yaw angle wrto the Earth Fixed Frame */
    y[10] = FYR;     /* Rudder force resolved to Body y Axis */
    y[11] = FYT;     /* Thrust resolved to Body y Axis */
    y[12] = FRA;     /* Force exerted by the tailcone on the rudder actuator */
    y[13] = U(0);    /* BEWARE: U is a #define above.  This is deltaR */
    y[14] = x[0];    /* Forward speed u */
}


#define MDL_DERIVATIVES
/* Function: mdlDerivatives =================================================
 */
static void mdlDerivatives(SimStruct *S)
{
    real_T            *dx   = ssGetdX(S);
    real_T            *x    = ssGetContStates(S);
    real_T            FY, FX, TrqN, u, u1, v, r, FYTC, CL, CD, Lift, Drag;
    real_T            magVelR, magVelR2, Tp, TrqNT, TrqNR, psi, deltaR;
    real_T            k1, k2, num, sinGamR, rudActDist;
    InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);
/*
**  Conveniently rename some things:
*/
    u      = x[0];          /* Extract some of the states */
    v      = x[1];
    r      = x[2];
    psi    = x[5];
    deltaR = U(0);          /* Careful; U is a #define, above, not an array */
    Tp     = U(2);
/*
**  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 */
    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 */

#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 Sway/Yaw EOM.  See notes, p11, 99/2/23.
**  This is the sum of all forces and torques which are functions of the state.
*/
    FY   = -P[massIx]*u*r + P[YvIx]*u*v + P[YrIx]*u*r + 
            P[YvabsvIx]*v*fabs(v) + P[YvabsrIx]*v*fabs(r) + FYTC;
    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) + TrqNTC;
/*
**  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].
*/
    u1 = x[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) + FXTC;
/*
**  Multiply by the mass matrix inverse to get accelerations
*/
    /* xdot=f(x,u) */
    dx[0] = FX/P[MSurgeIx];
    dx[1] = MSwayInv[0][0]*FY  +  MSwayInv[0][1]*TrqN;   /* v dot */
    dx[2] = MSwayInv[1][0]*FY  +  MSwayInv[1][1]*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[3] = cos(psi)*u - sin(psi)*v + U(3);  /* x dot in Earth Fixed */
    dx[4] = sin(psi)*u + cos(psi)*v + U(4);  /* y dot in Earth Fixed */
    dx[5] = r;                               /* yaw dot wrto E. Fixed*/
}
/* 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