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

//#include "rigidBodyMotion.H"

#define	PAI	3.14159265359
#define	deg(a)	((a)*180.0/PAI)	// from rad to deg

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

namespace Foam
{
namespace RBD
{
namespace restraints
{
    defineTypeNameAndDebug(rollpitchyawControlWaveCOM, 0);

    addToRunTimeSelectionTable
    (
        restraint,
        rollpitchyawControlWaveCOM,
        dictionary
    );
}
}
}


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

Foam::RBD::restraints::rollpitchyawControlWaveCOM::rollpitchyawControlWaveCOM
(
    const word& name,
    const dictionary& dict,
    const rigidBodyModel& model
)
:
    restraint(name, dict, model),
    z_angle_(Tensor<scalar>::zero),
    y_angle_(Tensor<scalar>::zero),
    x_angle_(Tensor<scalar>::zero),
    gain_(Zero),
    relaxation_time_(0.0),
    rotation_order_(0)
{
    read(dict);
}


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

Foam::RBD::restraints::rollpitchyawControlWaveCOM::~rollpitchyawControlWaveCOM()
{}


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

void Foam::RBD::restraints::rollpitchyawControlWaveCOM::restrain
(
    scalarField& tau,
    Field<spatialVector>& fx,
    const rigidBodyModelState& state
) const
{
	label referenceID = model_.bodyID(reaction_);				// reaction_ means a rigid body name  
	Tensor<scalar> Rr = model_.X0(referenceID).E().T(), Rb = model_.X0(bodyID_).E().T();
	Tensor<scalar> rRb = Rr.T() & Rb;
	Tensor<scalar> bRt, rRt, Rz, Ry, Rx, ptRt, rRpt;			
	scalar	target_angle[3], t = state.t(),	dt = state.deltaT(), theta;	// 0:z(yaw), 1:y(pitch), 2:x(roll)
	vector moment = Zero, bTaxis = Zero, ptVaxis = Zero;
	vector angular_vel = Rr.T() & ( model_.v(bodyID_).w() - model_.v(referenceID).w() );

    	const double amp[3]  ={z_angle_.xx(), y_angle_.xx(), x_angle_.xx()};
    	const double freq[3] ={z_angle_.xy(), y_angle_.xy(), x_angle_.xy()};
    	const double phase[3]={z_angle_.xz(), y_angle_.xz(), x_angle_.xz()};
    	const double shift[3]={z_angle_.yx(), y_angle_.yx(), x_angle_.yx()};

	/*** Target attitude: Rodrigues rotation ***/
	for(int i=0; i<3; i++ )	target_angle[i] = amp[i] * sin( 2.0*PAI * freq[i] * t + phase[i] ) + shift[i];

	// z: yaw, warning: this matrix is transformed because of openFOAM definition. 
	Rz.xx() = cos( target_angle[0] );	Rz.xy() =-sin( target_angle[0] );	Rz.xz() = 0.0;	 	
	Rz.yx() = sin( target_angle[0] );	Rz.yy() = cos( target_angle[0] );	Rz.yz() = 0.0;	 	
	Rz.zx() = 0.0;				Rz.zy() = 0.0;				Rz.zz() = 1.0;	 	
	// y: pitch
	Ry.xx() = cos( target_angle[1] );	Ry.xy() = 0.0;	Ry.xz() = sin( target_angle[1] );	 	
	Ry.yx() = 0.0;				Ry.yy() = 1.0;	Ry.yz() = 0.0;	 	
	Ry.zx() =-sin( target_angle[1] );	Ry.zy() = 0.0;	Ry.zz() = cos( target_angle[1] );	 	
	// x: roll, warning: this matrix is transformed because of openFOAM definition. 
	Rx.xx() = 1.0;	Rx.xy() = 0.0;				Rx.xz() = 0.0;	 	
	Rx.yx() = 0.0;	Rx.yy() = cos( target_angle[2] );	Rx.yz() =-sin( target_angle[2] );	 	
	Rx.zx() = 0.0;	Rx.zy() = sin( target_angle[2] );	Rx.zz() = cos( target_angle[2] );	 	

	switch( rotation_order_ ){
		case 1:		rRt = Rr.T() & Rz & Rx & Ry; break;
		case 2:		rRt = Rr.T() & Ry & Rx & Rz; break;
		case 3:		rRt = Rr.T() & Ry & Rz & Rx; break;
		case 4:		rRt = Rr.T() & Rx & Ry & Rz; break;
		case 5:		rRt = Rr.T() & Rx & Rz & Ry; break;
		default:	rRt = Rr.T() & Rz & Ry & Rx;
	}

	bRt = rRb.T() & rRt;	// target attitude viewing from body attitude

	theta = atan2( sqrt( pow(bRt.yx()-bRt.xy(),2.0) + pow(bRt.xz()-bRt.zx(),2.0) + pow(bRt.zy()-bRt.yz(),2.0) ), bRt.xx() + bRt.yy()+ bRt.zz() - 1 );

	if( 1.0e-32 < mag(sin(theta)) ){
	    bTaxis[2] = bRt.yx() - bRt.xy(); 
	    bTaxis[1] = bRt.xz() - bRt.zx(); 
	    bTaxis[0] = bRt.zy() - bRt.yz();
	    bTaxis *= (theta/(2.0*sin(theta)));
	}

	/*** Previous attitude: Rodrigues rotation ***/
	for(int i=0; i<3; i++ )	target_angle[i]  = amp[i] * sin( 2.0*PAI * freq[i] * ( t - dt ) + phase[i] ) + shift[i];

	// z: yaw, warning: this matrix is transformed because of openFOAM definition. 
	Rz.xx() = cos( target_angle[0] );	Rz.xy() =-sin( target_angle[0] );	Rz.xz() = 0.0;	 	
	Rz.yx() = sin( target_angle[0] );	Rz.yy() = cos( target_angle[0] );	Rz.yz() = 0.0;	 	
	Rz.zx() = 0.0;				Rz.zy() = 0.0;				Rz.zz() = 1.0;	 	
	// y: pitch
	Ry.xx() = cos( target_angle[1] );	Ry.xy() = 0.0;	Ry.xz() = sin( target_angle[1] );	 	
	Ry.yx() = 0.0;				Ry.yy() = 1.0;	Ry.yz() = 0.0;	 	
	Ry.zx() =-sin( target_angle[1] );	Ry.zy() = 0.0;	Ry.zz() = cos( target_angle[1] );	 	
	// x: roll, warning: this matrix is transformed because of openFOAM definition. 
	Rx.xx() = 1.0;	Rx.xy() = 0.0;				Rx.xz() = 0.0;	 	
	Rx.yx() = 0.0;	Rx.yy() = cos( target_angle[2] );	Rx.yz() =-sin( target_angle[2] );	 	
	Rx.zx() = 0.0;	Rx.zy() = sin( target_angle[2] );	Rx.zz() = cos( target_angle[2] );	 	

	rRpt = Rr.T() & Rz & Rx & Ry;

	switch( rotation_order_ ){	// difference
		case 1:		ptRt = rRpt.T() & rRt; break;
		case 2:		ptRt = rRpt.T() & rRt; break;
		case 3:		ptRt = rRpt.T() & rRt; break;
		case 4:		ptRt = rRpt.T() & rRt; break;
		case 5:		ptRt = rRpt.T() & rRt; break;
		default:	ptRt = rRpt.T() & rRt;
	}

	theta = atan2( sqrt( pow(ptRt.yx()-ptRt.xy(),2.0) + pow(ptRt.xz()-ptRt.zx(),2.0) + pow(ptRt.zy()-ptRt.yz(),2.0) ), ptRt.xx() + ptRt.yy()+ ptRt.zz() - 1 );

	if( 1.0e-32 < mag(sin(theta)) ){
	    ptVaxis[2] = ptRt.yx() - ptRt.xy(); 
	    ptVaxis[1] = ptRt.xz() - ptRt.zx(); 
	    ptVaxis[0] = ptRt.zy() - ptRt.yz();
	    ptVaxis *= (theta/(2.0*sin(theta)));
	}

	moment = gain_[0] * ( rRb & bTaxis ) + gain_[1] * ( ( rRpt & ptVaxis ) / dt - angular_vel );

	double	torque = mag( moment );	
	if( gain_[2] < torque )	moment *= ( gain_[2] / torque );

	if( t < relaxation_time_  )	moment *= (t/relaxation_time_);

    if( model_.debug )
//    if ( RBD::rigidBodyMotion::Iteration_number_for_MB_ == 0 )
    {
	Info << endl << " **** rollpitchyawControlWaveCOM: **** " << endl
	<< model_.name(bodyID_) << "["<< bodyID_ << ", " << bodyIndex_ << "], " << reaction_ << "[" <<referenceID << "]" << endl
	<< "z: amp[deg] = " << deg(amp[0]) << ", freq = " << freq[0] << ", phase[deg] = " << deg(phase[0]) << ", shift[deg] = " << deg(shift[0]) << endl
	<< "y: amp[deg] = " << deg(amp[1]) << ", freq = " << freq[1] << ", phase[deg] = " << deg(phase[1]) << ", shift[deg] = " << deg(shift[1]) << endl
	<< "x: amp[deg] = " << deg(amp[2]) << ", freq = " << freq[2] << ", phase[deg] = " << deg(phase[2]) << ", shift[deg] = " << deg(shift[2]) << endl
	<< " gain = " << gain_ << ", relaxation time= " << relaxation_time_ << endl;

	Info 	<< "Target Rodrigues axis (difference) = " << ( rRb & bTaxis ) << " theta = " << mag(rRb &bTaxis) << endl
		<< "Target vel: ptVaxis / dt = " << ( rRpt & ptVaxis ) / dt << endl
		<< "angular velocity         = " << angular_vel << endl;
	Info	<< "rRb = " << rRb << endl;

	Info << "moment = " << moment << ", P= " << gain_[0] * ( rRb & bTaxis ) << ", D= " <<  gain_[1] * ( ( rRpt & ptVaxis ) / dt - angular_vel ) << endl;
	if( ( referenceID == 0 ) || ( bodyID_ == 0 ) )	Info << "The moment is not reacted to the root" << endl;
    }

    // Accumulate the ONLY MOMENT for the restrained body
    if( bodyIndex_ != 0 )	fx[bodyIndex_]  += spatialVector( Rr & moment, Zero /*force*/ );
    if( referenceID != 0 )	fx[referenceID] -= spatialVector( Rr & moment, Zero /*force*/ );
}


bool Foam::RBD::restraints::rollpitchyawControlWaveCOM::read
(
    const dictionary& dict
)
{
    restraint::read(dict);

    coeffs_.readEntry("z_angle", z_angle_);
    coeffs_.readEntry("y_angle", y_angle_);
    coeffs_.readEntry("x_angle", x_angle_);
    coeffs_.readEntry("gain", gain_);
    coeffs_.readEntry("relaxation_time", relaxation_time_);
    coeffs_.readEntry("reaction", reaction_);
    coeffs_.readEntry("rotation_order", rotation_order_);

    return true;
}


void Foam::RBD::restraints::rollpitchyawControlWaveCOM::write
(
    Ostream& os
) const
{
    restraint::write(os);

    os.writeEntry("z_angle", z_angle_);
    os.writeEntry("y_angle", y_angle_);
    os.writeEntry("x_angle", x_angle_);
    os.writeEntry("gain", gain_);
    os.writeEntry("relaxation_time", relaxation_time_);
    os.writeEntry("reaction", reaction_);
    os.writeEntry("rotation_order", rotation_order_);
}


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