/*
 *  =======================================================================
 *  Use Newmark Integration to compute time-history response of three-story
 *  shear structure subject to external puluse loading.
 *  =======================================================================
 *  Copyright (C) 1996 by Mark Austin and David Mazzoni.
 *                                                                     
 *  This software is provided "as is" without express or implied warranty.
 *  Permission is granted to use this software on any computer system,
 *  and to redistribute it freely, subject to the following restrictions:
 * 
 *  1. The authors are not responsible for the consequences of use of
 *     this software, even if they arise from defects in the software.
 *  2. The origin of this software must not be misrepresented, either
 *     by explicit claim or by omission.
 *  3. Altered versions must be plainly marked as such, and must not
 *     be misrepresented as being the original software.
 *  4. This notice is to remain intact.
 *
 *  Written By : Mark Austin                                      July 1996
 *  =======================================================================
 */

#include <stdio.h>
#include <math.h>
#include "matrix.h"
#include "vector.h"
#include "miscellaneous.h"

enum { NoDof = 3, NoWorkingPointers = 20, NoTimeSteps = 250 };

#define EIGEN

int main( void ) {
MATRIX  *spStiff, *spMass, *spEload, *spPt;
MATRIX  *spDispl,    *spVelocity,    *spAccel;
MATRIX  *spDisplNew, *spVelocityNew, *spAccelNew;
MATRIX  *spResponse, *spR,           *spMasshat; 
MATRIX  *spWork [ NoWorkingPointers ];
VECTOR  *spPivot;
double dTime  = 0.02;
double dBeta  = 0.25;
double dGamma = 0.50;
int ii, ij;

MATRIX *spEigenvalue, *spEigenvector;

   /* [a] : Setup mass and stiffness matrices */

   spMass  = matAllocIndirect(     "Mass", DoubleArray, NoDof, NoDof );
   spStiff = matAllocIndirect("Stiffness", DoubleArray, NoDof, NoDof );

   for(ii = 1; ii <= NoDof; ii++) 
       spMass->uMatrix.dpp[ii-1][ii-1] = 1000.0;

   spStiff->uMatrix.dpp[ 0 ][ 0 ] =  2.00e+6;
   spStiff->uMatrix.dpp[ 0 ][ 1 ] = -1.00e+6;

   spStiff->uMatrix.dpp[ 1 ][ 0 ] = -1.00e+6;
   spStiff->uMatrix.dpp[ 1 ][ 1 ] =  2.00e+6;
   spStiff->uMatrix.dpp[ 1 ][ 2 ] = -1.00e+6;

   spStiff->uMatrix.dpp[ 2 ][ 1 ] = -1.00e+6;
   spStiff->uMatrix.dpp[ 2 ][ 2 ] =  1.00e+6;

   matPrint( spMass );
   matPrint( spStiff );

#ifdef EIGEN
   spEigenvalue  = matAllocIndirect( "Eigenvalues", DoubleArray, NoDof,     1);
   spEigenvector = matAllocIndirect("Eigenvectors", DoubleArray, NoDof, NoDof);

   matGeneralisedEigen( spStiff, spMass, spEigenvalue, spEigenvector );

   printf("\n\n");
   for(ii = 1; ii <= NoDof; ii++) {
       printf("*** T = %10.4f\n", 2*3.1416/sqrt(spEigenvalue->uMatrix.dpp[ii-1][0]));
   }

   matPrint( spEigenvalue  );
   matPrint( spEigenvector );
#endif

   /* [b] : Initialize external load vector */

   spPt = matAllocIndirect("P(t)", DoubleArray, 21, 1 );
   for(ii = 0; ii <=  10; ii++) 
       spPt->uMatrix.dpp[ ii ][ 0 ] = 200*ii;
   for(ii = 11; ii  < 21; ii++) 
       spPt->uMatrix.dpp[ ii ][ 0 ] = 200*(20 - ii);

   /* [c] : Initialize working displacement, velocity and acceleration vectors */

   spDispl    = matAllocIndirect( "Displacement", DoubleArray, NoDof, 1 );
   spVelocity = matAllocIndirect(     "Velocity", DoubleArray, NoDof, 1 );
   spAccel    = matAllocIndirect( "Acceleration", DoubleArray, NoDof, 1 );
   spEload    = matAllocIndirect("External Load", DoubleArray, NoDof, 1 );

   /* [d] : Allocate matrix to store time-history response --
    *       Column 1 = time (sec).
    *       Column 2 = Displacement of roof (m).
    *       Column 3 = Energy (J).
    */

   spResponse = matAllocIndirect( "Response", DoubleArray, NoTimeSteps + 1, 3 );

   /* [e] : Compute effective mass matrix */

   spWork[0] = matScale (spStiff, dTime*dTime*dBeta );
   spMasshat = matAdd   (spMass,  spWork[0]);

   /* [f] : Compute LU decomposition of effective mass matrix */

   spPivot   = SetupPivotVector( spMasshat );
   spMasshat = matLUDecompositionIndirect( spMasshat, spPivot);

   /* [g] : Compute time-history response */

   for(ii = 1; ii <= NoTimeSteps; ii++) {

       /* [g.1] : Update external load vector */

       spEload->uMatrix.dpp[ 2 ][ 0 ] = 0.0;
       if (ii < 21)
           spEload->uMatrix.dpp[ 2 ][ 0 ] = spPt->uMatrix.dpp[ ii ][ 0 ];

       spWork[1] = matScale (spAccel,   (dTime*dTime/2.0)*(1-2*dBeta) );
       spWork[2] = matScale (spVelocity, dTime );
       spWork[3] = matAdd   (spDispl,   spWork[2] );
       spWork[4] = matAdd   (spWork[3], spWork[1] );
       spWork[5] = matMult  (spStiff,   spWork[4] );
       spR       = matSub   (spEload,   spWork[5] );

       /* [g.2] : Compute new acceleration, velocity and displacement */

       spAccelNew = matLUSubstitutionIndirect( spPivot , spMasshat, spR );
       spAccelNew->cpName = saveString("New Acceleration", __FILE__, __LINE__ );

       spWork[6]     = matScale (spAccel,    dTime*(1-dGamma));
       spWork[7]     = matScale (spAccelNew, dTime*dGamma);
       spWork[8]     = matAdd   (spWork[6],  spWork[7]);
       spVelocityNew = matAdd   (spVelocity, spWork[8]);

       spWork[9]     = matScale (spAccel,     (1-2*dBeta));
       spWork[10]    = matScale (spAccelNew,  2*dBeta );
       spWork[11]    = matAdd   (spWork[9],   spWork[10]);
       spWork[12]    = matScale (spWork[11],  dTime*dTime/2.0 );

       spWork[13]    = matAdd   (spWork[2],  spWork[12]);
       spDisplNew    = matAdd   (spDispl,    spWork[13]);

       /* [g.3] : Update response */

       matFree( spAccel );
       matFree( spVelocity );
       matFree( spDispl );
       spAccel    = matCopy( spAccelNew );
       spVelocity = matCopy( spVelocityNew );
       spDispl    = matCopy( spDisplNew );

       /* [g.4] : Compute "kinetic + potential" energy */

       spWork[14] = matTranspose ( spDispl );
       spWork[15] = matMult      ( spStiff, spDispl );
       spWork[16] = matMult      ( spWork[14], spWork[15] );

       spWork[17] = matTranspose ( spVelocity );
       spWork[18] = matMult      ( spMass, spVelocity );
       spWork[19] = matMult      ( spWork[17], spWork[18] );

       /* [g.5] : Save components of time-history response */

       spResponse->uMatrix.dpp[ ii ][ 0 ] = ii*dTime;
       spResponse->uMatrix.dpp[ ii ][ 1 ] = spDispl->uMatrix.dpp[2][0];
       spResponse->uMatrix.dpp[ ii ][ 2 ] = 0.5*(spWork[16]->uMatrix.dpp[0][0] +
                                                 spWork[19]->uMatrix.dpp[0][0] );

       /* [g.6] : Cleanup memory before moving onto next integration step */

       matFree (spR);
       for(ij = 1; ij < NoWorkingPointers; ij++)  {
           matFree (spWork [ij]);
       }
   }

   /* [h] : Print matrix of response components */

   matPrint (spResponse );
}