/* Fesslix - Stochastic Analysis
 * Copyright (C) 2010-2014 Wolfgang Betz
 *
 * Fesslix is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * Fesslix is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Fesslix.  If not, see <http://www.gnu.org/licenses/>. 
 */


#include "flxdyna_kernel.h"
#include "flxMtx_Eigen.h"

#if FLX_DEBUG
int flxDynaBox::Cinst = 0;
#endif

flxDynaProj::flxDynaProj(const std::string name, const tuint Ndof, const tuint Nt)
: Ndof(Ndof), Nt(Nt), dt(ONE), M(NULL), K(NULL), D(NULL), C0(NULL), L0(NULL), Ltri(NULL), hv(Ndof), x(Nt+1,Ndof), v(Nt+1,Ndof), a(Nt+1,Ndof), f(Nt,Ndof), x0(Ndof), v0(Ndof), a0(Ndof), name(name),
  emode_valid(false), Eigenvalues(NULL)
{

}

flxDynaProj::~flxDynaProj()
{
  if (M) delete M;
  if (K) delete K;
  if (D) delete D;
  if (C0) delete C0;
  if (L0) delete L0;
  if (Ltri) delete Ltri;
  if (Eigenvalues) delete Eigenvalues;
}

void flxDynaProj::set_M(FlxMtx_baseS* MV)
{
  if (M) {
    delete MV;
    throw FlxException("flxDynaProj::set_M_1","The mass matrix can only be assigned once.");
  }
  if (MV->nrows()!=Ndof||MV->ncols()!=Ndof) {
    delete MV;
    throw FlxException("flxDynaProj::set_M_2","Matrix has wrong size.");
  }
  emode_valid = false;
  M = MV;
}

void flxDynaProj::set_K(FlxMtx_baseS* KV)
{
  if (K) {
    delete KV;
    throw FlxException("flxDynaProj::set_K_1","The stiffness matrix can only be assigned once.");
  }
  if (KV->nrows()!=Ndof||KV->ncols()!=Ndof) {
    delete KV;
    throw FlxException("flxDynaProj::set_K_2","Matrix has wrong size.");
  }
  emode_valid = false;
  K = KV;
}

void flxDynaProj::set_D(FlxMtx_baseS* DV)
{
  if (D) {
    delete DV;
    throw FlxException("flxDynaProj::set_D_1","The damping matrix can only be assigned once.");
  }
  if (DV->nrows()!=Ndof||DV->ncols()!=Ndof) {
    delete DV;
    throw FlxException("flxDynaProj::set_D_2","Matrix has wrong size.");
  }
  D = DV;
}

void flxDynaProj::set_dratio(const flxVec& a0V)
{
  if (emode_valid==false) solve_modes();
  // get the proper matrix types
    FlxMtxDiag* Md = dynamic_cast<FlxMtxDiag*>(M);
    if (Md==NULL) {
      throw FlxException_NotImplemented("flxDynaProj::set_dratio_1");
    }
    const tdouble* const Mdp = &(Md->get_mtx_tVec())[0];
    if (D==NULL) {
      D = new FlxMtxSym(Ndof);
    }
    FlxMtxSym* Df = dynamic_cast<FlxMtxSym*>(D);
    if (Df==NULL) {
      throw FlxException("flxDynaProj::set_dratio_2","Damping matrix has wrong type.");
    }
    solve_prepare();
    FlxMtxSym* Cs = dynamic_cast<FlxMtxSym*>(C0);
    if (Cs==NULL) throw FlxException_Crude("flxDynaProj::set_dratio_3");
    tdouble* Csp = Cs->get_VecPointer();
    flxVec cV(Csp,Ndof);
  // assemble m^-1*beta matrix (diagonal matrix)
    for (tuint i=0;i<Ndof;++i) {
      Csp[i] = 2*a0V[i]*Eigenvalues->operator[](i)*Mdp[i];
    }
  // perform theta*(m^-1*beta)*theta^T
    for (tuint i=0;i<Ndof;++i) {	// loop over columns
      // assemble help vector
	for (tuint j=0;j<Ndof;++j) {
	  hv[j] = ((Eigenvectors[j])[i])*Csp[j];
	}
      // assemble the ith column
	for (tuint j=i;j<Ndof;++j) {
	  tdouble d = ZERO;
	  for (tuint m=0;m<Ndof;++m) {
	    d += hv[m]*((Eigenvectors[m])[j]);
	  }
	  Df->operator()(j,i) = d;
	}
    }
}

void flxDynaProj::set_dt(const tdouble& dtV)
{
  if (dtV<ZERO) {
    std::ostringstream ssV;
    ssV << "Negative values (" << GlobalVar.Double2String(dtV) << ") are not allowed for 'dt'.";
    throw FlxException("flxDynaProj::set_dt", ssV.str() ); 
  }
  dt = dtV;
}

void flxDynaProj::solve_prepare()
{
  if (M==NULL||K==NULL||D==NULL) {
    throw FlxException("flxDynaProj::solve_prepare_1","Not all matrices are assembled.");
  }
  if (C0==NULL) {	// do this only once !!!
    FlxMtxDiag* Ms = dynamic_cast<FlxMtxDiag*>(M);
    FlxMtxSymBand* Ks = dynamic_cast<FlxMtxSymBand*>(K);
    FlxMtxSymBand* Ds = dynamic_cast<FlxMtxSymBand*>(D);
    if (Ms && Ks && Ds && (Ks->get_bsize()==Ds->get_bsize()) ) {	// use band matrices
      const tuint bsize = Ks->get_bsize();
      C0 = new FlxMtxSymBand(Ndof,bsize);
      L0 = new FlxMtxLTriBand(Ndof,bsize);
    } else {			// use full symmetric matrices
      C0 = new FlxMtxSym(Ndof);
      Ltri = new FlxMtxLTri(Ndof);
    }
  }
}

// Müller Dynamic
void flxDynaProj::solve_x()
{
  solve_prepare();
  // set up C0
    C0->set_zeroMtx();
    FlxMtxDiag* Ms = dynamic_cast<FlxMtxDiag*>(M);
    {
      FlxMtxSymBand* Ks = dynamic_cast<FlxMtxSymBand*>(K);
      FlxMtxSymBand* Ds = dynamic_cast<FlxMtxSymBand*>(D);
      FlxMtxSymBand* C0s = dynamic_cast<FlxMtxSymBand*>(C0);
      if (Ms && Ks && Ds) {	// it is a series system
	#if FLX_DEBUG
	  if (C0s==NULL || L0==NULL) throw FlxException_Crude("flxDynaProj::solve_x_1");
	#endif
	C0s->add_mtx(*Ks,ONE);
	C0s->add_mtx(*Ds,2/dt);
	C0s->add_mtx(*Ms,4/pow2(dt));
	L0->CholeskyDec(*C0s);
      } else {
	FlxMtxSym* Cs = dynamic_cast<FlxMtxSym*>(C0);
	#if FLX_DEBUG
	  if (Cs==NULL || Ltri==NULL) throw FlxException_Crude("flxDynaProj::solve_x_2");
	#endif
	Cs->add_mtx(*K,ONE);
	Cs->add_mtx(*D,2/dt);
	Cs->add_mtx(*M,4/pow2(dt));
	Cs->Invert(*Ltri);
      }
    }
  // set initial conditions
    tdouble* xp = &(x.operator()(0,0));
    tdouble* vp = &(v.operator()(0,0));
    tdouble* ap = &(a.operator()(0,0));
    tdouble* fp = &(f.operator()(0,0));
    {
      flxVec xi(xp,Ndof);
      xi = x0;
      flxVec vi(vp,Ndof);
      vi = v0;
      flxVec ai(ap,Ndof);
      ai = a0;
    }
  // constants
    const tdouble W11 = 4/dt;
    const tdouble W12 = 4/pow2(dt);
    const tdouble W22 = 2/dt;
  // start the main loop
    tnlong c = 0;
    for (tuint i=1;i<=Nt;++i) {
      // the previous state
	flxVec xl(xp+c,Ndof);
	flxVec vl(vp+c,Ndof);
	flxVec al(ap+c,Ndof);
      // the current state
	flxVec fi(fp+c,Ndof);
	c+=Ndof;
	flxVec xi(xp+c,Ndof);
	flxVec vi(vp+c,Ndof);
	flxVec ai(ap+c,Ndof);
      // position
	xi = fi;
	ai = al;
	ai.add(vl,W11);
	ai.add(xl,W12);
	M->MultMv(ai,vi);
	xi += vi;
	ai = vl;
	ai.add(xl,W22);
	D->MultMv(ai,vi);
	xi += vi;
	if (L0) {
	  L0->MultInv(xi,ai);
	  L0->MultInvTrans(ai,xi);
	} else {
	  C0->MultMv(xi,ai);
	  xi = ai;
	}
      // velocity
	vi = vl;
	vi *= -1;
	vi.add(xl,-W22);
	vi.add(xi,W22);
      // acceleration
	ai = fi;
	D->MultMv(vi,hv);
	ai.add(hv,-ONE);
	K->MultMv(xi,hv);
	ai.add(hv,-ONE);
	hv = ai;
	if (Ms) {
	  Ms->MultInv(hv,ai);
	} else {
	  throw FlxException_NotImplemented("flxDynaProj::solve_x_4");
	}
    }
}

// Giulio
void flxDynaProj::solve_a()
{
  solve_prepare();
  const tdouble beta = ONE/4;
  // define the other constants
    const tdouble C02 = (ONE/2-beta)*pow2(dt);
    const tdouble C03 = beta*pow2(dt);
    const tdouble C04 = dt/2;
  // set up C0
    C0->set_zeroMtx();
    {
      FlxMtxDiag* Ms = dynamic_cast<FlxMtxDiag*>(M);
      FlxMtxSymBand* Ks = dynamic_cast<FlxMtxSymBand*>(K);
      FlxMtxSymBand* Ds = dynamic_cast<FlxMtxSymBand*>(D);
      FlxMtxSymBand* C0s = dynamic_cast<FlxMtxSymBand*>(C0);
      if (Ms && Ks && Ds && C0s && L0) {	// it is a series system
	C0s->add_mtx(*Ks,C03);
	C0s->add_mtx(*Ds,C04);
	*C0s += *M;
	L0->CholeskyDec(*C0s);
      } else {
	FlxMtxSym* Cs = dynamic_cast<FlxMtxSym*>(C0);
	#if FLX_DEBUG
	  if (Cs==NULL || Ltri==NULL) throw FlxException_Crude("flxDynaProj::solve_a_2");
	#endif
	Cs->add_mtx(*K,C03);
	Cs->add_mtx(*D,C04);
	Cs->add_mtx(*M,ONE);
	Cs->Invert(*Ltri);
      }
    }
  // set initial conditions
    tdouble* xp = &(x.operator()(0,0));
    tdouble* vp = &(v.operator()(0,0));
    tdouble* ap = &(a.operator()(0,0));
    tdouble* fp = &(f.operator()(0,0));
    {
      flxVec xi(xp,Ndof);
      xi = x0;
      flxVec vi(vp,Ndof);
      vi = v0;
      flxVec ai(ap,Ndof);
      ai = a0;
    }
  // start the main loop
    tnlong c = 0;
    for (tuint i=1;i<=Nt;++i) {
      // the previous state
	flxVec xl(xp+c,Ndof);
	flxVec vl(vp+c,Ndof);
	flxVec al(ap+c,Ndof);
      // the current state
	flxVec fi(fp+c,Ndof);
	c+=Ndof;
	flxVec xi(xp+c,Ndof);
	flxVec vi(vp+c,Ndof);
	flxVec ai(ap+c,Ndof);
      // acceleration	a_{i+1} = C^{-1}*( f_{i+1} - D*(v_i+dt/2*a_i) - K*(x_i+dt*v_i+(1/2-beta)*dt^2*a_i) )
	xi = fi;
	ai = vl;
	ai.add(al,C04);
	D->MultMv(ai,vi);
	xi.add(vi,-ONE);
	ai = xl;
	ai.add(vl,dt);
	ai.add(al,C02);
	K->MultMv(ai,vi);
	xi.add(vi,-ONE);
	if (L0) {
	  L0->MultInv(xi,ai);
	  L0->MultInvTrans(ai,ai);
	} else {
	  C0->MultMv(xi,ai);
	}
      // position	x_{i+1} = x_i + dt*v_i + (1/2-beta)*dt^2*a_i + beta*dt^2*a_{i+1}
	xi = xl;
	xi.add(vl,dt);
	xi.add(al,C02);
	xi.add(ai,C03);
      // velocity	v_{i+1} = v_i + dt/2*(a_i+a_{i+1})
	vi = al;
	vi += ai;
	vi *= C04;
	vi += vl;
    }
}

void flxDynaProj::solve_modes()
{
  if (M==NULL||K==NULL) {
    std::ostringstream ssV;
    ssV << "Before the eigenmodes can be computed, the mass and the stiffness matrix have to be assembled.";
    throw FlxException("flxDynaProj::solve_modes_1", ssV.str() );
  }
  if (Eigenvalues==NULL) {
    Eigenvalues = new flxVec(Ndof);
    for (tuint i = 0; i < Ndof; i++) {
      flxVec Vh(Ndof);
      Eigenvectors.push_back(Vh);
    }
  }
  #if FLX_DEBUG
    if (emode_valid) throw FlxException_Crude("flxDynaProj::solve_modes_2");
  #endif
  MtxEigenValue_GSL(*K,*M,Ndof,*Eigenvalues,Eigenvectors,2);
  // deal with eigenvalues
    for (tuint i=0;i<Ndof;++i) (*Eigenvalues)[i] = sqrt((*Eigenvalues)[i]);
  // normalize eigenvectors
    for (tuint i=0;i<Ndof;++i) {
      flxVec& hV = Eigenvectors[i];
      hV.normalize();
    }
  emode_valid = true;
}

const tdouble flxDynaProj::get_natfreq(const tuint mID)
{
  if (mID>=Ndof) {
    std::ostringstream ssV;
    ssV << "ID is larger than the number of eigenmodes of the system.";
    throw FlxException("flxDynaProj::get_natfreq_1", ssV.str() );
  }
  if (emode_valid==false) solve_modes();
  return Eigenvalues->operator[](mID);
}




// ------------------------------------------------------------------------------------------------


flxDynaBox::flxDynaBox()
{
  #if FLX_DEBUG
    ++Cinst;
    if (Cinst > 1) {
      std::ostringstream ssV;
      ssV << "More than one instance of 'flxDynaBox' created ...";
      throw FlxException("flxDynaBox::flxDynaBox", ssV.str() );
    }
  #endif
}

flxDynaBox::~flxDynaBox()
{
  for (std::map<std::string, flxDynaProj*>::iterator pos = box.begin(); pos != box.end(); ++pos) {
    delete pos->second;
  }
}

void flxDynaBox::insert(const std::string name, const tuint Ndof, const tuint Nt)
{
  std::map<std::string, flxDynaProj*>::iterator pos = box.find(name);
  if ( pos != box.end() ) {
    std::ostringstream ssV;
    ssV << "A dyna-project with the name '" << name << "' does already exist.";
    throw FlxException("flxDynaBox::insert_1", "Project already exists", ssV.str() ); 
  } else {
    flxDynaProj* valuep = new flxDynaProj(name,Ndof,Nt);
    std::pair<std::string, flxDynaProj*> Element(name, valuep);
    box.insert(Element);
  }
}

flxDynaProj& flxDynaBox::get(const std::string& name)
{
  std::map<std::string, flxDynaProj*>::iterator pos = box.find(name);
  if ( pos != box.end() ) {
    return (*pos->second);
  } else {
    std::ostringstream ssV_2;
    ssV_2 << "The dyna-project '" << name << "' does not exist.";
    throw FlxException("flxDynaBox::get_1", ssV_2.str(), "You have to define the project before you can use it."); 
  }
}





