// check_sphere_deriv.c  sphereical coordinates Phi angle from Z
// x = r cos(theta) sin(phi)    0 < theta < 2 Pi
// y = r sin(theta) sin(phi)    0 < phi < Pi restricted for derivatives
// z = r cos(phi)               r < 0        restricted for derivatives
// r = sqrt(x^2+y^2+z^2)
// c = sqrt(x^2+y^2)            shortcut length in X Y plane
// theta = arctan(y,x) if theta<0 then theta = 2 Pi - theta
// theta = acos(x/c)
// theta = asin(y/c)
// phi = arctan(c,z)
// phi = acos(c/r)
// phi = asin(z/r)
//
// compute del U(radius,theta,phi) r=radius, t=theta angle, p=phi angle
// del U  =  (d/dr U, 1/(r*sin(p)) d/dt U, (1/r) d/dp U)
//
// compute del^2 U(radius,theta,phi) r=radius, t=theta angle, p=phi angle
// del^2 U  = (1/r^2) d/dr(r^2 dU/dr) + 1/(r^2 sin(p)^2) d^2U/dt^2 +
//            (1/(r^2 sin(p) d/dp(sin(p) dU/dp)
//          = (2/r) dU/dr + d^2U/dr^2 + (1/(r^2 sin(p)^2) d^2U/dt^2 +
//            (cos(p)/(r^2 sin(p)) dU/dp + (1/(r^2)) d^2U/dp^2

#include <stdio.h>
#include <math.h>
#include "nuderiv.h"
#undef  abs
#define abs(a) ((a)<0.0?(-(a)):(a))
#undef  min
#define min(a,b) ((a)<(b)?(a):(b))
#undef  max
#define max(a,b) ((a)>(b)?(a):(b))

// number of radius 
#define Nr 5
// number of theta angles
#define Nt 7
// number of phi angles                                            
#define Np 5
// solution points
#define Nrtp Nr*Nt*Np
static const  double Pi = 3.1415926535897932384626433832795028841971;

// solution for boundary and checking                                                        

  static double Uxyz(double x, double y, double z)
  {
    double rtr, ttr, ptr;
    rtr = sqrt(x*x+y*y+z*z);
    ttr = atan2(y, x);
    if(ttr<0.0) ttr = 2.0*Pi + ttr; // atan2 -Pi to Pi fix
    ptr = atan2(sqrt(x*x+y*y),z);
    return rtr*rtr*rtr + ttr*ttr*ttr + ptr*ptr*ptr;
  } // end Uxyz

  static double U(double R, double T, double P)
  {
    return R*R*R + T*T*T + P*P*P;
  } // end U

  static double Ur(double R, double T, double P)
  {
    return 3.0*R*R;
  } // end Ur

  static double Urr(double R, double T, double P)
  {
    return 6.0*R;
  } // end Urr;

  static double Ut(double R, double T, double P)
  {
    return 3.0*T*T;
  } // end Ut

  static double Utt(double R, double T, double P)
  {
    return 6.0*T;
  } // end Utt;

  static double Up(double R, double T, double P)
  {
    return 3.0*P*P;
  } // end Up

  static double Upp(double R, double T, double P)
  {
    return 6.0*P;
  } // end Upp

  static double Uuu(double R, double T, double P)
  {
    return 2.0*Ur(R,T,P)/R + Urr(R,T,P) + 
	   Utt(R,T,P)/(R*R*sin(P)*sin(P)) +
           cos(P)*Up(R,T,P)/(R*R*sin(P)) + 
	   Upp(R,T,P)/(R*R);
  } // end Uuu

  static double D2(double Urv, double Urrv, double Uttv, double Upv,
		   double Uppv, double R, double P)
  {
    return 2.0*Urv/R + Urrv + 
	   Uttv/(R*R*sin(P)*sin(P)) + 
	   cos(P)*Upv/(R*R*sin(P)) + 
           Uppv/(R*R);
  } // end D2

  static int S(int I, int J, int K)
  { // index of each grid point
    return I*Nt*Np + J*Np + K;
  } // end S

int main(int argc, char * argv[]) // check_sphere_deriv
{
  double Rmin = 0.3;
  double Rmax = 1.0;
  double Tmin = 0.1;
  double Tmax = 300.0*Pi/180.0;
  double Pmin = 0.2;
  double Pmax = Pi-0.5;
  double Rg[Nr]; // r grid values
  double Tg[Nt]; // theta grid values
  double Pg[Np]; // p grid values
  double Ua[Nrtp];
  double Cr[Nr]; // first deriv wrt r
  double Crr[Nr]; // second deriv wrt r
  double Ct[Nt]; // first deriv wrt t
  double Ctt[Nt]; // second deriv wrt t
  double Cp[Np]; // first deriv wrt p
  double Cpp[Np]; // second deriv wrt p
  double R, T, P, Ana, Cmp, Err;
  double Urv, Urrv, Utv, Uttv, Upv, Uppv; // computed derivatives
  double x, y, z, c, rtr, ttr, ptr;
  double Maxerrur=0.0, Maxerrurr=0.0, Maxerrut=0.0, Maxerrutt=0.0;
  double Maxerrup=0.0, Maxerrupp=0.0, Maxerr=0.0, Maxerrlap=0.0;
  double x2, y2, z2, R2, R3, T2, P2, T3, P3;
  double dx, dy, dz, dR, dT, dP, dvol; 
  int I, J, K, Ii, Jj, Kk;

    printf("check_sphere_deriv.c starting\n");
    printf("quick numeric check on equations in comments\n");
    x = 0.6;
    y = 0.7;
    z = 0.8;
    printf("x=%f, y=%f, z=%f \n", x, y, z);
    R = sqrt(x*x+y*y+z*z);
    c = sqrt(x*x+y*y); // short cut
    P = atan2(c,z);
    P2 = acos(z/R);
    P3 = asin(c/R);
    printf("P atan2(z,c)=%f, acos(z/r)=%f, asin(c/r)=%f \n", P, P2, P3);
    
    T = atan2(y,x);
    T2 = acos(x/c);
    T3 = asin(y/c);
    printf("T atan2(y,x)=%f, acos(x/c)=%f, asin(y/c)=%f \n", T, T2, T3);

    printf("R=%f, T=%f, P=%f \n", R, T, P);
    x2 = R*sin(P)*cos(T);
    y2 = R*sin(P)*sin(T);
    z2 = R*cos(P);
    printf("x2=%f, y2=%f, z2=%f \n", x2, y2, z2);
    printf(" \n");

    printf("simple derivative checks dx,dy,dz to dR,dT,dP\n");
    printf("have x,y,z R,T,P from above \n");
    dx = 0.01;
    x2 = x+dx;
    R2 = sqrt(x2*x2+y*y+z*z);
    c = sqrt(x2*x2+y*y); // short cut
    P2 = atan2(c,z);
    T2 = atan2(y,x2);
    dR = R2 - R;  //  /dx ??
    dT = T2 - T;
    dP = P2 - P;
    printf("dx=%f, dR=%f, dT=%f, dP=%f \n", dx, dR, dT, dP);
    dy = 0.015;
    y2 = y+dy;
    R2 = sqrt(x*x+y2*y2+z*z);
    c = sqrt(x*x+y2*y2); // short cut
    P2 = atan2(c,z);
    T2 = atan2(y2,x);
    dR = R2 - R;  //  /dy   ??
    dT = T2 - T;
    dP = P2 - P;
    printf("dy=%f, dR=%f, dT=%f, dP=%f \n", dy, dR, dT, dP);
    dz = 0.02;
    z2 = z+dz;
    R2 = sqrt(x*x+y*y+z2*z2);
    c = sqrt(x*x+y*y); // short cut
    P2 = atan2(c,z2);
    T2 = atan2(y,x);
    dR = R2 - R;  //  /dz  ??
    dT = T2 - T;
    dP = P2 - P;
    printf("dz=%f, dR=%f, dT=%f, dP=%f \n", dz, dR, dT, dP);
    R2 = sqrt(x2*x2+y2*y2+z2*z2);
    c = sqrt(x2*x2+y2*y2); // short cut
    P2 = atan2(c,z2);
    T2 = atan2(y2,x2);
    dR = R2 - R;
    dT = T2 - T;
    dP = P2 - P;
    printf("dall, dR=%f, dT=%f, dP=%f \n", dR, dT, dP);
    printf(" \n");

    printf("simple derivative checks dR,dT,dP to dx,dy,dz\n");
    printf("have x,y,z R,T,P from above \n");
    dR = 0.01;
    R2 = R+dR;
    x2 = R2*sin(P)*cos(T);
    y2 = R2*sin(P)*sin(T);
    z2 = R2*cos(P);
    dx = x2 - x;  //  /dR ??
    dy = y2 - y;
    dz = z2 - z;
    printf("dR=%f, dx=%f, dy=%f, dz=%f \n", dR, dx, dy, dz);
    dT = 0.015;
    T2 = T+dT;
    x2 = R*sin(P)*cos(T2);
    y2 = R*sin(P)*sin(T2);
    z2 = R*cos(P);
    dx = x2 - x;  //  /dT ??
    dy = y2 - y;
    dz = z2 - z;
    printf("dT=%f, dx=%f, dy=%f, dz=%f \n", dT, dx, dy, dz);
    dP = 0.02;
    P2 = P+dP;
    x2 = R*sin(P2)*cos(T);
    y2 = R*sin(P2)*sin(T);
    z2 = R*cos(P2);
    dx = x2 - x;  //  /dP ??
    dy = y2 - y;
    dz = z2 - z;
    printf("dP=%f, dx=%f, dy=%f, dz=%f \n", dP, dx, dy, dz);
    x2 = R2*sin(P2)*cos(T2);
    y2 = R2*sin(P2)*sin(T2);
    z2 = R2*cos(P2);
    dx = x2 - x;  //  /d?
    dy = y2 - y;
    dz = z2 - z;
    printf("dall,   dx=%f, dy=%f, dz=%f \n", dx, dy, dz);
    // theory 
    dx = dR*sin(P)*cos(T) + R*dP*cos(P)*cos(T) - R*sin(P)*dT*sin(T);
    dy = dR*sin(P)*sin(T) + R*dP*cos(P)*sin(T) + R*sin(P)*dT*cos(T);
    dz = dR*cos(P)        - R*dP*sin(P);
    printf("theory, dx=%f, dy=%f, dz=%f \n", dx, dy, dz);
    dvol = R*R*sin(T)*dR*dT*dP;
    printf("dvolume for dR,dT,dP = %e\n", dvol);
    printf(" \n");

    printf("check in all octants for numeric derivative accuracy\n");
    printf("r gird values, not equally spaced\n");
    Rg[0] = 0.3;
    Rg[1] = 0.5;
    Rg[2] = 0.7;
    Rg[3] = 0.9;
    Rg[4] = 1.0;
    for(I=0; I<Nr; I++)
      printf("Rg[%1d]=%f\n", I, Rg[I]);
    printf("theta gird values, not equally spaced\n");
    Tg[0] = 0.1;
    Tg[1] = 40.0*Pi/180.0;
    Tg[2] = 85.0*Pi/180.0;
    Tg[3] = 130.0*Pi/180.0;
    Tg[4] = 185.0*Pi/180.0;
    Tg[5] = 240.0*Pi/180.0;
    Tg[6] = 300.0*Pi/180.0;
    for(J=0; J<Nt; J++)
      printf("Tg[%1d]=%f\n", J, Tg[J]);
    printf("phi gird values, not equally spaced\n");
    Pg[0] = 0.2;
    Pg[1] = 0.9;
    Pg[2] = 1.7;
    Pg[3] = 2.4;
    Pg[4] = Pi-0.5;
    for(K=0; K<Np; K++)
      printf("Pg[%1d]=%f\n", K, Pg[K]);

    printf("analytic partial second derivative\n");
    for(I=0; I<Nr; I++)
    {
      R = Rg[I];
      for(J=0; J<Nt; J++)
      {
        T = Tg[J];
	for(K=0; K<Np; K++)
	{
          P = Pg[K];
          Ua[S(I,J,K)] = U(R,T,P);
          printf("Ua[%1d,%1d,%1d]=%f, Uuu=%f, D2=%f\n", I, J, K,
                 Ua[S(I,J,K)], Uuu(Rg[I],Tg[J],Pg[K]),
                 D2(Ur(R,T,P),Urr(R,T,P),Utt(R,T,P),Up(R,T,P),
		    Upp(R,T,P),R,P));
	  x = Rg[I]*cos(Tg[J])*sin(Pg[K]);
	  y = Rg[I]*sin(Tg[J])*sin(Pg[K]);
          z = Rg[I]*cos(Pg[K]);
          if(I==0 && J==0 && K==0) 
            printf("x=%e, y=%e, z=%e\n", x, y, z);
          rtr = sqrt(x*x+y*y+z*z);
          ttr = atan2(y, x);
          if(ttr<0.0) ttr = 2.0*Pi + ttr; // atan2 -Pi to Pi fix
          ptr = atan2(sqrt(x*x+y*y),z);
          Err = abs(rtr-Rg[I]);
          if(Err>0.001) 
            printf("R=%f, rtr=%f, Err=%e\n",Rg[I], rtr, Err);
          Err = abs(ttr-Tg[J]);
          if(Err>0.001) 
	    {printf("T=%f, ttr=%f, Err=%e\n",Tg[J], ttr, Err);
	     printf("x=%e, y=%e, z=%e\n", x, y, z);}
          Err = abs(ptr-Pg[K]);
          if(Err>0.001) 
            printf("P=%f, ptr=%f, Err=%e\n",Pg[K], ptr, Err);
          Err = abs(U(Rg[I],Tg[J],Pg[K])-Uxyz(x,y,z));
          if(Err>0.001) 
	    {printf("U=%f, Uxyz=%f, Err=%e\n", U(Rg[I],Tg[J],Pg[K]),
                     Uxyz(x,y,z), Err);
	     printf("x=%e, y=%e, z=%e\n", x, y, z);}
	} // K
      } // J
    } // I
    for(I=0; I<Nr; I++)
    {
      R = Rg[I];
      for(J=0; J<Nt; J++)
      {
        T = Tg[J];
        for(K=0; K<Np; K++)
	{
          P = Pg[K];
          // compute Ur, Urr, Utt, Upp and call D2
          nuderiv(1, Nr, I, Rg, Cr);
          nuderiv(2, Nr, I, Rg, Crr);
          Urv = 0.0;
          Urrv = 0.0;
          for(Ii=0; Ii<Nr; Ii++)
	  {
            Urv = Urv + Cr[Ii]*U(Rg[Ii],T,P);
            Urrv = Urrv + Crr[Ii]*U(Rg[Ii],T,P);
          }
          Utv = 0.0;
          Uttv = 0.0;
          nuderiv(1, Nt, J, Tg, Ct);
          nuderiv(2, Nt, J, Tg, Ctt);
          for(Jj=0; Jj<Nt; Jj++)
	  {
            Utv = Utv + Ct[Jj]*U(R,Tg[Jj],P);
            Uttv = Uttv + Ctt[Jj]*U(R,Tg[Jj],P);
          }
          Upv = 0.0;
          Uppv = 0.0;
          nuderiv(1, Np, K, Pg, Cp);
          nuderiv(2, Np, K, Pg, Cpp);
          for(Kk=0; Kk<Np; Kk++)
	  {
            Upv = Upv + Cp[Kk]*U(R,T,Pg[Kk]);
            Uppv = Uppv + Cpp[Kk]*U(R,T,Pg[Kk]);
          }

          Cmp = D2(Urv, Urrv, Uttv, Upv, Uppv, R, P);
          Ana = Uuu(R,T,P);
          Err = Ana - Cmp;
          Maxerr = max(Maxerr, abs(Err));
          printf("i=%1d, j=%1d, k=%1d, Ana=%f, Cmp=%f, Err=%e\n",
                 I, J, K, Ana, Cmp, Err);
          Err = Ur(R,T,P)-Urv;
          Maxerrur = max(Maxerrur, abs(Err));
          printf("i=%1d, j=%1d, k=%1d, Ur=%f, Urv=%f, Err=%e\n",
                 I, J, K, Ur(R,T,P), Urv, Err);
          Err = Urr(R,T,P)-Urrv;
          Maxerrurr = max(Maxerrurr, abs(Err));
          printf("i=%1d, j=%1d, k=%1d, Urr=%f, Urrv=%f, Err=%e\n",
                 I, J, K, Urr(R,T,P), Urrv, Err);
          Err = Ut(R,T,P)-Utv;
          Maxerrut = max(Maxerrut, abs(Err));
          printf("i=%1d, j=%1d, k=%1d, Ut=%f, Utv=%f, Err=%e\n",
                 I, J, K, Ut(R,T,P), Utv, Err);
          Err = Utt(R,T,P)-Uttv;
          Maxerrutt = max(Maxerrutt, abs(Err));
          printf("i=%1d, j=%1d, k=%1d, Utt=%f, Uttv=%f, Err=%e\n",
                 I, J, K, Utt(R,T,P), Uttv, Err);
          Err = Up(R,T,P)-Upv;
          Maxerrupp = max(Maxerrupp, abs(Err));
          printf("i=%1d, j=%1d, k=%1d, Up=%f, Upv=%f, Err=%e\n",
                 I, J, K, Up(R,T,P), Upv, Err);
          printf(" ");
          Err = Upp(R,T,P)-Uppv;
          Maxerrupp = max(Maxerrupp, abs(Err));
          printf("i=%1d, j=%1d, k=%1d, Upp=%f, Uppv=%f, Err=%e\n",
                 I, J, K, Upp(R,T,P), Uppv, Err);
          printf("\n ");
        }
      }
    }
    printf("Maxerr Ur=%e\n", Maxerrur);
    printf("Maxerr Urr=%e\n", Maxerrurr);
    printf("Maxerr Ut=%e\n", Maxerrut);
    printf("Maxerr Utt=%e\n", Maxerrutt);
    printf("Maxerr Up=%e\n", Maxerrup);
    printf("Maxerr Upp=%e\n", Maxerrupp );
    printf("Maxerr deriv=%e\n", Maxerr );

    printf("check_sphere_deriv.c finishing\n");
    return 0;
} // end check_sphere_deriv.c