/*
 * Morschhauser.hh
 *
 *  Created on: May 30, 2016
 *      Author: AKKA IS
 */

#ifndef MORSCHHAUSER_HH_
#define MORSCHHAUSER_HH_

#include "morschhauser_constants.hh"

#include "Parameter.hh"
#include "ParamData.hh"
#include "DataTypeMath.hh"
#include "Operation.hh"
#include "SpiceKernelMgr.hh"

namespace AMDA {
namespace Parameters {

/**
 * @class MorschhauserCommon
 * @brief It is responsible to compute Morshhauser Mars Model Magnetic Field along an orbit. Abstract class
 * @details This class implement the interface Operation. Input vector given in MSO coordinates system.
 */
template<typename DataType, class TOutputParamData>
class MorschhauserCommon : public Operation {
public:
	/**
	 * @brief Constructor.
	 * @details Create the ParamData type of the input ParamData.
	 */
	MorschhauserCommon(Process& pProcess, ParamDataSpec<std::vector<DataType> >&paramInput): Operation(pProcess),
	  _paramInput(paramInput),
	  _paramOutput(new TOutputParamData())
	{
		_paramDataOutput=_paramOutput;

		init_coef_V_int();
		init_coef_V_ext_night();
		init_coef_V_ext_day();
	}

	virtual ~MorschhauserCommon() {
	}

	/**
	 * @overload Operation::write(ParamDataIndexInfo &pParamDataIndexInfo)
	 */
	void write(ParamDataIndexInfo &pParamDataIndexInfo) {
		for (unsigned int _index = pParamDataIndexInfo._startIndex ;
				_index < pParamDataIndexInfo._startIndex + pParamDataIndexInfo._nbDataToProcess;
				++_index)
		{
			std::vector<DataType> lVal = _paramInput.getDataList()[_index];
			double crtTime = _paramInput.getTime(_index);

			std::vector<DataType> input;
			input.resize(3);

			double MSO2PC[9];
			double JulDay = SpiceKernel::SpiceKernelMgr::getInstance()->timeStampToJulianDate(crtTime);
			mso2pc(JulDay, MSO2PC);

			input[0] = MSO2PC[0]*lVal[0]+ MSO2PC[1]*lVal[1]+ MSO2PC[2]*lVal[2];
			input[1] = MSO2PC[3]*lVal[0]+ MSO2PC[4]*lVal[1]+ MSO2PC[5]*lVal[2];
			input[2] = MSO2PC[6]*lVal[0]+ MSO2PC[7]*lVal[1]+ MSO2PC[8]*lVal[2];

			short sign = (lVal[0] > 0) ? 1 : 0;

			std::vector<DataType> outputB;
			outputB.resize(3);

			DataType outputBm;
			field_(input, sign, outputB, outputBm);

			std::vector<DataType> outputBMSO;
			outputBMSO.resize(3);

			outputBMSO[0] = MSO2PC[0]*outputB[0] + MSO2PC[3]*outputB[1] + MSO2PC[6]*outputB[2];
			outputBMSO[1] = MSO2PC[1]*outputB[0] + MSO2PC[4]*outputB[1] + MSO2PC[7]*outputB[2];
			outputBMSO[2] = MSO2PC[2]*outputB[0] + MSO2PC[5]*outputB[1] + MSO2PC[8]*outputB[2];

			_paramOutput->pushTime(crtTime);
			pushResult(outputBMSO, outputBm);
		}
	}

protected:

	virtual void pushResult(std::vector<DataType> field, DataType magnitude_) = 0;

	/**
	 * @brief Input paramter data.
	 */
	ParamDataSpec<std::vector<DataType> >& _paramInput;

	/**
	 * @brief Output parameter data.
	 */
	TOutputParamData *_paramOutput;

private:
	long double _coef_V_int[111][221], _coef_V_ext_day[6][11], _coef_V_ext_night[11][21];

	int field_(std::vector<DataType> inputElt, short sign, std::vector<DataType>& outputB, DataType& outputBm) {
		long double r,dtheta,dphi,x,y,z;

		x =  (long double)inputElt[0];
		y =  (long double)inputElt[1];
		z =  (long double)inputElt[2];

		r = sqrtl( x * x + y * y + z * z );

		dtheta = acosl( z / r );
		dphi   = atanl( y / x );

		if ( x < 0.0 )
		{
			dphi += M_PIl;
		}

		if ( r < Rlim )
		{
			long double P[111][111]={ {0.0L} , {0.0L} };
			long double cos_theta, sin_theta, cos_phi, sin_phi;
			long double Br, Btheta, Bphi;
			cos_theta = cosl( dtheta );
			sin_theta = sinl( dtheta );
			cos_phi = cosl( dphi );
			sin_phi = sinl( dphi );

			plgndr( cos_theta, P );
			V_int( Rma * r , dtheta , dphi, sign, P, Br, Btheta, Bphi);
			outputBm   = (double)sqrtl( Br * Br + Btheta * Btheta + Bphi * Bphi );
			outputB[0] = (double)(Br * cos_phi * sin_theta - Bphi * sin_phi  + Btheta * cos_phi * cos_theta);
			outputB[1] = (double)(Br * sin_phi * sin_theta + Bphi * cos_phi);
			outputB[2] = (double)(Br * cos_theta - Btheta * sin_theta);
		}
		else
		{
			outputBm << NotANumber();
			outputB << NotANumber();
		}
		return 0;
	}

	void init_coef_V_int(void) {
		for (int i = 0; i < V_INT_SIZE; ++i)
		{
			_coef_V_int[(int)V_INT[i][0]][(int)(V_INT[i][0]+V_INT[i][1])] = V_INT[i][2];
		}
	}

	void init_coef_V_ext_night(void) {
		for (int i = 0; i < V_EXT_NIGHT_SIZE; ++i)
		{
			_coef_V_ext_night[(int)V_EXT_NIGHT[i][0]][(int)(V_EXT_NIGHT[i][0]+V_EXT_NIGHT[i][1])] = V_EXT_NIGHT[i][2];
		}
	}

	void init_coef_V_ext_day(void) {
		for (int i = 0; i < V_EXT_DAY_SIZE; ++i)
		{
			_coef_V_ext_day[(int)V_EXT_DAY[i][0]][(int)(V_EXT_DAY[i][0]+V_EXT_DAY[i][1])] = V_EXT_DAY[i][2];
		}
	}

	int  V_int(long double r , long double theta , long double phi, short sign, long double P[111][111],
			long double& Br, long double& Btheta, long double& Bphi) {
		long double x,y,z,Ypm,Ymm, Ypmav,Ymmav;
		int l,m;
		long double tan_theta, sin_theta, cos_theta;

		tan_theta = tanl( theta );
		sin_theta = sinl( theta );
		cos_theta = cosl( theta );

		Br     = 0.0L;
		Bphi   = 0.0L;
		Btheta = 0.0L;
		x = ( y = ( z = 0.0L ) );
		Ypm = ( Ypmav = 0.0L );
		Ymm = ( Ymmav = 0.0L );

		for (l = 1 ; l <= 110 ; l++ )
		{
			long double V1 = 0.0L;
			long double V2 = 0.0L;
			long double V3 = 0.0L;
			long double V4 = 0.0L;
			long double V5 = 0.0L;
			long double V6 = 0.0L;
			Ypmav = 0.0L;
			Ymmav = 0.0L;

			for ( m = l ; m > 0 ; m-- )
			{
				Ypm = Ylm( l ,  m , theta , phi, P );
				Ymm = Ylm( l , -m , theta , phi, P );

				V1 += _coef_V_int[l][l+m] * Ypm;
				V1 += _coef_V_int[l][l-m] * Ymm;

				V2 -= m * _coef_V_int[l][l+m] * Ymm;
				V2 += m * _coef_V_int[l][l-m] * Ypm;

				if( sign == 0 && l <= 10 ) // night side MSO
				{
					V4 += _coef_V_ext_night[l][l+m] * Ypm;
					V4 += _coef_V_ext_night[l][l-m] * Ymm;
					V5 -= m * _coef_V_ext_night[l][l+m] * Ymm;
					V5 += m * _coef_V_ext_night[l][l-m] * Ypm;
					V6 += _coef_V_ext_night[l][l+m] * ( m * Ypm / tan_theta - sqrtl( l - m + l * l - m * m ) * cosl( m * phi ) / cosl( ( m + 1.0l ) * phi ) * Ypmav );
					V6 += _coef_V_ext_night[l][l-m] * ( m * Ymm / tan_theta - sqrtl( l - m + l * l - m * m ) * sinl( m * phi ) / sinl( ( m + 1.0l ) * phi ) * Ymmav );
				}

				if( sign > 0 && l <= 5 )  // day side MSO
				{
					V4 += _coef_V_ext_day[l][l+m] * Ypm;
					V4 += _coef_V_ext_day[l][l-m] * Ymm;
					V5 -= m * _coef_V_ext_day[l][l+m] * Ymm;
					V5 += m * _coef_V_ext_day[l][l-m] * Ypm;
					V6 += _coef_V_ext_day[l][l+m] * ( m * Ypm / tan_theta - sqrtl( l - m + l * l - m * m ) * cosl( m * phi ) / cosl( ( m + 1.0l ) * phi ) * Ypmav );
					V6 += _coef_V_ext_day[l][l-m] * ( m * Ymm / tan_theta - sqrtl( l - m + l * l - m * m ) * sinl( m * phi ) / sinl( ( m + 1.0l ) * phi ) * Ymmav );
				}

				if ( phi != 0.0L )
				{
					V3 += _coef_V_int[l][l+m] * ( m * Ypm / tan_theta - sqrtl( l - m + l * l - m * m ) * cosl( m * phi ) / cosl( ( m + 1.0l ) * phi ) * Ypmav );
					V3 += _coef_V_int[l][l-m] * ( m * Ymm / tan_theta - sqrtl( l - m + l * l - m * m ) * sinl( m * phi ) / sinl( ( m + 1.0l ) * phi ) * Ymmav );
				}
				else
				{
					V3 += _coef_V_int[l][l+m] * ( m * Ypm / tan_theta - sqrtl( l - m + l * l - m * m ) * Ypmav );
				}

				Ypmav = Ypm;
				Ymmav = Ymm;
			}

			V1 +=   _coef_V_int[l][l] * Ylm( l , 0 , theta , phi, P );

			if( sign == 0 && l <= 10 ) // night side MSO
			{
				V4 +=   _coef_V_ext_night[l][l] * Ylm( l , 0 , theta , phi, P );
			}

			if( sign > 0 && l <= 5 ) // day side MSO
			{
				V4 +=   _coef_V_ext_day[l][l] * Ylm( l , 0 , theta , phi, P );
			}

			V3 +=   _coef_V_int[l][l] * l / sin_theta * ( cos_theta * P[l][0]-P[l-1][0] );

			if( sign == 0 && l <= 10 ) // night side MSO
			{
				V6 +=   _coef_V_ext_night[l][l] * l / sin_theta * ( cos_theta * P[l][0]-P[l-1][0] );
			}

			if( sign > 0 && l <= 5 ) // day side MSO
			{
				V6 +=   _coef_V_ext_day[l][l] * l / sin_theta * ( cos_theta * P[l][0]-P[l-1][0] );
			}


			long double puis = powl( Rma / r , l + 2.0L );
			long double puis2= powl( r / Rma , l - 1.0L );

			z -= - ( l + 1.0L ) * V1 * puis + l * V4 * puis2; //minus sign disappear because magnetic field is the ooposite of the gradient
			x -=                  ( V2 * puis + V5 * puis2 ) / sin_theta;
			y -=                  ( V3 * puis + V6 * puis2 );

		}

		Br = z;
		Bphi = x;
		Btheta= y;

		return 0;
	}

	void plgndr(double x, long double P[111][111]) {
		int m,l;
		double fact,coef;

		fact = 1.0;
		coef = sqrt( 1.0 - x * x );
		P[0][0] = 1.0;


		for ( m = 1 ; m <= 110 ; m++ )
		{

			P[m][m] = - P[m-1][m-1] * fact * coef;
			fact += 2.0;
		}

		for ( m = 0 ; m <= 109 ; m++ )
		{
			P[m+1][m] = x * ( 2.0 * m + 1.0 ) * P[m][m];
		}

		for ( m = 0 ; m <= 108 ; m++ )
		{
	        for ( l = m + 2 ; l <= 110 ; l++ )
	        {
	        	P[l][m] = ( x * ( 2.0 * l - 1.0 ) * P[l-1][m] - ( l + m - 1.0 ) * P[l-2][m] ) / ( l - m );
	        }

		}
	}

	long double Ylm(int l , int m , long double /*theta*/ , long double phi, long double P[111][111]) {
		long double Plm;
		long double p, sqrt2 = sqrtl( 2.0L );

		Plm = P[l][abs(m)];

		p = 1;

		if ( m != 0 )
		{
	        for ( int k = (l - abs(m) + 1 ) ; k <= ( l + abs(m) ) ; k++)
	        {
	        	p *= sqrtl( (long double) k );
	        }
	        Plm /= p;
	        Plm *= sqrt2;
		}

		if ( m < 0 )
		{
			Plm *= sinl( fabsl( m ) * phi );
		}
		else if ( m > 0 )
		{
	        Plm *= cosl(  fabsl( m ) * phi );
		}

		Plm *= powl( - 1.0L , fabsl( m ) );

		if ( abs(m) > l )
		{
			Plm = 0.0L;
		}

		return Plm;
	}

	void mso2pc(double JDay, double MSO2PC[9])
	{
		int i;
		double deg2pi = M_PI /180.0, M,  Afms, PBS = 0.0, Ls, dTJ2000, Vm, As;
		double A[7] =  {7.e-3, 6.e-3,4.e-3, 4.e-3, 2.e-3, 2.e-3, 2.e-3};
		double tau[7] = {2.2353, 2.7543, 1.1177, 15.7866, 2.1354, 2.4694, 32.8493};
		double phi[7] = {49.409, 168.173, 191.837, 21.736, 15.704, 95.528, 49.095};
		double  ang = 0.985626, Tilt = 25.19;
		double cosT, sinT, cosN, sinN, cosC, sinC;

		dTJ2000 =  JDay -  JD2000;

	    /*-------------------- Mean Anomaly ----------------------*/
	    M =  (19.3870 + 0.52402075*dTJ2000)*deg2pi;

	    /*------------------ Fictitious Mean Sun Angle ------------*/
	    Afms = 270.3863 + 0.52403840*dTJ2000;

	    /*--------------------   Primary short-term perturbations --------*/
	    ang *= dTJ2000;
	    for ( i = 0; i < 7; i++ )
	    	PBS = PBS+A[i]*cos((ang/tau[i]+phi[i])*deg2pi);

	    /*---------------------- areocentric solar longitude  Ls ---------------------------*/
	    Ls =   Afms + (10.691+ 3.e-7*dTJ2000)*sin(M) + 0.623*sin(2.0*M) +
	    		5.e-2*sin(3.0*M) + 5.e-3*sin(4.0*M) + 5.e-4*sin(5.0*M) +PBS;
	    Ls -= (int)(Ls/360.0)*360.0;
	    Ls *= deg2pi;
	    /*---------------- Prime Meridian degrees from Vernal Equinox---------------------*/
	    Vm =  (313.384 + 350.891985* dTJ2000) * deg2pi;
	    //         Vm =  (176.63 + 350.89198226* dTJ2000) * deg2pi;

	    /*---------------------Sub solar Longitude degrees from Vernal Equinox---------*/
	    Tilt *= deg2pi;
	    As = atan(cos(Tilt)*tan(Ls));
	    if (Ls >= M_PI/2.0 && Ls < M_PI*3.0/2.0) As = As + M_PI;
	    if (Ls >= M_PI*3.0/2.0 && Ls < M_PI*2.0) As = As + 2.0*M_PI;

	    /*----------------------------  transformatiom matrix -----------------------*/
	    cosT = cos(Tilt*sin(Ls));
	    sinT = sin(Tilt*sin(Ls)) ;
	    cosC = cos(Tilt*cos(Ls)) ;
	    sinC = sin(Tilt*cos(Ls));
	    cosN = cos(-Vm + As);
	    sinN = sin(-Vm + As);

	    MSO2PC[0] =  cosT*cosN;
	    MSO2PC[1] = -(sinC*sinT*cosN+cosC*sinN);
	    MSO2PC[2] = -(cosC*sinT*cosN-sinC*sinN);

	    MSO2PC[3] = cosT*sinN;
	    MSO2PC[4] =  -(sinC*sinT*sinN-cosC*cosN);
	    MSO2PC[5] =  -(cosC*sinT*sinN+sinC*cosN);

	    MSO2PC[6] =  sinT;
	    MSO2PC[7] = sinC*cosT;
	    MSO2PC[8] =  cosC*cosT;
	}
};

/**
 * @class MorschhauserBField
 * @brief It is responsible to compute Morshhauser Mars Model Magnetic Field along an orbit.
 * @details Input vector given in MSO coordinates system.
 */
template<typename DataType>
class MorschhauserBField : public MorschhauserCommon<DataType, ParamDataSpec<std::vector<DataType> >> {
public:
	/**
	 * @brief Constructor.
	 * @details Create the ParamData type of the input ParamData.
	 */
	MorschhauserBField(Process& pProcess, ParamDataSpec<std::vector<DataType> >& paramInput) : MorschhauserCommon<DataType, ParamDataSpec<std::vector<DataType> >>(pProcess, paramInput) {
	}

	virtual ~MorschhauserBField() {

	}

protected:
	virtual void pushResult(std::vector<DataType> field, DataType /*magnitude_*/) {
		MorschhauserCommon<DataType, ParamDataSpec<std::vector<DataType> >>::_paramOutput->getDataList().push_back(field);
	}
};

/**
 * @class MorschhauserBMag
 * @brief It is responsible to compute Morshhauser Mars Model Magnetic Field along an orbit.
 * @details Input vector given in MSO coordinates system.
 */
template<typename DataType>
class MorschhauserBMag : public MorschhauserCommon<DataType, ParamDataSpec<DataType>> {
public:
	/**
	 * @brief Constructor.
	 * @details Create the ParamData type of the input ParamData.
	 */
	MorschhauserBMag(Process& pProcess, ParamDataSpec<std::vector<DataType> >& paramInput) : MorschhauserCommon<DataType, ParamDataSpec<DataType>>(pProcess, paramInput) {
	}

	virtual ~MorschhauserBMag() {
	}

protected:
	virtual void pushResult(std::vector<DataType> /*field*/, DataType magnitude_) {
		MorschhauserCommon<DataType, ParamDataSpec<DataType>>::_paramOutput->getDataList().push_back(magnitude_);
	}
};

} /* namespace Parameters */
} /* namespace AMDA */
#endif /* MORSCHHAUSER_HH_ */