/*---------------------------------------------------------------------------*\
  =========                 |
  \\      /  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::fishlocomotionCOM

Group
    grpRigidBodyDynamicsRestraints

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

Usage
    Example:
    \verbatim
    restraints
    {
        fishmotion_between_bodies
        {
            type        fishlocomotionCOM;
            body        rigidObject;
	    bodyLength  0.2;	// [m]
	    frequency	40;	// [Hz]
	    lambda	0.2;	// [m]
	    carangiform	(0.02 0.02 0); // A0 amplitude [m], A1 amplitude [m],   dummy
	    rolling	(0.175 0.175 0); // p0 amplitude [rad], p1 amplitude [rad], phase=0
	    gain	(1 0.001 0.1);	// Pgain, Dgain, max
	    x_origin 	-0.1;	// origin of fish coordinate system
	    initialPitch 0;	// Initail pitch angle [rad]
	    relaxationTime 0.025;	//[s]
	    reaction	rigidObject2;
        }
    }
    \endverbatim

SourceFiles
    fishlocomotion.C

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

#ifndef RBD_restraints_fishlocomotionCOM_H
#define RBD_restraints_fishlocomotionCOM_H

#include "rigidBodyRestraint.H"

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

namespace Foam
{
namespace RBD
{
namespace restraints
{

/*---------------------------------------------------------------------------*\
                          Class fishlocomotionCOM Declaration
\*---------------------------------------------------------------------------*/

class fishlocomotionCOM
:
    public restraint
{
    // Private data

	// fish length [m]
	scalar	bodyLength_;

	// frequency [Hz]
	scalar	freq_;

	// wave length [m]
	scalar	lambda_;

	// caringiform parameter, A0 amplitude [m], A1 amplitude [m],   dummy	
	vector	carangiform_;

	// rolling parameter, p0 amplitude [rad], p1 amplitude [rad], phase=0	
	vector	rolling_;

        //- (P gain , D gain, max):  
        vector gain_;

	// origin of fish coordinate system
	scalar	x_origin_;
	
	// initial Pitch angle [rad]
	scalar	initialPitch_;

	// relaxation time
        scalar relaxationTime_;
	
	//- body ID for the reaction force
	word	reaction_;

        //- report flg (default: false)
        const bool	fishlocomotionCOMReport_;

public:

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


    // Constructors

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

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


    //- Destructor
    virtual ~fishlocomotionCOM();


    // 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;

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

	// Transform Rodrigues matrix to Rodrigues vector
	vector	RodriguesVectorFromMatrix
	(
	    const Tensor<scalar> R
	) const;
};


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

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

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

#endif

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