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

Group
    grpRigidBodyDynamicsRestraints

Description
    Time-dependent angle control by torque input, based on wave function.

Usage
    Example:
    \verbatim
    restraints
    {
        angle
        {
            type        rollpitchyawControlWaveCOM;
            body        rigidObject;
	    reaction	name;
            z_angle 	(0.523 10 0  0 0 0  0 0 0);	// ( A[rad], f [Hz], P [rad], S [rad], dummy ... ), A*sin(2pi*f*t+P)+S
            y_angle 	(0.523 10 0  0 0 0  0 0 0);	// ( A[rad], f [Hz], P [rad], S [rad], dummy ... ), A*sin(2pi*f*t+P)+S
            x_angle 	(0.523 10 0  0 0 0  0 0 0);	// ( A[rad], f [Hz], P [rad], S [rad], dummy ... ), A*sin(2pi*f*t+P)+S
	    rotation_order	0;	// 0 means Rz&Ry&Rx
	    relaxation_time	T [s];	// else: Rz Ry Rx, 1:  Rz Rx Ry, 2:  Ry Rx Rz, 3: Ry Rz Rx, 4: Rx Ry Rz, 5: Rx Rz Ry 
            gain    	( Pgain Dgain max );
        }
    }
    \endverbatim

SourceFiles
    rollpitchyawControlWaveCOM.C

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

#ifndef RBD_restraints_rollpitchyawControlWaveCOM_H
#define RBD_restraints_rollpitchyawControlWaveCOM_H

#include "rigidBodyRestraint.H"

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

namespace Foam
{
namespace RBD
{
namespace restraints
{

/*---------------------------------------------------------------------------*\
                          Class rollpitchyawControlWaveCOM Declaration
\*---------------------------------------------------------------------------*/

class rollpitchyawControlWaveCOM
:
    public restraint
{
    // Private data

        //- (Amplitude [rad], frequency [Hz], Phase [rad], Shift [rad], dummy, dummy, dummy, dummy, dummy ):  A * sin( 2*pi*f*t + P ) + S 
        Tensor<scalar> z_angle_;
        Tensor<scalar> y_angle_;
        Tensor<scalar> x_angle_;

	vector 	gain_;			// Pgain, Dgain, max input
	scalar	relaxation_time_;		// if( t < relaxation_time ) torque * ( t / relaxation_time )
	scalar	relaxation_time_for_amp_;	// if( t < relaxation_time ) amplitude * ( t / relaxation_time )
	label	rotation_order_;		// else: Rz Ry Rx, 1:  Rz Rx Ry, 2:  Ry Rx Rz, 3: Ry Rz Rx, 4: Rx Ry Rz, 5: Rx Rz Ry 
	
	//- body ID for the reaction force
	word	reaction_;

public:

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


    // Constructors

        //- Construct from components
        rollpitchyawControlWaveCOM
        (
            const word& name,
            const dictionary& dict,
            const rigidBodyModel& model
        );

        //- Construct and return a clone
        virtual autoPtr<restraint> clone() const
        {
            return autoPtr<restraint>
            (
                new rollpitchyawControlWaveCOM(*this)
            );
        }


    //- Destructor
    virtual ~rollpitchyawControlWaveCOM();


    // Member Functions

        virtual void restrain
        (
            scalarField& tau,
            Field<spatialVector>& fx,
            const rigidBodyModelState& state
        ) const;

        //- Update properties from given dictionary
        virtual bool read(const dictionary& dict);

        //- Write
        virtual void write(Ostream&) const;
};


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

} // End namespace restraints
} // End namespace RBD
} // End namespace Foam

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

#endif

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