/*
 * SRFFT  Real FFT S-function.
 * DSP Blockset S-Function to perform an FFT on real data.
 * This SIMULINK S-function computes the FFT of the input vector;
 * it is called by the FFT blocks in the DSP Blockset.
 * It computes the length N FFT of real data using a length N/2 
 * complex FFT.
 *
 * For compatibility with DSP Blockset V1.0a only.
 * Superceded by srfftc.c in V2.0.
 *
 *  Note: This file must be linked with an FFT routine that does the core
 *        computation.  The FFT routine itself can be coded in either C or
 *        assembly language.
 *
 *  Copyright 1995-2000 The MathWorks, Inc.
 *  $Revision: 1.15 $  $Date: 2000/05/05 20:16:15 $
 */

#define S_FUNCTION_NAME srfft

#include <string.h>
#include <math.h>

#include "dsp_sim.h"
#include "fft_rt.h"

#define SAMPLE_TIME ssGetArg(S,0)
#define NUM_ARGS    1


#ifdef MATLAB_MEX_FILE
#define MDL_CHECK_PARAMETERS
static void mdlCheckParameters(SimStruct *S) {
    int_T M = mxGetM(SAMPLE_TIME);
    int_T N = mxGetN(SAMPLE_TIME);

    if ( (M != 1) || ((N != 1) && (N != 2)) ) {
        ssSetErrorStatus(S, "The sample time must be a "
                            "scalar or a 2-element row vector");
    }
}
#endif


static void mdlInitializeSizes(SimStruct *S)
{
    ssSetNumSFcnParams(S, NUM_ARGS);

#if defined(MATLAB_MEX_FILE)
    if (ssGetNumSFcnParams(S) == ssGetSFcnParamsCount(S)) {
        mdlCheckParameters(S);
        if (ssGetErrorStatus(S) != NULL) {
            return;
        }
    } else {
        return; /* Simulink will report a parameter mismatch error */
    }
#endif

    ssSetNumInputs(        S, DYNAMICALLY_SIZED);
    ssSetNumOutputs(       S, DYNAMICALLY_SIZED);
    ssSetDirectFeedThrough(S, 1);
    ssSetNumSampleTimes(   S, 1);
    ssSetOptions(          S, SS_OPTION_EXCEPTION_FREE_CODE |
                              SS_OPTION_USING_ssGetUPtrs);
}


static void mdlInitializeSampleTimes(SimStruct *S)
{
    ssSetSampleTimeEvent(S, 0, mxGetPr(SAMPLE_TIME)[0]);
    ssSetOffsetTimeEvent(S, 0, (mxGetN(SAMPLE_TIME) == 2)
                              ? mxGetPr(SAMPLE_TIME)[1] : 0.0);
}


static void mdlInitializeConditions(real_T *x0, SimStruct *S)
{
#ifdef MATLAB_MEX_FILE
  /*
   * Check to make sure input size is a power of 2
   */
  int_T numinputs = ssGetNumInputs(S);
  if (numinputs != -1) {
    int_T dummy;
    if (!(frexp((real_T) numinputs,&dummy) == 0.5))
      ssSetErrorStatus(S,"Width of input to FFT must be a power of 2");
      return;
  }
  if (numinputs == 1) {
    mexWarnMsgTxt("Computing the FFT of a scalar input.\n");
  }
#endif /* MATLAB_MEX_FILE */
}


static void mdlOutputs(real_T *y, const real_T *x, const real_T *u, 
                       SimStruct *S, int_T tid)
{
    /* 
     * Use a length N/2 complex FFT to compute a length N real FFT.
     * Equivalent MATLAB code:
     *    function y=rfft(x)
     *    
     *      N=length(x);
     *      xe = x(1:2:N);
     *      xo = x(2:2:N);
     *      V = fft(xe + j*xo);
     *    
     *      X0 =    1/2*(V+cflip(conj(V)));
     *      X1 = -j*1/2*(V-cflip(conj(V)));
     *    
     *      W=exp(-j*2*pi*(0:N/2-1)/N);
     *      y=X0(:) + W(:).*X1(:);
     *      y=[y; X0(1) - X1(1); conj(y(N/2:-1:2))];
     */

  if (ssIsMajorTimeStep(S)) {
    int_T numinputs = ssGetNumInputs(S);
    int_T N         = numinputs / 2;  /* length of real input vector  */
    int_T N2        = N / 2;          /* length of complex FFT        */
    int_T N4        = N2 / 2;         /* half length of complex FFT   */
    int_T i;

    /*
     * First, copy even index inputs u into real part of outputs y, and
     * odd index inputs u into complex (latter half) of outputs y.
     * FFT operates in-place.
     */
    {
        UPtrsType uptr = ssGetUPtrs(S);
        real_T   *xe   = y;
        real_T   *xo   = y + N;
        for (i = N; i-- != 0; ) {
            *xe++ = **(uptr++);
            *xo++ = **(uptr++);
        }
    }
    
    /*
     * now call fft; it does most of the work
     */
    fft(N2, y, y + N);

    {
        real_T *X0 = y;
        real_T *X1 = y+N;
        real_T *yr = y; 
        real_T *yi = y+N;

        real_T Wr = 1.0;
        real_T Wi = 0.0;
        /* Use "volatile" to fix PPC/MrC optimizer bug */
        volatile real_T theta = -8 * atan(1.0) / N;
        real_T ct = cos(theta);
        real_T st = sin(theta);

        yr[N2] = X0[0] - X1[0];
        yi[N2] = 0.0;

        yr[0]  = X0[0] + X1[0];
        yi[0]  = 0.0;

        for (i = 1; i < N4; i++) {
          real_T X0_1  = X0[i];
          real_T X1_1  = X1[i];
          real_T X0_2  = X0[N2 - i];
          real_T X1_2  = X1[N2 - i];
          real_T X0_1_ =  0.5 * (X0_1 + X0_2);
          real_T X0_2_ =  0.5 * (X1_1 - X1_2);
          real_T X1_1_ =  0.5 * (X1_1 + X1_2);
          real_T X1_2_ = -0.5 * (X0_1 - X0_2);
          real_T Wr_t  = Wr;
          Wr = Wr_t * ct - Wi * st;
          Wi = Wr_t * st + Wi * ct;
          yr[i]      =  X0_1_ + Wr * X1_1_ - Wi * X1_2_;
          yi[i]      =  X0_2_ + Wr * X1_2_ + Wi * X1_1_;
          yr[N2 - i] =  X0_1_ - Wr * X1_1_ + Wi * X1_2_;
          yi[N2 - i] = -X0_2_ + Wr * X1_2_ + Wi * X1_1_;
        }
    
        yr[N4] =  X0[N4];
        yi[N4] = -X1[N4];
    
        for (i = 1; i < N2; i++) {
          yr[i + N2] =  yr[N2 - i];
          yi[i + N2] = -yi[N2 - i];
        }
      }
   }
}


static void mdlUpdate(real_T *x, const real_T *u, SimStruct *S, int_T tid)
{
}


static void mdlDerivatives(real_T *dx, const real_T *x, const real_T *u, 
                           SimStruct *S, int_T tid)
{
}


static void mdlTerminate(SimStruct *S)
{
}

#include "dsp_trailer.c"

/* [EOF]: srfft.c */