/*
 * SRFFTC  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.
 *
 *  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.10 $  $Date: 2000/05/05 20:16:15 $
 */

#define S_FUNCTION_NAME srfftc

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

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

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

#if defined(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);	        /* number of input arguments */

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


#if defined(MATLAB_MEX_FILE)
#define MDL_GET_INPUT_PORT_WIDTH
static int_T mdlGetInputPortWidth(SimStruct *S, int_T outputPortWidth)
  {
      return(outputPortWidth/2);
  }

#define MDL_GET_OUTPUT_PORT_WIDTH
static int_T mdlGetOutputPortWidth(SimStruct *S, int_T inputPortWidth)
  {
      return(inputPortWidth*2);
  }
#endif


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))];
     *
     * cflip: keeps first element, flips remaining, performs conjugate
     */

  if (ssIsMajorTimeStep(S)) {
    
	int_T    N  = ssGetNumInputs(S); /* 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;
    real_T  *yr, *yi;

    /*
     * 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.
     *
     * NOTE: We leave a "cushion" after the real and imaginary parts:
     *       [ [Real (N/2)]  [Extra (N/2)] [Imag (N/2)] [Extra (N/2)] ]
     */
    {
		UPtrsType uptr = ssGetUPtrs(S);
        
		yr = y;
        yi = y + N;

		if (N==1) {
			*yr = **uptr;
			*yi = 0.0;
			return;
		}

		for (i=N2; i-- > 0; ) {
			*yr++ = **(uptr++);
			*yi++ = **(uptr++);
		}
	}

	/* Compute length N/2 FFT: */
    fft(N2, y, 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);

        real_T *X0 = y;
        real_T *X1 = y+N;

        yr = y;
        yi = y+N;

		
        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]: srfftc.c */