#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include "sopnamsp.h"
#include "mollweide.h"

/*! \ingroup XAstroPack
 \brief La vraie projection Mollweide (longitude,latitude) -> (xmoll,ymoll)
 \param input: longitude [0,2PI[ en radians
 \param input: latitude  [-Pi/2,Pi/2] en radians
 \param output: xmoll dans [-2*sqrt(2),2*sqrt(2)]
 \param output: ymoll dans [-sqrt(2),sqrt(2)]
 \warning Probleme de precision pour t pres de Pi/2 (latitude pres de -Pi/2 ou Pi/2)
 \warning Longitude=Pi=180 au milieu en xmoll=0
 \return t
 \verbatim
 - La vraie MollWeide:
 Equation implicite en t: 2*t+sin(2*t) = Pi*sin(lat) = a
     | xmoll = 2*sqrt(2)*(lon-Pi)/Pi*cos(t)
     | ymoll = sqrt(2)*sin(t)
 --------------------------------------------------------
 INPUT: longitude [0,2PI[      en radians
        latitude  [-Pi/2,Pi/2] en radians
 OUTPUT: xmoll dans [-2*sqrt(2),2*sqrt(2)]
         ymoll dans [-sqrt(2),sqrt(2)]
 RETURN: t
 --------------------------------------------------------
 REMARQUES:
 - xmoll varie [-2*sqrt(2),2*sqrt(2)]
        centre galactique (longitude=0) au bord,
        anti-centre (longitude=180) au milieu.
   pour centre galactique (longitude=0) au milieu,
        anti-centre (longitude=180) au bord:
        xmoll' = 2*sqrt(2)*longitude/Pi*cos(t) = xmoll+2*sqrt(2)
        xmoll' varie [0,4*sqrt(2)]
 - ymoll varie [-sqrt(2),sqrt(2)] plan gal en 0
 - t varie [-Pi/2,Pi/2]
 - a varie [-Pi,Pi]  (a <-> -a  <==> t <-> -t)
 - Pour latitude --> -latitude alors t --> -t
        latitude=90   --> t=Pi/2
        latitude=-90  --> t=-Pi/2
 RAPIDITE: 10^6 appels a MollWeide_XY en 5.2 seconde (PC Linux 500Mhz)
 PRECISION: eps=1e-16  -> |t-tvrai|<1e-6 toujours
                          |t-tvrai|<1e-10 pour t<1.57
                          |t-tvrai|<1e-10 pour t<1.57
                          |t-tvrai|<3e-14 pour t<1.50
                          |t-tvrai|<5e-15 pour t<1.40
 \endverbatim
*/
double MollWeide_XY(double longitude,double latitude,double& xmoll,double& ymoll)
{
 int i,niter=2;
 double tini,t0,f0,f1,f2,e1,e2,dt,a,asgn,eps=1.e-16;
 
 a = M_PI*sin(latitude);
 if(a<0.) {a*=-1.; asgn=-1.;} else asgn=1.;
 // premier guess t1 ok pour t->0 (droite)
 f1 = a/4.;
 // second guess t2 ok pour t->Pi/2 (DL en Pi/2, type a+b*x^3)
 f2 = 0.75*(M_PI-a);
 if(f2>0.) f2=M_PI_2-pow(f2,0.33333333333333333); else f2=M_PI_2;
 e1 = 2.*f1+sin(2.*f1)-a; if(e1<0.) e1 *= -1.;
 e2 = 2.*f2+sin(2.*f2)-a; if(e2<0.) e2 *= -1.;
 tini = (f1*e2+f2*e1)/(e1+e2);
 //printf("---> a=%e tini=%e f1=%e f2=%e\n",a,tini,f1,f2);
 if(tini<0) tini=0.; else if(tini>M_PI_2) tini=M_PI_2;
 // L'approximation par le DL au 2sd ordre, on resoud:
 // a = f(x0) + f'(x0)*(x-x0) + f''(x0)*(x-x0)^2/2
 t0 = tini;
 for(i=0;i<niter;i++) {
   e1 = sin(2.*t0);
   f0 = 2.*t0+e1;           // (f(x0))     >=0 sur 0<t<Pi/2
   //printf(".%d    a-f0=%e\n",i,a-f0);
   if(a-f0<eps && a-f0>-eps) break;
   f1 = 2.*(1.+cos(2.*t0)); // (f'(x0))    >=0 sur 0<t<Pi/2
   f2 = -2.*e1;             // (f''(x0)/2) <=0 sur 0<t<Pi/2
   dt = f1*f1 - 4.*(f0-a)*f2;
   //printf(".%d    f0=%e f1=%e f2=%e disc=%e\n",i,f0,f1,f2,dt);
   if(dt>=0.) dt=-f1+sqrt(dt); else dt=0.;
   ////printf(".%d    -b+sqrt(disc)=%e\n",i,dt);
   if(f2<-eps && (dt>eps || dt<-eps)) { // approx parabolique
     t0 += dt/(2.*f2);
     //printf(".%d  parab  t0=%e\n",i,t0);
   } else if(f1>eps) {
     t0 += (a-f0)/f1; // approx lineaire
     //printf(".%d  lin  t0=%e\n",i,t0);
   }
   if(t0<0) t0=0.; else if(t0>M_PI_2) t0=M_PI_2;
 }

 t0 *= asgn;
 xmoll = 2.*M_SQRT2*(longitude-M_PI)/M_PI*cos(t0);
 ymoll = M_SQRT2*sin(t0);

 return t0;
}

/*! \ingroup XAstroPack
 \brief La vraie projection Mollweide (xmoll,ymoll) -> (longitude,latitude)
 \param input: xmoll dans [-2*sqrt(2),2*sqrt(2)]
 \param input: ymoll dans [-sqrt(2),sqrt(2)]
 \param output: longitude [0,2PI[ en radians
 \param output: latitude  [-Pi/2,Pi/2] en radians
 \return t
 \warning Probleme de precision pour t pres de Pi/2 (latitude pres de -Pi/2 ou Pi/2)
 \warning Longitude=Pi=180 au milieu en xmoll=0
 \verbatim
 --------------------------------------------------------
 INPUT: xmoll dans [-2*sqrt(2),2*sqrt(2)]
        ymoll dans [-sqrt(2),sqrt(2)]
 OUTPUT: longitude [0,2PI[      en radians
         latitude  [-Pi/2,Pi/2] en radians
 RETURN: t if OK
         -991 : xmoll hors limites
         -992 : ymoll hors limites
         -993 : non inversible (poles de la sphere)
 --------------------------------------------------------
 \endverbatim
*/
double MollWeide_LL(double xmoll,double ymoll,double& longitude,double& latitude)
{
 double t,a,eps=1.e-20;

 longitude = latitude = -999.;
 if(xmoll<-2*M_SQRT2 || xmoll>2*M_SQRT2) return(-901.);
 if(ymoll<-M_SQRT2 || ymoll>M_SQRT2) return(-902.);

 t = ymoll/M_SQRT2; if(t<-1.) t=-1.; else if(t>1.) t=1.;
 t = asin(t);

 a = (2.*t+sin(2.*t))/M_PI; if(a<-1.) a=-1.; else if(a>1.) a=1.;
 latitude = asin(a);

 a= cos(t);
 if(fabs(a)<eps) return(-903.);
 longitude = M_PI*(xmoll/(2.*M_SQRT2*a)+1.);

 return(t);
}