/*****************************************************************
* Calculate the acceleration of the sismic mass of an elementary *
* oscillator (1 degree of freedom), the basis of which is submit-*
* ted to a given speed, VIT(t). The input signal VIT is digitali-*
* zed with a constant time step, TS. The table VIT(I) contains   *
* NDATA speed values.                                            *
*                                                                *
*                              C++ Version By J-P Moreau, Paris  *
*                                     (www.jpmoreau.fr)          *
* -------------------------------------------------------------- *
* SAMPLE RUN:                                                    *
*                                                                *
* Read data from file speed.dat                                  *
* Write results to file response.txt.                            *
*****************************************************************/
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <basis.h>
#include <vmblock.h>


FILE *fp_in, *fp_out;
REAL *ACC, *VIT;
int i, NDATA;
REAL dzeta, F0, FMAX, T, TBEGIN, temp, temp1, TEND, TS;
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) *
    *         TS...........: 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;
      }                                                                          
    }                                                                       


void main()  {

    fp_in=fopen("speed.dat","r");
    fp_out=fopen("reponse.txt","w");

    fprintf(fp_out, "\n Mass Response to speed(T) at Basis\n\n");

// Frequency of oscillator
    F0=250;
    fprintf(fp_out," FREQUENCY OF OSCILLATOR: %6.1f HERTZ\n",F0);
                                                                                
// oscillator damping
    dzeta=0.05;
    fprintf(fp_out," DAMPING OF OSCILLATOR: %5.2f\n",dzeta);
                                                                                
// Read input speed
    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);
    if (! vmcomplete(vmblock)) {
      LogError ("Memory full !", 0, __FILE__, __LINE__);
      return;
    }	  

    //READ DATA;
    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);

	TBEGIN = 0; TEND = TBEGIN + (TS * (NDATA - 1));
    FMAX= (0.5/TS);
    fprintf(fp_out," NYQUIST FREQUENCY: %6.1f\n",FMAX);
                                                                               
    // Compute mass response
    OSCIL(F0, VIT, ACC, TS, dzeta, NDATA);

    fprintf(fp_out,"\n");
    fprintf(fp_out, "    TIME (S)    MASS ACC. (M/S2) \n");
    fprintf(fp_out, "  ------------  -----------------\n");

    T = TBEGIN;

	for (i = 1; i<=NDATA; i++) {
      fprintf(fp_out, " %10.6f      %10.6f\n", T, ACC[i]);
      T += TS;
    }

    fprintf(fp_out,"\n End of file response.txt\n");
    fclose(fp_out);
    printf("\n Results in file response.txt...\n\n");
    free(vmblock);
}
                                                                                      
// End of file reponse1.cpp