/*---------------------------------------------------------------------------*\
  =========                 |
  \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
   \\    /   O peration     |
    \\  /    A nd           | www.openfoam.com
     \\/     M anipulation  |
-------------------------------------------------------------------------------
    Copyright (C) 2016 OpenFOAM Foundation
-------------------------------------------------------------------------------
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/>.

Class
    Foam::RBD::rigidBodySolvers::NewmarkCOMimplicit

Description
    NewmarkCOMimplicit 2nd-order time-integrator for multibody-body motion around "center of mass" (not joint rotational center).
    a[n+1] is found by convergence calculation. Ffluid[n+1] is estimated by least square method.
    Since the data of least square method is saved, you cannot calculate again from halfway and must calculate from the begining (t=0). 

    Reference:
    \verbatim
        Newmark, N. M. (1959).
        A method of computation for structural dynamics.
        Journal of the Engineering Mechanics Division, 85(3), 67-94.
    \endverbatim

    Example specification in dynamicMeshDict:
    \verbatim
    solver
    {
        type    NewmarkCOMimplicit;
        gamma   0.5;    // Velocity integration coefficient
        beta    0.25;   // Position integration coefficient
	accelN	1000;	// maximum iteration number of convergence calculation for a[n+1]
	LSMN	8;	// data number for estimation of Ffluid[n+1] by least square method
	updateRatio	1.0;	// update coefficient for Newton Raphson method
	errorThreshold 1.0e-6;	// error threshold [m/s^2] or [rad/s^2]
	dynamicsReport	on;	// force and moment report in the restraints functions
    }
    \endverbatim

SourceFiles
    NewmarkCOMimplicit.C

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

#ifndef NewmarkCOMimplicit_H
#define NewmarkCOMimplicit_H

#include "rigidBodySolver.H"
#include "SquareMatrix.H"

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

namespace Foam
{
namespace RBD
{
namespace rigidBodySolvers
{

/*---------------------------------------------------------------------------*\
                           Class NewmarkCOMimplicit Declaration
\*---------------------------------------------------------------------------*/

class NewmarkCOMimplicit
:
    public rigidBodySolver
{
    // Private data

        //- Coefficient for velocity integration (default: 0.5)
        const scalar gamma_;

        //- Coefficient for position and orientation integration (default: 0.25)
        const scalar beta_;

        //- maximum iteration number of convergence calculation for a[n+1] (default: 1000)
        const label accelN_;

        //- data number for estimation of Ffluid[n+1] by least square method (default: 8)
        const label LSMN_;

        //- update ratio (default: 0.1)
        const scalar updateRatio_;

        //- error threshold [m/s^2 or rad/s^2] (default: 1.0e-6)
        const scalar errorThreshold_;

        //- report flg (default: true)
        const Switch	dynamicsReport_;

public:

    //- Runtime type information
    TypeName("NewmarkCOMimplicit");


    // Constructors

        //- Construct for the given body from dictionary
        NewmarkCOMimplicit
        (
            rigidBodyMotion& body,
            const dictionary& dict
        );


    //- Destructor
    virtual ~NewmarkCOMimplicit();


    // Member Functions

        //- Integrate the rigid-body motion for one time-step
        virtual void solve
        (
            const scalarField& tau,
            const Field<spatialVector>& fx
        );

	void    LUfactorization
	( 
	    SquareMatrix<scalar>& A, 
	    SquareMatrix<scalar>& LU, 
	    scalarField &b
	);

	void	solve_Ax_b_byCrout
	( 
	    const	SquareMatrix<scalar>& A,
	    scalarField& x, 
	    const scalarField& b 
	);

	// Transform Rodrigues vector to Rodrigues matrix
	void	RodriguesMatrixFromVector
	(
	    Tensor<scalar>& R,
	    const vector rod
	);

	// Transform Rodrigues matrix to Rodrigues vector
	void	RodriguesVectorFromMatrix
	(
	    vector& n,
	    const Tensor<scalar> R
	);

	void	updateQ
	( 
	    scalarField&	q,	// update
	    const scalarField	q0, 
	    const scalarField	qDot0, 
	    const scalarField 	qDdot,
	    const scalarField 	qDdot0 
	);
};


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

} // End namespace rigidBodySolvers
} // End namespace RBD
} // End namespace Foam

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

#endif

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