#include "vs.h"
const double PI = 3.141592654;
int main() 
{// two parameters approximation
	// A. Bode's Integration Formula
   double weight[33] = {14.0/45.0, 64.0/45.0, 24.0/45.0, 64.0/45.0, 28.0/45.0,
                                  64.0/45.0, 24.0/45.0, 64.0/45.0, 28.0/45.0,
                                  64.0/45.0, 24.0/45.0, 64.0/45.0, 28.0/45.0, 
                                  64.0/45.0, 24.0/45.0, 64.0/45.0, 28.0/45.0,
                                  64.0/45.0, 24.0/45.0, 64.0/45.0, 28.0/45.0,
                                  64.0/45.0, 24.0/45.0, 64.0/45.0, 28.0/45.0,
                                  64.0/45.0, 24.0/45.0, 64.0/45.0, 28.0/45.0,
                                  64.0/45.0, 24.0/45.0, 64.0/45.0, 14.0/45.0};
   Quadrature qp(weight, 0.0, 1.0, 33);
   J d_l(1.0/32.0);      // per normalized length

   // B. Define Basis Functions
   H2 x((double*)NULL, qp),
      phi = INTEGRABLE_VECTOR_OF_TANGENT_OF_TANGENT_BUNDLE("int, int, Quadrature",
            2/*vector size*/, 1/*spatial dim.*/, qp);
   phi[0] = 1.0-cos(2.0*PI*x); phi[1] = 1.0-cos(4.0*PI*x);
   H0 d2_phi = INTEGRABLE_VECTOR("int, Quadrature", 2, qp);
   for(int i = 0; i < 2; i++) d2_phi[i] = dd(phi)(i)[0][0]; // degenerated hessian matrix

   // B. Variational Formulation
   C0 mass = (((H0)phi)%((H0)phi))|d_l,
      stiff = (d2_phi%d2_phi)|d_l;

   // C. Time Marching Scheme and Initial Conditions
   // C.1 Newmark scheme
   double gamma_ = 0.5, beta_ = 0.25, // Newmark scheme; constant-average-acceleration method
          dt_ = 0.01;   // time step,
   C0 c_old(2, (double*)0),   c_new(2, (double*)0),
      dc_old(2, (double*)0),  dc_new(2, (double*)0),   // velocity
      ddc_old(2, (double*)0), ddc_new(2, (double*)0);  //acceleration
   double a[8]; // Newmark scheme coefficients
   a[0] = 1.0/(beta_*pow(dt_,2)); a[1] = gamma_/(beta_*dt_); a[2] = 1.0/(beta_*dt_);
   a[3] = 1.0/(2.0*beta_)-1.0;    a[4] = gamma_/beta_-1.0;    a[5] = dt_/2.0*(gamma_/beta_-2.0);
   a[6] = dt_*(1.0-gamma_);       a[7] = gamma_*dt_;
   // C.2 Initial Condition
   //     solve the coefficients for the initial condition by Bubnov-Galerkin Method
   H0 w_0 = sin(PI*((H0)x))-PI*((H0)x)*(1.0-((H0)x));                   // initial condition
   c_old = ( ( ((H0)phi)*w_0 )|d_l ) / ( ( ((H0)phi)%((H0)phi) )|d_l ); // Bubnov-Galerkin approximation

   // D. Time Integration
	C0	LHS = stiff + a[0]*mass;
   C0 d_LHS = !LHS; // decomposed only once
   for(i = 0; i < 28; i++) {
   	C0 RHS = mass * (a[0]*c_old + a[2] * dc_old + a[3] * ddc_old);
   	c_new = d_LHS*RHS; // forward/backward substitution
      double iptr;
      if(modf( ((double)(i+1))/2.0, &iptr)==0)
      	//cout << "t= " << ((i+1)*dt_) << ", u(0.5) = " <<
      	//(c_new[0]*(1.0-cos(PI))+c_new[1]*(1.0-cos(2.0*PI)))  << endl;
         cout << c_new << endl;
      ddc_new = a[0]*(c_new - c_old)-a[2]*dc_old-a[3]*ddc_old;
      dc_new = dc_old + a[6]*ddc_old + a[7]*ddc_new;
      c_old = c_new;  dc_old = dc_new; ddc_old = ddc_new; // update
   }
	return 0;
}