// check_spherical_gradient.c   check numerical calculation of derivatives 
//                      in spherical coordinates, for test function u
//        u(r,t,p) = r*r + 2.0*t*t + 3.0*p*p + 4.0*r*t + 5.0*r*p + 6.0*t*p
// using: x = r*sin(p)*cos(t)       0 <= t <= 2Pi
//        y = r*sin(p)*sin(t)       0 <= p <= Pi
//        z = r*cos(p)
//        r = sqrt(x^2 + y^2 + z^2)
//        t = atan(y/x)
//        p = atan(sqrt(x^2+y^2)/z)
//
// gradient del, vector:
//       ^         ^             ^
// del = r du/dr + t 1/r du/dt + p 1/(r*sin(t)) du/dp
//
// data structure  rtp[][][] field values at radius, theta and phi   
//                 rval[]    values of r at index 0, 1, ..., nr-1    
//                 tval[]    values of theta 0, dt, 2dt, ... 2Pi-dt  
//                 pval[]    values of phi   0,,dp, 2dp, ... Pi-dt    
//                                                                   
// method, use nuderiv to compute numerical derivatives              
// u(r,t,p) = r*r + 2.0*t*t + 3.0*p*p + 4.0*r*t + 5.0*r*p + 6.0*t*p
// du/dr  computed analytic and numeric 
// du/dt  computed analytic and numeric 
// du/dp  computed analytic and numeric 
// spherical du/dr = du/dr computed analytic and numeric, sdudr 
// spherical du/dt = 1/r du/dt computed analytic and numeric, sdudt
// spherical du/dp = 1/(r sin(t) du/dp computed analytic and numeric, sdudp 
//
// del^2=Laplacian= d^2U/dr^2 + 2/r dU/dr + 1/r^2 d^2U/dt^2 + 
//                  cos(t)/(r^2 sin(t)) dU/dt + 1/(r^2 sin(t)^2) d^2U/dp^2
// Notation         Urr +       2/r Ur +    1/r^2 Utt + 
//                 cos(t)/(r^2 sin(t)) Ut +     1/(r^2 sin(t)^2) Upp

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

#define nr 5
#define nt 5
#define np 5
static double rtp[nr][nt][np]; // u(rval,tval,pval)
static double rval[nr] = { 1.20, 1.21, 1.22, 1.23, 1.25 };
static double tval[nt]; // computed below 
static double pval[np];
static double xval[nr][nt][np];
static double yval[nr][nt][np];
static double zval[nr][nt][np];
static double rcoef[nr]; // computed derivative
static double tcoef[nt];
static double pcoef[np];
static double r2coef[nr]; // computed second derivative
static double t2coef[nt];
static double p2coef[np];

double u(double r, double t, double p)
{
  return r*r + 2.0*t*t + 3.0*p*p + 4.0*r*t + 5.0*r*p + 6.0*t*p;
}

double dudr(double r, double t, double p) // first derivative Ur
{
  return 2.0*r + 4.0*t + 5.0*p;
}

double ddudrr(double r, double t, double p) // second derivative Urr
{
  return 2.0;
}

double sdudr(double r, double t, double p) // spherical del du(r,t,p)/dr
{
  return dudr(r, t, p); // spherical
}

double sddudrr(double r, double t, double p) // second derivative Urr
{
  return (2.0/r)*dudr(r, t, p) + ddudrr(r, t, p); // spherical
}

double dudt(double r, double t, double p)
{
  return 4.0*t + 4.0*r + 6.0*p;
}

double ddudtt(double r, double t, double p)
{
  return 4.0;
}

double sdudt(double r, double t, double p) // spherical del 1/r du(r,t,p)/dt
{
  return dudt(r, t, p)/r;
}

double sddudtt(double r, double t, double p)
{
  return (cos(t)/(r*r*sin(t)))*dudt(r, t, p) + (1.0/(r*r))*ddudtt(r, t, p);
}

double dudp(double r, double t, double p)
{
  return 6.0*p + 5.0*r + 6.0*t;
}

double ddudpp(double r, double t, double p)
{
  return 6.0;
}

double sdudp(double r, double t, double p) // spherical 1/(r*sin(t) du(r,t,p)/dp
{
  return dudp(r, t, p)/(r*sin(t));
}

double sddudpp(double r, double t, double p) // spherical del^2 p term
{
  return ddudpp(r, t, p)/(r*r*sin(t)*sin(t));
}

double xp(double r, double t, double p)
{
  return r*sin(p)*cos(t);
}

double yp(double r, double t, double p)
{
  return r*sin(p)*sin(t);
}

double zp(double r, double t, double p)
{
  return r*cos(p);
}

double rc(double x, double y, double z)
{
  return sqrt(x*x + y*y + z*z);
}

double tc(double x, double y, double z) // 0 <= t <= 2Pi
{
  double a = atan2(y, x);
  if(a<0.0) return 2.0*Pi + a;
  return a;
}

double pc(double x, double y, double z) // 0 <= p <= Pi
{
  double a = atan2(sqrt(x*x+y*y), z);
  return a;
}

int main(int argc, char * argv[])
{
  int i, j, k, m;
  double rd, td, pd, r, t, p, x, y, z;
  double drc, dtc, dpc;
  double ddrc, ddtc, ddpc;
  double del2a, del2c;
  double deg = 180.0/Pi;
  double err, maxerrr, maxerrt, maxerrp, maxerrl;

  printf("check_spherical_gradient.c checking numerical derivatives \n");
  printf("using: x = r*sin(p)*cos(t)   0 <= t <= 2Pi  \n");
  printf("       y = r*sin(p)*sin(t)   0 <= p <= Pi   \n");
  printf("       z = r*cos(p)          r > 0 \n");
  printf("       r = sqrt(x*x + y*y + z*z)  \n");
  printf("       t = atan2(y,x)             \n");
  printf("       p = atan2(sqrt(x*x+y*y),z) \n");
  printf(" \n");
  printf("using test function u(r,t,p)= \n");
  printf("    r*r + 2.0*t*t + 3.0*p*p + 4.0*r*t + 5.0*r*p + 6.0*t*p \n");
  printf("    del = vector in r,t,p spherical coordinates = \n");
  printf("       du/dr = 2.0*r + 4.0*t + 5.0*p    dudr \n");
  printf("       du/dt = 4.0*t + 4.0*r + 6.0*p    dudt \n");
  printf("       du/dp = 6.0*p + 5.0*r + 6.0*t    dudp \n");
  printf("    del in spherical coordinates is: \n");
  printf("       r direction = du/dr              sdudr \n");
  printf("       t direction = 1/r du/dt          sdudt \n");
  printf("       p direction = 1/(r sin(t)) du/dp sdudp \n");
  printf("    del^2 Laplacian in spherical coordinates \n");
  printf("       1/r^2 d(r^2 du/dr)/dr +               \n");  
  printf("       1/(r^2 sin(t)) d(sin(t) du/dt)/dt +   \n");
  printf("       1/(r^2 sin(t)^2) d^2u/dp^2            \n");
  printf(" these expressions reduce to easier to compute: \n");
  printf("     2/r du/dr + d^2u/dr^2 +                 \n");
  printf("     cos(t)/(r^2 sin(t)) du/dt + 1/r^2 d^2U/du^2) d^2u/dt^2 + \n");
  printf("     1/(r^2 sin(t)^2) d^2u/dp^2              \n");
  printf(" \n");


  for(i=0; i<nr; i++) printf("rval[%d]=%f \n", i, rval[i]);
  printf("\n");

  tval[0] = 0.7071;
  td = 0.01;
  for(j=1; j<nt; j++)
  {
    tval[j] = tval[j-1]+td;
    printf("tval[%d]=%f \n", j, tval[j]);
  }
  printf("\n");

  pval[0] = 0.72;
  pd = 0.02;
  for(k=1; k<np; k++)
  {
    pval[k] = pval[k-1]+pd;
    printf("pval[%d]=%f \n", k, pval[k]);
  }
  printf("\n");
  for(i=0; i<nr; i++)
  {
    for(j=0; j<nt; j++)
    {
      for(k=0; k<np; k++)
      {
        rtp[i][j][k]  =  u(rval[i], tval[j], pval[k]);
        xval[i][j][k] = xp(rval[i], tval[j], pval[k]);
        yval[i][j][k] = yp(rval[i], tval[j], pval[k]);
        zval[i][j][k] = zp(rval[i], tval[j], pval[k]);
      }
    }
  }

  printf("some basic checking of equations \n");
  x = 1.0;
  y = 1.0;
  z = 1.0;
  printf(" x=%f,  y=%f,  z=%f \n", x, y, z);
  r = rc(x, y, z);
  t = tc(x, y, z);
  p = pc(x, y, z);
  printf("rc=%f, tc=%f, pc=%f   td=%f, pd=%f \n", r, t, p, deg*t, deg*p);
  x = xp(r, t, p);
  y = yp(r, t, p);
  z = zp(r, t, p);
  printf(" x=%f,  y=%f,  z=%f \n", x, y, z);
  printf(" \n");

  x =-1.0;
  y = 1.0;
  z = 1.0;
  printf(" x=%f,  y=%f,  z=%f \n", x, y, z);
  r = rc(x, y, z);
  t = tc(x, y, z);
  p = pc(x, y, z);
  printf("rc=%f, tc=%f, pc=%f   td=%f, pd=%f \n", r, t, p, deg*t, deg*p);
  x = xp(r, t, p);
  y = yp(r, t, p);
  z = zp(r, t, p);
  printf(" x=%f,  y=%f,  z=%f \n", x, y, z);
  printf(" \n");

  x = 1.0;
  y =-1.0;
  z = 1.0;
  printf(" x=%f,  y=%f,  z=%f \n", x, y, z);
  r = rc(x, y, z);
  t = tc(x, y, z);
  p = pc(x, y, z);
  printf("rc=%f, tc=%f, pc=%f   td=%f, pd=%f \n", r, t, p, deg*t, deg*p);
  x = xp(r, t, p);
  y = yp(r, t, p);
  z = zp(r, t, p);
  printf(" x=%f,  y=%f,  z=%f \n", x, y, z);
  printf(" \n");

  x = 1.0;
  y = 1.0;
  z =-1.0;
  printf(" x=%f,  y=%f,  z=%f \n", x, y, z);
  r = rc(x, y, z);
  t = tc(x, y, z);
  p = pc(x, y, z);
  printf("rc=%f, tc=%f, pc=%f   td=%f, pd=%f \n", r, t, p, deg*t, deg*p);
  x = xp(r, t, p);
  y = yp(r, t, p);
  z = zp(r, t, p);
  printf(" x=%f,  y=%f,  z=%f \n", x, y, z);
  printf(" \n");

  x =-1.0;
  y =-1.0;
  z = 1.0;
  printf(" x=%f,  y=%f,  z=%f \n", x, y, z);
  r = rc(x, y, z);
  t = tc(x, y, z);
  p = pc(x, y, z);
  printf("rc=%f, tc=%f, pc=%f   td=%f, pd=%f \n", r, t, p, deg*t, deg*p);
  x = xp(r, t, p);
  y = yp(r, t, p);
  z = zp(r, t, p);
  printf(" x=%f,  y=%f,  z=%f \n", x, y, z);
  printf(" \n");

  x =-1.0;
  y = 1.0;
  z =-1.0;
  printf(" x=%f,  y=%f,  z=%f \n", x, y, z);
  r = rc(x, y, z);
  t = tc(x, y, z);
  p = pc(x, y, z);
  printf("rc=%f, tc=%f, pc=%f   td=%f, pd=%f \n", r, t, p, deg*t, deg*p);
  x = xp(r, t, p);
  y = yp(r, t, p);
  z = zp(r, t, p);
  printf(" x=%f,  y=%f,  z=%f \n", x, y, z);
  printf(" \n");

  x = 1.0;
  y =-1.0;
  z =-1.0;
  printf(" x=%f,  y=%f,  z=%f \n", x, y, z);
  r = rc(x, y, z);
  t = tc(x, y, z);
  p = pc(x, y, z);
  printf("rc=%f, tc=%f, pc=%f   td=%f, pd=%f \n", r, t, p, deg*t, deg*p);
  x = xp(r, t, p);
  y = yp(r, t, p);
  z = zp(r, t, p);
  printf(" x=%f,  y=%f,  z=%f \n", x, y, z);
  printf(" \n");

  x =-1.0;
  y =-1.0;
  z =-1.0;
  printf(" x=%f,  y=%f,  z=%f \n", x, y, z);
  r = rc(x, y, z);
  t = tc(x, y, z);
  p = pc(x, y, z);
  printf("rc=%f, tc=%f, pc=%f   td=%f, pd=%f \n", r, t, p, deg*t, deg*p);
  x = xp(r, t, p);
  y = yp(r, t, p);
  z = zp(r, t, p);
  printf(" x=%f,  y=%f,  z=%f \n", x, y, z);
  printf(" \n");

  r = rval[0];
  t = tval[0];
  p = pval[0];
  x = xp(r, t, p);
  y = yp(r, t, p);
  z = zp(r, t, p);
  printf(" x=%f,  y=%f,  z=%f \n", x, y, z);
  printf(" r=%f,  t=%f,  p=%f \n", r, t, p);
  r = rc(x, y, z);
  t = tc(x, y, z);
  p = pc(x, y, z);
  printf("rc=%f, tc=%f, pc=%f \n", r, t, p);
  printf(" \n");

  r = rval[nr-1];
  t = tval[nt-1];
  p = pval[np-1];
  x = xp(r, t, p);
  y = yp(r, t, p);
  z = zp(r, t, p);
  printf(" x=%f,  y=%f,  z=%f \n", x, y, z);
  printf(" r=%f,  t=%f,  p=%f \n", r, t, p);
  r = rc(x, y, z);
  t = tc(x, y, z);
  p = pc(x, y, z);
  printf("rc=%f, tc=%f, pc=%f \n", r, t, p);
  printf(" \n");

  printf(" \n");
  maxerrr = 0.0;
  maxerrt = 0.0;
  maxerrp = 0.0;
  maxerrl = 0.0;
  for(i=0; i<nr; i++)
  {
    nuderiv(1, nr, i, rval, rcoef);
    printf("nuderiv rval %f \n", rcoef[0]);
    nuderiv(2, nr, i, rval, r2coef);
    printf("nuderiv r2val %f \n", r2coef[0]);
    r = rval[i];
    for(j=0; j<nt; j++)
    {
      nuderiv(1, nt, j, tval, tcoef);
      printf("nuderiv tval %f \n", tcoef[0]);
      nuderiv(2, nt, j, tval, t2coef);
      printf("nuderiv t2val %f \n", t2coef[0]);
      t = tval[j];
      for(k=0; k<np; k++)
      {
        nuderiv(1, np, k, pval, pcoef);
        printf("nuderiv pval %f \n", pcoef[0]);
        nuderiv(2, np, k, pval, p2coef);
        printf("nuderiv p2val %f \n", p2coef[0]);
        p = pval[k];
        printf("rtp[%d][%d][%d]=%f \n", i, j, k, rtp[i][j][k]);
        printf("at r=%f, t=%f, p=%f \n", r, t, p);

        drc = 0.0;
        for(m=0; m<nr; m++) drc = drc + rcoef[m]*rtp[m][j][k];
        dtc = 0.0;
        for(m=0; m<nt; m++) dtc = dtc + tcoef[m]*rtp[i][m][k];
        dpc = 0.0;
        for(m=0; m<np; m++) dpc = dpc + pcoef[m]*rtp[i][j][m];
        printf("  analytic    dudr=%f,  dudt=%f,  dudp=%f \n",
               dudr(r, t, p), dudt(r, t, p), dudp(r, t, p));
        printf("  computed    dudr=%f,  dudt=%f,  dudp=%f \n",
               drc, dtc, dpc);

        drc = drc;
        dtc = dtc/r;
        dpc = dpc/(r*sin(t));
        err = abs(drc-sdudr(r, t, p));
        maxerrr = max(maxerrr, err);
        err = abs(dtc-sdudt(r, t, p));
        maxerrt = max(maxerrt, err);
        err = abs(dpc-sdudp(r, t, p));
        maxerrp = max(maxerrp, err);

        printf("  analytic   sdudr=%f, sdudt=%f,  sdudp=%f \n",
               sdudr(r, t, p), sdudt(r, t, p), sdudp(r, t, p));
        printf("  computed   sdudr=%f, sdudt=%f,  sdudp=%f \n",
               drc, dtc, dpc, err);

        drc = 0.0;
        for(m=0; m<nr; m++) drc = drc + rcoef[m]*rtp[m][j][k];
        dtc = 0.0;
        for(m=0; m<nt; m++) dtc = dtc + tcoef[m]*rtp[i][m][k];
        ddrc = 0.0;
        for(m=0; m<nr; m++) ddrc = ddrc + r2coef[m]*rtp[m][j][k];
        ddtc = 0.0;
        for(m=0; m<nt; m++) ddtc = ddtc + t2coef[m]*rtp[i][m][k];
        ddpc = 0.0;
        for(m=0; m<np; m++) ddpc = ddpc + p2coef[m]*rtp[i][j][m];
        printf("  analytic   ddudrr=%f, ddudtt=%f, ddudpp=%f \n",
               ddudrr(r, t, p), ddudtt(r, t, p), ddudpp(r, t, p));
        printf("  computed   ddudrr=%f, ddudtt=%f, ddudpp=%f \n",
               ddrc, ddtc, ddpc);

        del2a = sddudrr(r, t, p)+ sddudtt(r, t, p)+ sddudpp(r, t, p);
	del2c = (2.0/r)*drc + ddrc +
	        (cos(t)/(r*r*sin(t)))*dtc + (1.0/(r*r))*ddtc +
	        (1.0/(r*r*sin(t)*sin(t)))*ddpc;
        err = abs(del2a - del2c);
        maxerrl = max(maxerrl, err);
        printf("  analytic  del^2=%f \n", del2a);
        printf("  computed  del^2=%f   err=%e\n\n", del2c, err);
      }
    }
  }
  printf("maxerr r=%e. t=%e, p=%e \n", maxerrr, maxerrt, maxerrp);
  printf("maxerr Laplace=del^2=%e \n", maxerrl);
  printf("\nend check_spherical_gradient.c \n");
  return 0;
} // end main of check_spherical_gradient.c