/*---------------------------------------------------------------------------*\
  =========                 |
  \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
   \\    /   O peration     |
    \\  /    A nd           | www.openfoam.com
     \\/     M anipulation  |
-------------------------------------------------------------------------------
    Copyright (C) 2016 OpenFOAM Foundation
    Copyright (C) 2020 OpenCFD Ltd.
-------------------------------------------------------------------------------
License
    This file is part of OpenFOAM.

    OpenFOAM 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.

    OpenFOAM 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 OpenFOAM.  If not, see <http://www.gnu.org/licenses/>.

\*---------------------------------------------------------------------------*/

#include "NewmarkCOMimplicit.H"
#include "addToRunTimeSelectionTable.H"

#include <iostream>
#include <fstream>
#include <iomanip>
#include <ios>

#include "SquareMatrix.H"

// * * * * * * * * * * * * * * Static Data Members * * * * * * * * * * * * * //

namespace Foam
{
namespace RBD
{
namespace rigidBodySolvers
{
    defineTypeNameAndDebug(NewmarkCOMimplicit, 0);
    addToRunTimeSelectionTable(rigidBodySolver, NewmarkCOMimplicit, dictionary);
}
}
}


// * * * * * * * * * * * * * * * * Constructors  * * * * * * * * * * * * * * //

Foam::RBD::rigidBodySolvers::NewmarkCOMimplicit::NewmarkCOMimplicit
(
    rigidBodyMotion& body,
    const dictionary& dict
)
:
    rigidBodySolver(body),
    gamma_(dict.getOrDefault<scalar>("gamma", 0.5)),
    beta_( dict.getOrDefault<scalar>("beta", 0.25)),
    accelN_( dict.getOrDefault<label>("accelN", 1000)),
    LSMN_(   dict.getOrDefault<label>("LSMN", 8)),
    updateRatio_( dict.getOrDefault<scalar>("updateRatio", 1.0)),
    errorThreshold_( dict.getOrDefault<scalar>("errorThreshold", 1.0e-6)),
    dynamicsReport_( dict.getOrDefault<Switch>("report", true))
{}


// * * * * * * * * * * * * * * * * Destructor  * * * * * * * * * * * * * * * //

Foam::RBD::rigidBodySolvers::NewmarkCOMimplicit::~NewmarkCOMimplicit()
{}


// * * * * * * * * * * * * * * * Member Functions  * * * * * * * * * * * * * //

/*** LU factorization: L is lower side of "LU" matrix and U upper side. ***/
void    Foam::RBD::rigidBodySolvers::NewmarkCOMimplicit::LUfactorization
( 
	SquareMatrix<scalar>& A, 
	SquareMatrix<scalar>& LU, 
	scalarField& b
)
{
        int    	num;
        scalar	pivot[A.m()], val;

        for(int l=0; l<A.m(); l++ ){
            /* diagonal */
            for(int i=l; i<A.m(); i++ ){
	      pivot[i] = A[i][l];
	      for(int j=0; j<l; j++ ) pivot[i] -= LU[i][j] * LU[j][l];
	    }
	    num = l;
	    for(int i=l+1; i<A.m(); i++ )
	      if( mag( pivot[num] ) < fabs( pivot[i] ) )   num = i;
	    if( num != l ){
	      /*** row exchange ***/
	      for(int j=0; j<A.m(); j++ ){
	      	  val = A[l][j];
	      	  A[l][j] = A[num][j];
	      	  A[num][j] = val;
	      }
	      	  val = b[l];
	      	  b[l] = b[num];
	      	  b[num] = val;
              for(int j=0; j<l; j++ ){
	      	  val = LU[l][j];
	      	  LU[l][j] = LU[num][j];
	      	  LU[num][j] = val;
	      }
	    }
	    LU[l][l] = pivot[num];
            /* lower */
            for(int i=l+1; i<A.m(); i++ ){
              LU[i][l] = A[i][l];
              for(int k=0; k<l; k++ )  LU[i][l] -= LU[i][k] * LU[k][l];
              LU[i][l] /= LU[l][l];
            }
            /* upper */
            for(int j=l+1; j<A.m(); j++ ){
              LU[l][j] = A[l][j];
              for(int k=0; k<l; k++ )  LU[l][j] -= LU[l][k] * LU[k][j];
            }
        }
}

/**** Crout method ***/
void	Foam::RBD::rigidBodySolvers::NewmarkCOMimplicit::solve_Ax_b_byCrout
( 
	const	SquareMatrix<scalar>& A,
	scalarField& x, 
	const scalarField& b 
)
{
	SquareMatrix<scalar> LU(A.m()), AA=A;
	scalarField	bb(b);

        LUfactorization( AA, LU, bb );

        for(int i=0; i<A.m(); i++ ){
          x[i] = bb[i];
          for(int j=0; j<i; j++ ) x[i] -= LU[i][j] * x[j];
        }
        for(int i=A.m()-1; i>=0; i-- ){
          for(int j=i+1; j<A.m(); j++ ) x[i] -= LU[i][j] * x[j];
          x[i] /= LU[i][i];
        }
}

void Foam::RBD::rigidBodySolvers::NewmarkCOMimplicit::RodriguesMatrixFromVector
(
    Tensor<scalar>& R,
    const vector rod
)
{
    scalar theta = mag(rod);
    vector n(rod/(theta + VSMALL));

    R.xx() = cos(theta) + n[0]*n[0] * ( 1.0 - cos(theta) );		R.xy() = n[0]*n[1] * ( 1.0 - cos(theta) ) - n[2] * sin(theta);	R.xz() = n[2]*n[0] * ( 1.0 - cos(theta) ) + n[1] * sin(theta);
    R.yx() = n[0]*n[1] * ( 1.0 - cos(theta) ) + n[2] * sin(theta);	R.yy() = cos(theta) + n[1]*n[1] * ( 1.0 - cos(theta) );		R.yz() = n[1]*n[2] * ( 1.0 - cos(theta) ) - n[0] * sin(theta);
    R.zx() = n[2]*n[0] * ( 1.0 - cos(theta) ) - n[1] * sin(theta);	R.zy() = n[1]*n[2] * ( 1.0 - cos(theta) ) + n[0] * sin(theta);	R.zz() = cos(theta) + n[2]*n[2] * ( 1.0 - cos(theta) );
}

void Foam::RBD::rigidBodySolvers::NewmarkCOMimplicit::RodriguesVectorFromMatrix
(
    vector& n,
    const Tensor<scalar> R
)
{
    scalar	xx, yy, theta;
    yy = sqrt( sqr(R.xy()-R.yx()) + sqr(R.xz()-R.zx()) + sqr(R.yz()-R.zy()) ) / 2.0;
    xx = ( R.xx() + R.yy() + R.zz() - 1 ) / 2.0;

    theta = Foam::atan2( yy, xx );

    if( mag(theta) < (100*VSMALL) ){	// theta = 0 deg.
	n = Zero;
    }else if( mag( mag(theta) - mag(Foam::atan2(0.0,-1.0)) ) < (100*VSMALL) ){	// theta = +-180 deg.
	if( (R.xx()+1) < 0.0 ) 	n[0] = 0.0;
	else			n[0] = sqrt( (R.xx() + 1)/2.0 );
	if( (R.yy()+1) < 0.0 )  n[1] = 0.0;
	else{
	  if( R.yx() > 0 )	n[1] = sqrt( (R.yy() + 1)/2.0 );
	  else			n[1] =-sqrt( (R.yy() + 1)/2.0 );
	}
	if( (R.zz()+1) < 0.0 )	n[2] = 0.0;
	else{
	  if( R.zx() > 0 )	n[2] = sqrt( (R.zz() + 1)/2.0 );
	  else			n[2] =-sqrt( (R.zz() + 1)/2.0 );
	}
	n *= theta;

Info << " theta*n " << n << endl;


    }else{
	n[0] = -(R.yz()-R.zy()) / (2.0*sin(theta));
	n[1] =  (R.xz()-R.zx()) / (2.0*sin(theta));
	n[2] = -(R.xy()-R.yx()) / (2.0*sin(theta));
	n *= theta;
    }
}

void	Foam::RBD::rigidBodySolvers::NewmarkCOMimplicit::updateQ
( 
	scalarField&	q,	// update
	const scalarField	q0, 
	const scalarField	qDot0, 
	const scalarField 	qDdot,
	const scalarField 	qDdot0 
)
{
	for(int k=0; k<(q.size()/6); k++ ){	// Pxyz + Rxyz = 6 dofs. k means rigid body ID.
	    /* position */
	    for(int i=0; i<3; i++ ) q[6*k+i] = q0[6*k+i] + deltaT()*qDot0[6*k+i] + sqr(deltaT())*( beta_*qDdot[6*k+i] + (0.5 - beta_)*qDdot0[6*k+i] );

	    /* Rodrigues angle vector */
    	    Tensor<scalar> R0, R;
    	    vector n0(q0[6*k+3], q0[6*k+4], q0[6*k+5]);
	    RodriguesMatrixFromVector( R0, n0 );

    	    vector rod;
	    for(int i=0; i<3; i++ ) rod[i] = deltaT()*qDot0[6*k+3+i] + sqr(deltaT())*( beta_*qDdot[6*k+3+i] + (0.5 - beta_)*qDdot0[6*k+3+i] );
	    RodriguesMatrixFromVector( R, rod );

	    R = R & R0;
	
	    RodriguesVectorFromMatrix( rod, R );
	    for(int i=0; i<3; i++ ) q[6*k+3+i] = rod[i];	
	}
}

void Foam::RBD::rigidBodySolvers::NewmarkCOMimplicit::solve
(
    const scalarField& tau,
    const Field<spatialVector>& fx
)
{
    Info << endl << "NewmarkCOMimplicit::solve, Rigid bodies = " << q().size()/6 << endl;

    const Switch flg = model_.debug;
    const scalar Time = state().t();
  
    Field<spatialVector> prefx[LSMN_], beta0(fx), beta1(fx);
    scalar	previousdt[LSMN_], D, m00, m01, m10, m11, f0, f1, time;	//  [[m00 m01] [m10 m11]] [b0 b1] = [f0 f1], parameters for least square method

    // **** Calclulate the current ( t - dt ) & correct acceleration, a[n] = qDdot():  n means t - dt  ****
    if( dynamicsReport_ ) 	model_.debug = true;
    else			model_.debug = false;
    state().t() = Time - deltaT();
    scalarField rtau(tau);
    Field<spatialVector> rfx(fx);
    model_.applyRestraints(rtau, rfx, state());
    model_.forwardDynamicsCOM(state(), rtau, rfx);
    scalar rotationalError = 0.0, translationalError = 0.0, max_rotationalError = 0.0, max_translationalError = 0.0;
    scalar ave_rotationalAccel = 0.0, ave_translationalAccel = 0.0, max_rotationalAccel = 0.0, max_translationalAccel = 0.0;
    label max_rotationalError_num = 0, max_translationalError_num = 0, max_rotationalAccel_num = 0, max_translationalAccel_num = 0;
    forAll( qDdot(), j ){
	if( (j%6) < 3 ){	// translation: Px, Py, Pz
	    translationalError += sqr( qDdot()[j] - qDdot0()[j] );
	    if( max_translationalError < mag(qDdot()[j] - qDdot0()[j]) ){	max_translationalError = mag(qDdot()[j] - qDdot0()[j]); max_translationalError_num = j; }		    
	    ave_translationalAccel += mag(qDdot()[j]);
	    if( max_translationalAccel < mag(qDdot()[j]) ){	max_translationalAccel = mag(qDdot()[j]); max_translationalAccel_num = j; }	
	}else{			// rotation: Rx, Ry, Rz
	    rotationalError += sqr( qDdot()[j] - qDdot0()[j] );
	    if( max_rotationalError < mag(qDdot()[j] - qDdot0()[j]) ){	max_rotationalError = mag(qDdot()[j] - qDdot0()[j]); max_rotationalError_num = j; }	
	    ave_rotationalAccel += mag(qDdot()[j]);
	    if( max_rotationalAccel < mag(qDdot()[j]) ){	max_rotationalAccel = mag(qDdot()[j]); max_rotationalAccel_num = j; }	
	}
    }
    ave_translationalAccel /= ( qDdot().size() / 2.0 );
    ave_rotationalAccel /= ( qDdot().size() / 2.0 );
    translationalError = sqrt( translationalError / ( qDdot().size() / 2.0 ) );
    rotationalError = sqrt( rotationalError / ( qDdot().size() / 2.0 ) );

    std::cout << std::scientific << std::setprecision(4);
    Info << "Ave. error between converged and correct accelerations: translation = " << translationalError << " rotation = " << rotationalError << endl;
    Info << "Max. trans[" << max_translationalError_num << "]: qDdot= " << qDdot()[max_translationalError_num] << " qDdot0=" << qDdot0()[max_translationalError_num] << endl;
    Info << "Max. rotation[" << max_rotationalError_num << "]: qDdot= " << qDdot()[max_rotationalError_num] << " qDdot0=" << qDdot0()[max_rotationalError_num] << endl;
    Info << "Ave. accel: translation = " << ave_translationalAccel << " rotation = " << ave_rotationalAccel << endl;
    Info << "Max. accel: translation[" << max_translationalAccel_num << "]= " << max_translationalAccel << " rotation[" << max_rotationalAccel_num << "]= " << max_rotationalAccel << endl;





    if( ave_translationalAccel < 1.0 )	ave_translationalAccel = 1.0;	// 0.001 m/s^2
    if( ave_rotationalAccel < 1.0 )	ave_rotationalAccel = 1.0;	// 1.0 deg/s^2




    if( 0.01 < (translationalError/ave_translationalAccel) )	Info << "Warning: The translational acceleration estimation error was " <<  100*translationalError/ave_translationalAccel << " %. " << endl;
    if( 0.01 < (rotationalError/ave_rotationalAccel) )		Info << "Warning: The rotational acceleration estimation error was " <<  100*rotationalError/ave_rotationalAccel << " %. " << endl;

    const scalarField an(qDdot());		// changes converged acceleration into correct acceleration, i.e., uses correct acceleration (with noise) as a[n]
//    const scalarField an(qDdot0());		// changes converged acceleration into correct acceleration, i.e., uses converged acceleration (smoothed) as a[n]


    // ****** Ffluid[n+1] estimation by Lease square method: Ffluid[n+1] = beta0 + beta1 * dt  *******
    for(int i=0; i<LSMN_; i++ ){	// initialize
	prefx[i] = fx;
        previousdt[i] = deltaT();
    }
    std::ifstream iffx("fxHistory_for_LSM.dat");
    if (!iffx){	// if there is no file:
	Info << "cannot open fxHistory_for_LSM.dat file. " << endl;
	forAll( beta0, i )
	    forAll( beta0[i], j ){ beta0[i][j] = fx[i][j]; beta1[i][j] = 0.0;	}
    }else{	// if there is the file
	for(int i=1; i<LSMN_; i++ ){	// read the past history of fluid reaction
	    iffx >> previousdt[i];
	    forAll( prefx[i], j )
		forAll( prefx[i][j], k ){ iffx >> prefx[i][j][k]; }
	}

	m00 = LSMN_; m01 = 0.0; m10 = 0.0; m11 = 0.0;
     	for(int i=0; i<LSMN_; i++ ){
	    time = 0.0;			// time = 0.0 means t - dt, i.e., current time. time = dt is future, n+1.
	    for(int j=1; j<=i; j++ )	time -= previousdt[j];
	    m01 += time;
	    m10 += time;
	    m11 += pow( time, 2.0 ) ;
	}
	D = m00 * m11 - m01*m10;

	forAll( fx, i )
	    forAll( fx[i], j ){
	    	f0 = 0.0;	f1 = 0.0;
	    	for( int k=0; k<LSMN_; k++ ){
			time = 0.0;
			for(int l=1; l<=k; l++ )	time -= previousdt[l];
			f0 += prefx[k][i][j];
			f1 += prefx[k][i][j] * time;
	    	}
	    	beta0[i][j] = ( m11 * f0 - m10 * f1 ) / D;
	    	beta1[i][j] = (-m01 * f0 + m00 * f1 ) / D;
	    }
    }	


    // **** Convergence calclulation for future acceleration, a[n+1]:  n+1 means t ****
    scalarField	an1k(qDdot()), min_an1k(qDdot()), E(qDdot()), Et(qDdot()), x(qDdot());		// an1k = a[n+1](k), i.e., qDdot()
    scalar	ave_trans_accel_error, 	    max_trans_accel_error, 	min_trans_accel_error = 0.0, 	  max_trans_accel;
    scalar	ave_rotational_accel_error, max_rotational_accel_error, min_rotational_accel_error = 0.0, max_rotational_accel;
    scalar dh;						// means delta a(n+1). This value is severe. 
    SquareMatrix<scalar> roundE(qDdot().size());

    model_.debug = false;
    state().t() = Time;		// n+1
    label max_trans_num, max_rotational_num;
    scalar updateRatio = updateRatio_;
    for(label k=0; k<accelN_; k++ ){
	// **** Newton Raphson  ****
	// E( a[n+1] )
	qDdot() = an1k;
        qDot() = qDot0() + deltaT()*( gamma_*qDdot() + (1.0 - gamma_)*an );
	updateQ( q(), q0(), qDot0(), qDdot(), an );
	model_.forwardDynamicsCorrectionCOM( state() );		// update rigidbody parameters, a_, v_, d_, etc.
        rtau = tau;
        rfx = beta0 + beta1 * deltaT();				// Estimated Ffluid[n+1]
        model_.applyRestraints( rtau, rfx, state() );		// calculate Fstructure[n+1]
        model_.forwardDynamicsCOM( state(), rtau, rfx );	// calculate updated a[n+1] = qDdot()
	forAll( E, i ){
	    if( (i%6) < 3 )	E[i] = ( qDdot()[i] - an1k[i] );	// translational E( a[n+1] )
	    else		E[i] = ( qDdot()[i] - an1k[i] );	// rotational E( a[n+1] )
	}

	// convergence determination
	ave_trans_accel_error = 0.0;		max_trans_accel_error = 0.0;		max_trans_accel = 0.0; 		max_trans_num = 0;				
	ave_rotational_accel_error = 0.0;	max_rotational_accel_error = 0.0;	max_rotational_accel = 0.0; 	max_rotational_num = 0;			
	forAll( qDdot(), j ){
	    if( (j%6) < 3 ){
		ave_trans_accel_error += mag(E[j]);
		if( max_trans_accel_error < mag(E[j]) ){	max_trans_accel_error = mag(E[j]);	}
		if( max_trans_accel < mag(qDdot()[j]) ){	max_trans_accel = mag(qDdot()[j]);	max_trans_num = j; }
	    }else{
		ave_rotational_accel_error += mag(E[j]);
		if( max_rotational_accel_error < mag(E[j]) ){	max_rotational_accel_error = mag(E[j]);	}
		if( max_rotational_accel < mag(qDdot()[j]) ){	max_rotational_accel = mag(qDdot()[j]);	max_rotational_num = j; }
	    }
	}
	ave_trans_accel_error /= qDdot().size();
	ave_rotational_accel_error /= qDdot().size();
	ave_trans_accel_error /= ave_translationalAccel;
	ave_rotational_accel_error /= ave_rotationalAccel;
	max_trans_accel_error /= ave_translationalAccel;
	max_rotational_accel_error /= ave_rotationalAccel;
	if( ( (k%100) == 0 ) || ( 100.0 < ( ave_trans_accel_error + ave_rotational_accel_error ) ) )
	    Info << "Converge error [" << k << "]: Ave.= " << ave_trans_accel_error << ", " << ave_rotational_accel_error << " Max." << max_trans_accel_error << ", " << max_rotational_accel_error 
		 << " Max. qDdot[" << max_trans_num << "]= " << qDdot()[max_trans_num] << " [m/s^2], qDdot[" << max_rotational_num << "]= " << qDdot()[max_rotational_num] 
	         << " [rad/s^2], error threshold " << errorThreshold_ << " dh = " << max_trans_accel * 1.0e-6 + 1.0e-8 << " [m/s^2], " << max_rotational_accel * 1.0e-6 + 1.0e-8 << " [rad/s^2] updateRatio= "  << updateRatio << endl;
	if( ( k == 0 ) || ( ( ave_trans_accel_error + ave_rotational_accel_error ) < ( min_trans_accel_error + min_rotational_accel_error ) ) ){
		min_an1k = an1k;
		min_trans_accel_error = ave_trans_accel_error;
		min_rotational_accel_error = ave_rotational_accel_error;
	}

	if( errorThreshold_ < (( ave_trans_accel_error + ave_rotational_accel_error )/2.0) ){
	  if( k == (accelN_-1) ){
	    Info << "Warning: The future acceleration was NOT conveged. " << endl;
	    Info << " min.trans.error= " << min_trans_accel_error << " , min.rotational.rror = " << min_rotational_accel_error << endl;
	  }else{
       	    forAll( qDdot(), ii ){
		// E( a[n+1] + dh )


		if( (ii%6) < 3 )	dh = max_trans_accel * 1.0e-6 + 1.0e-8;
		else			dh = max_rotational_accel * 1.0e-6 + 1.0e-8;


		qDdot() = an1k;		// a[n+1](k)
		qDdot()[ii] += dh;
	        qDot() = qDot0() + deltaT()*( gamma_*qDdot() + (1.0 - gamma_)*an );
		updateQ( q(), q0(), qDot0(), qDdot(), an );
		model_.forwardDynamicsCorrectionCOM( state() );		// update rigidbody parameters, a_, v_, d_, etc.
        	rtau = tau;
        	rfx = beta0 + beta1 * deltaT();				// Estimated Ffluid[n+1]
       		model_.applyRestraints( rtau, rfx, state() );		// calculate Fstructure[n+1]
        	model_.forwardDynamicsCOM( state(), rtau, rfx );	// calculate updated a[n+1] = qDdot()
		forAll( Et, i ){
	    	  if( (i%6) < 3 ){	// translation
		    if( i == ii ){ 	Et[i] = ( qDdot()[i] - ( an1k[i] + dh ) );	// ( E( a[n+1] + dh ) - E( a[n+1] ) ) / dh
		    }else{		Et[i] = ( qDdot()[i] - an1k[i] ); }
	    	  }else{		// rotation
		    if( i == ii ){ 	Et[i] = ( qDdot()[i] - ( an1k[i] + dh ) );	// ( E( a[n+1] + dh ) - E( a[n+1] ) ) / dh
		    }else{		Et[i] = ( qDdot()[i] - an1k[i] ); }
	    	  }
                  roundE[i][ii] = ( Et[i] - E[i] ) / dh;
		}
            }
	    solve_Ax_b_byCrout( roundE, x, E );						// finds the update value, x

	    if( (100.0*( min_trans_accel_error + min_rotational_accel_error )) < ( ave_trans_accel_error + ave_rotational_accel_error ) )	updateRatio = 0.5 * updateRatio_;
	    else																updateRatio = updateRatio_;

	    if( 400 < k )	updateRatio *= 0.01;
	    else if( 200 < k ) 	updateRatio *= 0.1;
	    else if( 100 < k ) 	updateRatio *= 0.3;
	    else if( 50 < k ) 	updateRatio *= 0.6;

	    an1k -= updateRatio * x;
	  }
	}else{	
	    Info << "Converge error [" << k << "]: Ave.= " << ave_trans_accel_error << ", " << ave_rotational_accel_error << " Max." << max_trans_accel_error << ", " << max_rotational_accel_error 
		 << " Max. qDdot[" << max_trans_num << "]= " << qDdot()[max_trans_num] << " [m/s^2], qDdot[" << max_rotational_num << "]= " << qDdot()[max_rotational_num] 
	         << " [rad/s^2], error threshold " << errorThreshold_ << " dh = " << max_trans_accel * 1.0e-6 + 1.0e-8 << " [m/s^2], " << max_rotational_accel * 1.0e-6 + 1.0e-8 << " [rad/s^2]"  << endl;
	    k = accelN_;
	}
    }

    qDdot() = min_an1k;
    qDot() = qDot0() + deltaT()*( gamma_*qDdot() + (1.0 - gamma_)*an );
    updateQ( q(), q0(), qDot0(), qDdot(), an );

//    correctQuaternionJoints();


    // ****** Ffluid[n+1] estimation by Lease square method: Ffluid[n+1] = beta0 + beta1 * dt  *******
    std::ofstream offx( "fxHistory_for_LSM.dat" );
    offx << std::scientific << std::showpos << std::setw(16);	// std::setprecision(8);
    if(!offx){	Info << "Error: cannot open fxHistory_for_LSM.dat " << endl; }
    else{
	for(int i=0; i<(LSMN_-1); i++ ){
	    offx << previousdt[i] << " " << std::endl;
	    forAll( prefx[i], k ){
		forAll( prefx[i][k], j ) offx << prefx[i][k][j] << " ";
		offx << std::endl;
	    }
	    offx << std::endl;		
	}

        offx << "Estimated future fluid force, fx" << std::endl;
	offx 	<< previousdt[0] << " " << std::endl;
	prefx[0] = beta0 + beta1 * deltaT();
	forAll( fx, k ){
	    forAll( fx[k], j ) offx << prefx[0][k][j] << " ";
	    offx << std::endl;
	}

	offx << "beta0 " << std::endl;
	forAll( fx, k ){
	    forAll( fx[k], j ) offx << beta0[k][j] << " ";
	    offx << std::endl;
	}

	offx << "beta1 " << std::endl;
	forAll( fx, k ){
	    forAll( fx[k], j ) offx << beta1[k][j] << " ";
	    offx << std::endl;
	}

	offx << std::endl << " state.t() = " << Time << std::endl;

        offx << std::defaultfloat;
	offx.close();	
    }

    model_.debug = flg;
}


// ************************************************************************* //
