// Example C++ function "CppModTemplate" dynamically loaded into "load.edp"
// ------------------------------------------------------------------------
// ff-c++ LevelSet.cpp 

#include <ff++.hpp>
#include "AFunction_ext.hpp" // Extension of "AFunction.hpp" to deal with more than 3 parameters function
using namespace Fem2D;

  
Matrice_Creuse<double> *Dmx; Matrice_Creuse<double> *Dmy; Matrice_Creuse<double> *Dpx; Matrice_Creuse<double> *Dpy;
Matrice_Creuse<double> *Dmxx; Matrice_Creuse<double> *Dmyy; Matrice_Creuse<double> *Dpxx; Matrice_Creuse<double> *Dpyy; Matrice_Creuse<double> *D0xx; Matrice_Creuse<double> *D0yy;

double dx,dy;
 
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline double G(const double &u1,const double &u2,const double &u3,const double &u4)
{
	 double  p1=max(0.,u1),  p2=min(0.,u2),  p3=max(0.,u3),  p4=min(0.,u4);
	 return sqrt(p1*p1+p2*p2+p3*p3+p4*p4);
}

inline double Plus(const double &u) 
{
	 double  p1=max(0.,u);
	 return p1;
}

inline double Moins(const double &u)
{
	 double  p1=min(0.,u);
	 return p1;
}

inline double Minmod(const double &a, const double &b)
{
	 double sa= a > 0. ? 1. : -1.;
	 double sb= b > 0. ? 1. : -1.;
	 return    max(0.,sa*sb)*sa*min(abs(a),abs(b));
}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

int Derivatives1(Matrice_Creuse<double> *const &dpx,Matrice_Creuse<double> *const &dpy,Matrice_Creuse<double> *const &dmx,Matrice_Creuse<double> *const &dmy)
{
	Dmx = dmx; Dmy = dmy; Dpx = dpx; Dpy = dpy;
	return 1;
}

int Derivatives2(Matrice_Creuse<double> *const &dpxx,Matrice_Creuse<double> *const &dpyy,Matrice_Creuse<double> *const &dmxx,Matrice_Creuse<double> *const &dmyy,Matrice_Creuse<double> *const &d0xx,Matrice_Creuse<double> *const &d0yy)
{
	Dmxx = dmxx; Dmyy = dmyy; Dpxx = dpxx; Dpyy = dpyy; D0xx = d0xx; D0yy = d0yy;
	return 1;
}

int Parameters(const double & dx_, const double & dy_)
{
	dx = dx_;dy = dy_;
	return 1;
}

//int HJUpwind(KN<double> *const & psi0,KN<double> *const & psi,KN<double> *const & v,const double & dtReini,const double & dtUpwind)

int HJ(KN<double> *const & psi,KN<double> *const & v,const long &Nupwind,const double & dtUpwind)
{
	KN<double> &Psi(*psi),&V(*v);
	int nm=Psi.N();
	
	//cout<<"nm="<<nm<<endl;
	KN<double> DmxPsi(nm), DpxPsi(nm), DmyPsi(nm), DpyPsi(nm),DmxxPsi(nm),D0xxPsi(nm),DpxxPsi(nm), DmyyPsi(nm),D0yyPsi(nm), DpyyPsi(nm),A(nm),B(nm),C(nm),D(nm),GammaP(nm),GammaM(nm),Vp(nm),Vm(nm);
	
	for(int i=0;i<Nupwind;i++)
	{// loop on i (Upwind scheme){	
		DmxPsi=*(Dmx->A)*Psi;DpxPsi=*(Dpx->A)*Psi;DmyPsi=*(Dmy->A)*Psi;DpyPsi=*(Dpy->A)*Psi;
		DmxxPsi=*(Dmxx->A)*Psi; DpxxPsi=*(Dpxx->A)*Psi; D0xxPsi=*(D0xx->A)*Psi;
		DmyyPsi=*(Dmyy->A)*Psi; DpyyPsi=*(Dpyy->A)*Psi; D0yyPsi=*(D0yy->A)*Psi;
	
		for(int ii=0;ii<nm;ii++)
		{ // loop on ii 
			A(ii)=DmxPsi(ii)+dx/2.*Minmod(DmxxPsi(ii),D0xxPsi(ii));
			B(ii)=DpxPsi(ii)-dx/2.*Minmod(DpxxPsi(ii),D0xxPsi(ii));
			C(ii)=DmyPsi(ii)+dy/2.*Minmod(DmyyPsi(ii),D0yyPsi(ii));
			D(ii)=DpyPsi(ii)-dy/2.*Minmod(DpyyPsi(ii),D0yyPsi(ii));
			// Gamma functions	
			GammaP(ii)=G(A(ii),B(ii),C(ii),D(ii));
			GammaM(ii)=G(B(ii),A(ii),D(ii),C(ii));
			Vm(ii)=Moins(V(ii));
			Vp(ii)=Plus(V(ii));
			Psi(ii)=Psi(ii)-dtUpwind*(Vm(ii)*GammaM(ii)+Vp(ii)*GammaP(ii));	
		} // loop on ii
	}// loop on i (Ipwind scheme)
	return 1;
}

int regularization(KN<double> *const & psi,const long &Nreg,const double & dtReini)
{ 
	KN<double> &Psi(*psi);
	int nm=Psi.N();
	KN<double> DmxPsi0(nm), DpxPsi0(nm), DmyPsi0(nm), DpyPsi0(nm),
		DmxPsi(nm), DpxPsi(nm), DmyPsi(nm), DpyPsi(nm),DmxxPsi(nm), DpxxPsi(nm), DmyyPsi(nm), DpyyPsi(nm), 
		D0xxPsi(nm), D0yyPsi(nm),A(nm),B(nm),C(nm),D(nm),GammaP(nm),GammaM(nm),S0(nm),Sp(nm),Sm(nm);	
	KN<double> Psi0 = Psi;
	{ // reinitialization
		DmxPsi0=*(Dmx->A)*Psi0;DpxPsi0=*(Dpx->A)*Psi0;DmyPsi0=*(Dmy->A)*Psi0;DpyPsi0=*(Dpy->A)*Psi0;			
		for(int j=0;j<Nreg;j++)
		{// loop on j (reinitialisation)
			// Computation of the differentials of Psi
			DmxPsi=*(Dmx->A)*Psi; DpxPsi=*(Dpx->A)*Psi; DmyPsi=*(Dmy->A)*Psi; DpyPsi=*(Dpy->A)*Psi;
			DmxxPsi=*(Dmxx->A)*Psi; DpxxPsi=*(Dpxx->A)*Psi; D0xxPsi=*(D0xx->A)*Psi; DmyyPsi=*(Dmyy->A)*Psi; DpyyPsi=*(Dpyy->A)*Psi; D0yyPsi=*(D0yy->A)*Psi;
			
			for(int ii=0;ii<nm;ii++)
			{// loop on ii  
				A(ii)=DmxPsi(ii)+dx/2.*Minmod(DmxxPsi(ii),D0xxPsi(ii));
				B(ii)=DpxPsi(ii)-dx/2.*Minmod(DpxxPsi(ii),D0xxPsi(ii));
				C(ii)=DmyPsi(ii)+dy/2.*Minmod(DmyyPsi(ii),D0yyPsi(ii));
				D(ii)=DpyPsi(ii)-dy/2.*Minmod(DpyyPsi(ii),D0yyPsi(ii));
				// Gamma functions	
				GammaP(ii)=G(A(ii),B(ii),C(ii),D(ii));
				GammaM(ii)=G(B(ii),A(ii),D(ii),C(ii));
				// S functions
				S0(ii)=Psi0(ii)/sqrt(Psi0(ii)*Psi0(ii)+max(dx,dy)*
				(DmxPsi0(ii)*DmxPsi0(ii)+DpxPsi0(ii)*DpxPsi0(ii)+DmyPsi0(ii)*DmyPsi0(ii)+DpyPsi0(ii)*DpyPsi0(ii))/20.);
		/*
                                      +max(dx,dy)*(DmxPsi0(ii)*DmxPsi0(ii)+DpxPsi0(ii)*DpxPsi0(ii)
                                                  +DmyPsi0(ii)*DmyPsi0(ii)+DpyPsi0(ii)*DpyPsi0(ii))/20.);*/
				Sp(ii)=Plus(S0(ii));
				Sm(ii)=Moins(S0(ii));	
				Psi(ii)=Psi(ii)-dtReini*(Sm(ii)*GammaM(ii)+Sp(ii)*GammaP(ii)-S0(ii));
			}// loop on ii			
		}// loop on j (reinitialisation)	
	} // reinitialization
	Psi0=Psi;
	return 1;
}

//   add the function name to the freefem++ table 
class Init { public:
  Init();
};
Init init;
Init::Init(){
  // Add function with 3 arguments

  Global.Add("HJ","(",new OneOperator4_<int, KN<double>*, KN<double>*,long,double>(HJ)); 
  Global.Add("regularization","(",new OneOperator3_<int, KN<double>*,const long,const double>(regularization)); 
  Global.Add("Parameters","(",new OneOperator2_<int,double,double>(Parameters)); 
  Global.Add("Derivatives1","(",new OneOperator4_<int, Matrice_Creuse<double> *,Matrice_Creuse<double> *,Matrice_Creuse<double> *,Matrice_Creuse<double> *>(Derivatives1)); 
  Global.Add("Derivatives2","(",new OneOperator6_<int,Matrice_Creuse<double> *,Matrice_Creuse<double> *,Matrice_Creuse<double> *,
	     Matrice_Creuse<double> *,Matrice_Creuse<double> *,Matrice_Creuse<double> *>(Derivatives2)); 
}