/* toro2r.c  another toroidal coordinate system       */
/*  r1, r2, theta, phi  mapped to x,y,z               */
/*  x = (r1+r2*sin(phi))*cos(theta)                   */
/*  y = (r1+r2*sin(phi))*sin(theta)                   */
/*  z = r2 * cos(phi)                                 */
/*  0 < theta < 2Pi    0 < phi < 2Pi                  */
/*                                                    */
/*  r2, x, y, z mapped to r1, theta, phi              */
/*  theta = arctan(y/x)                               */
/*  phi = arccos(z/r2)                                */
/*  r1 = x/cos(theta) - r2*sin(phi)   or              */
/*  r1 = y/sin(theta) - r2*sin(phi) no divide by zero */

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#define PI 3.141592653589793238462643
#undef abs
#define abs(a) ((a)<0.0?(-(a)):(a))

static double Xc(double r1, double r2, double theta, double phi)
{
  return (r1+r2*sin(phi)) * cos(theta);
}

static double Yc(double r1, double r2, double theta, double phi)
{
  return (r1+r2*sin(phi)) * sin(theta);
}

static double Zc(double r1, double r2, double theta, double phi)
{
  return r2*cos(phi);    
}

static double thetac(double r2, double x, double y, double z)
{
  double ang;
  ang = atan2(y,x);
  if(ang<0.0) ang = 2.0*PI + ang;
  return ang;
}

static double phic(double r2, double x, double y, double z)
{
  double ang = acos(z/r2);
  if(z<0.0) return 2.0*PI-ang; /* 0 to 2PI */ 
  else      return ang; 
}

static double r1c(double r2, double x, double y, double z)
{
  double r1;
  double theta = thetac(r2, x, y, z);
  double phi = phic(r2, x, y, z);
  if(abs(cos(theta))>1.0e-4) r1 = x/cos(theta) - r2*sin(phi);
  else                       r1 = y/sin(theta) - r2*sin(phi);
  return r1;
}

static void pr(double r1, double r2, double theta, double phi)
{
  double x, y, z;
  x = Xc(r1, r2, theta, phi);
  y = Yc(r1, r2, theta, phi);
  z = Zc(r1, r2, theta, phi);
  printf("theta=%4.2f, phi=%5.2f, ", theta, phi);
  printf("Xc=%7.3f, Yc=%7.3f, Zc=%7.3f \n", x, y, z);
}

static void check(double r1, double r2, double theta, double phi)
{
  double x, y, z;
  double err = 0.0;
  double err1;
  double errt;
  double errp;

  x = Xc(r1, r2, theta, phi);
  y = Yc(r1, r2, theta, phi);
  z = Zc(r1, r2, theta, phi);
  err1 = r1 - r1c(r2, x, y, z);
  errt = theta - thetac(r2, x, y, z);
  errp = phi   -   phic(r2, x, y, z);

  err = abs(err1) + abs(errt) + abs(errp);  
  if(err > 1.0e-5)
  {
    printf("inverse transform error = %g \n",err);
    printf("err1=%g, errt=%g, errp=%g \n", err1, errt, errp);
  }
}

int main(int argc, char * argv[])
{
  FILE * outp;
  double r1, r2, theta, phi;
  double x, y, z;
  int i, j;

  printf("toroidat_coord.c running \n");
  r1 = 2.0;
  r2 = 0.5;
  
  printf("r1=%4.2f, r2=%4.2f \n", r1, r2);

  printf("\nchanging phi\n");
  theta = 0.0;
  phi = 0.0;
  pr(r1, r2, theta, phi);
  check(r1, r2, theta, phi);
  phi = 0.1;
  pr(r1, r2, theta, phi);
  check(r1, r2, theta, phi);
  phi = 0.2;
  pr(r1, r2, theta, phi);
  check(r1, r2, theta, phi);
  phi = PI/2.0-0.1;
  pr(r1, r2, theta, phi);
  check(r1, r2, theta, phi);
  phi = PI/2.0;
  pr(r1, r2, theta, phi);
  check(r1, r2, theta, phi);
  phi = PI/2.0+0.1;
  pr(r1, r2, theta, phi);
  check(r1, r2, theta, phi);
  phi = PI-0.1;
  pr(r1, r2, theta, phi);
  check(r1, r2, theta, phi);
  phi = PI;
  pr(r1, r2, theta, phi);
  check(r1, r2, theta, phi);
  phi = PI+0.1;
  pr(r1, r2, theta, phi);
  check(r1, r2, theta, phi);
  phi = 3.0*PI/2.0-0.1;
  pr(r1, r2, theta, phi);
  check(r1, r2, theta, phi);
  phi = 3.0*PI/2.0;
  pr(r1, r2, theta, phi);
  check(r1, r2, theta, phi);
  phi = 3.0*PI/2.0+0.1;
  pr(r1, r2, theta, phi);
  check(r1, r2, theta, phi);
  phi = 2.0*PI-0.1;
  pr(r1, r2, theta, phi);
  check(r1, r2, theta, phi);

  printf("\nchanging theta\n");
  phi = 0.0;
  theta = 0.0;
  pr(r1, r2, theta, phi);
  check(r1, r2, theta, phi);
  theta = 0.1;
  pr(r1, r2, theta, phi);
  check(r1, r2, theta, phi);
  theta = 0.2;
  pr(r1, r2, theta, phi);
  check(r1, r2, theta, phi);
  theta = PI/2.0-0.1;
  pr(r1, r2, theta, phi);
  check(r1, r2, theta, phi);
  theta = PI/2.0;
  pr(r1, r2, theta, phi);
  check(r1, r2, theta, phi);
  theta = PI/2.0+0.1;
  pr(r1, r2, theta, phi);
  check(r1, r2, theta, phi);
  theta = PI-0.1;
  pr(r1, r2, theta, phi);
  check(r1, r2, theta, phi);
  theta = PI;
  pr(r1, r2, theta, phi);
  check(r1, r2, theta, phi);
  theta = PI+0.1;
  pr(r1, r2, theta, phi);
  check(r1, r2, theta, phi);
  theta = 3.0*PI/2.0-0.1;
  pr(r1, r2, theta, phi);
  check(r1, r2, theta, phi);
  theta = 3.0*PI/2.0;
  pr(r1, r2, theta, phi);
  check(r1, r2, theta, phi);
  theta = 3.0*PI/2.0+0.1;
  pr(r1, r2, theta, phi);
  check(r1, r2, theta, phi);
  theta = 2.0*PI-0.1;
  pr(r1, r2, theta, phi);
  check(r1, r2, theta, phi);


  outp = fopen("toro2r.dat","w");
  r1 = 2.0;
  r2 = 0.5;
  theta = 0.0;  
  for(i=0; i<33; i++)
  {
    phi = 0.0;
    for(j=0; j<17; j++)
    {
      x = Xc(r1, r2, theta, phi);
      y = Yc(r1, r2, theta, phi);
      z = Zc(r1, r2, theta, phi);
      fprintf(outp, "%9.5f  %9.5f,  %9.5f \n", x, y, z);
      phi = phi + PI/8.0;
    }
    fprintf(outp,"\n");
    theta = theta + PI/16.0;
  }
  fclose(outp);
  printf("toro2f.c finished \n");
  return 0;
}