/************************************************************
* Calculate the acceleration shock spectrum of a speed V(t) *
* --------------------------------------------------------- *
* SAMPLE RUN:                                               *
* Input speed: in file speed.dat                            *
* Output shock spectrum in file shocksp1.txt                *
*                                                           *
*                         C++ Version By J-P Moreau, Paris. *
*                                (www.jpmoreau.fr)          *
************************************************************/
#include <basis.h>
#include <vmblock.h>


int      i,iordre,iordre1,ndata,nspec;
FILE     *fp_in, *fp_out;
REAL     df,dzeta,f0,fc,fc1,finf,fmax,fr,fsup,temp,temp1,ts;

REAL     *ACC, *VIT, *S;
void     *vmblock;


	 /************************************************************
     * This subroutine calculates the acceleration of the sismic *
     * mass of an elementary oscillator (1 degree of freedom),   *
     * the basis of which is submitted to a given speed, VIT(t). *
     * The input signal VIT is digitalized with a constant time  *
     * step, TS. The table VIT(I) contains NDATA speed values.   *
     * --------------------------------------------------------- *
     * INPUTS:                                                   *
     *         FREQU........: eigen frequency of oscillator      *
     *         VIT..........: input speed signal of basis VIT(t) *
     *         T ...........: time step of signal (constant)     *
     *         DZETA........: reduced damping factor             *
     *         NDATA........: number of points of signal         *
     * OUTPUT:                                                   *
     *         ACC..........: table containing acceleration res- *
     *                        ponse of oscillator (NDATA points) *
     *                                                           *
     *                         C++ Version By J-P Moreau, Paris. *
     ************************************************************/
    void OSCIL(REAL FREQU,REAL * VIT,REAL * ACC,REAL T,REAL DZETA,int NDATA) {
      REAL ARG,COSARG,DELTA,EXPA,GOMEGA,OMEGA,OMEGA2,SINARG;
      REAL Q0,Q1,Q2,QSI,R0,R1,R2,R3,XPN,XPNP1,YPN,YPPN,YPNP1,YPPNP1;
      int   I;
      OMEGA= (TWO*PI*FREQU);
      OMEGA2=OMEGA*OMEGA;
      DELTA= (sqrt(ONE-DZETA*DZETA));
      EXPA=(exp(-OMEGA*DZETA*T));
      GOMEGA=OMEGA*DELTA;
      ARG=GOMEGA*T;
      SINARG=sin(ARG);
      COSARG=cos(ARG);
      QSI=DZETA/DELTA;
      Q0=(COSARG-QSI*SINARG)*EXPA;
      Q1=OMEGA2*SINARG/GOMEGA*EXPA;
      Q2=((ONE+(QSI*SINARG-COSARG)*EXPA)/T);
      R1=SINARG/GOMEGA*EXPA;
      R0=COSARG*EXPA+DZETA*OMEGA*R1;
      R2=((ONE-DZETA*OMEGA*T)*R1/T-COSARG*EXPA);
      R3= (ONE-R1/T);
      XPNP1=VIT[1];
      YPPNP1=ZERO;
      YPNP1=ZERO;
      ACC[1]=Q2*XPNP1;
                                                                                
      for (I=2; I<NDATA+1; I++) {
        YPN=YPNP1;
        YPPN=YPPNP1;
        XPN=XPNP1;
        XPNP1=VIT[I];
        YPPNP1=Q0*YPPN+Q1*(XPN-YPN)+Q2*(XPNP1-XPN);
        ACC[I]=YPPNP1;
        YPNP1=R0*YPN+R1*YPPN+R2*XPN+R3*XPNP1;
      }                                                                          
    }                                                                       
    
    /*----------------------------------------------------------
    " SEEK MINIMUM AND MAXIMUM OF A TABLE C(I)
    "
    " Inputs:
    "            C(I)    Table with NDATA values of acceleration
    "            NDATA   Number of points of C(I)
    " Outputs:
    "            CMIN    minimum value of table C
    "            CMAX    maximum value of table C
    "---------------------------------------------------------*/	
    void EXTREM(REAL * C,REAL *CMIN,REAL *CMAX,int NDATA) {
      int I;
      *CMIN=ZERO;
      *CMAX=ZERO;
      for (I=1; I<NDATA+1; I++) {
        if (C[I] > *CMAX)  *CMAX=C[I];
        if (C[I] < *CMIN)  *CMIN=C[I];
      }                                                                    
    }                                                                       

    /*------------------------------------------------------------
    " ACCELERATION SHOCK SPECTRUM OF A SPEED
    "
    " Inputs:
    "            VIT(I)  speed of basis (from deconvolution or else)
    "            DZETA   relative damping factor
    "            FMIN    begin frequency of spectrum (<>0)
    "            FMAX    end frequency of spectrum (<=Nyquist)
    "            N       number of points of VIT(I)
    "            T       sampling time of speed 
    "            NFREQ   number of frequencies of spectrum (<=1024)
    " Output:
    "            S(I)    shock spectrum (negative & positive)
    "------------------------------------------------------------*/
    void SPEC(REAL * VIT,REAL DZETA,REAL FMIN,REAL FMAX,int N,REAL T,int NFREQ,REAL * S) {
      REAL DELTAF,F,YMIN,YMAX; int I;
      DELTAF=(FMAX-FMIN)/(NFREQ-1);
      //the algorithm fails if FMIN=0
      if (FMIN < DELTAF)  FMIN=DELTAF;
      for (I=1; I<NFREQ+1; I++) {
        F=FMIN+(I-1)*DELTAF;
        OSCIL(F,VIT,ACC,T,DZETA,N);                                           
        EXTREM(ACC,&YMIN,&YMAX,N);                                              
        S[2*I-1]=YMAX;
        S[2*I]=YMIN;
      }
    }


void main()  {

    fp_out=fopen("shocksp1.txt","w");
    fp_in=fopen("speed.dat","r");
                     
    fprintf(fp_out, "\n Acceleration Shock Spectrum of a speed(T) at Basis\n\n");	

// oscillator damping
    dzeta=0.05;
    fprintf(fp_out," DAMPING OF OSCILLATOR: %5.2f\n",dzeta);
                                                                                
// Number of frequencies for the shock spectrum
    nspec=991;
    fprintf(fp_out," NUMBER OF FREQUENCIES FOR SPECTRUM: %d\n",nspec);
                                                                                
// FREQUENCIES MINI & MAXI OF SPECTRUM
    finf=10.0; fsup=1000.0;                                                
    fprintf(fp_out," BEGIN FREQUENCY OF SPECTRUM: %6.1f Hz\n",finf);
    fprintf(fp_out," END FREQUENCY OF SPECTRUM  : %6.1f Hz\n\n",fsup);

// Read number of points
    fscanf(fp_in,"%d",&ndata);
    
    // dynamic allocations of vectors A,VIT,S
    vmblock = vminit();
    ACC  = (REAL *)vmalloc(vmblock, VEKTOR,  ndata+1, 0);
    VIT  = (REAL *)vmalloc(vmblock, VEKTOR,  ndata+1, 0);
    S    = (REAL *)vmalloc(vmblock, VEKTOR,  ndata+1, 0);
    if (! vmcomplete(vmblock)) {
      LogError ("Memory full !", 0, __FILE__, __LINE__);
      return;
    }	  

    //READ INPUT SPEED
    fprintf(fp_out," %d POINTS READ.\n",ndata);
    fscanf(fp_in,"%lf",&ts);                                                              
    fprintf(fp_out," SAMPLING DURATION: %f S.\n",ts);
    for (i=1; i<ndata+1; i++) {
      fscanf(fp_in," %lf %lf",&temp1,&temp);
      VIT[i]=temp;
    }
    fclose(fp_in);

    fmax= (0.5/ts);
    fprintf(fp_out," NYQUIST FREQUENCY: %6.1f\n",fmax);

// Compute shock spectrum
    SPEC(VIT,dzeta,finf,fsup,ndata,ts,nspec,S);

// print spectrum results to output file
    df=(fsup-finf)/(nspec-1);
    fprintf(fp_out,"\n\n THE ACCELERATION SHOCK SPECTRUM  IS COMPUTED FROM\n");
    fprintf(fp_out," THE FILTERED ACCELERATION IF IT EXISTS, ELSE FROM\n");
    fprintf(fp_out," THE UNFILTERED ACCELERATION.\n\n");
    
    fprintf(fp_out," FREQUENCY STEP OF SHOCK SPECTRUM:\n");
    fprintf(fp_out,"   DF = %6.2f\n\n",df);

    fprintf(fp_out,"   FREQUENCY HZ  SPECTRUM +     SPECTRUM -  IN M/S2\n");
    fprintf(fp_out,"   ------------  -----------    -----------\n");

    for (i=1; i<nspec+1; i++) {
      fr=finf+(i-1)*df;                                                          
      fprintf(fp_out,"   %8.2f    %12.6f    %12.6f\n", fr,S[2*i-1],S[2*i]);
    }
    fprintf(fp_out,"\n End of file shocksp1.txt\n");
    fclose(fp_out);
    printf("\n Results in file shocksp1.txt\n\n");
    free(vmblock);
}
                                                                                      
// End of file shocksp1.cpp