/***************************************************************************/
/* XW84T :                                                                 */
/*   Calculation of Datum Shift from WGS-84 to TOKYO                       */
/*   after T.Kanazawa (1988): "Calculation formula for the relationship    */
/*          between WGS-84 and Tokyo Datum indicated on Japanese charts",  */
/*          Data Rept.Hydrogr.Obs., Ser. Satellite Geodesy, no.1, 76-81.   */
/***************************************************************************/
#include <math.h>
#define PAI (3.14159265359)
#define RAD (10800./PAI)    /* Lat. & Lon. in minutes */
#define HU  (1000.)         /* Alt. in meters */
#define AW  (6378.137)      /* GRS80 ellipsoid parameters */
#define FW  (1./298.257222)
#define EEW (FW*(2.-FW))
#define AT  (6377.397155)   /* Bessel ellipsoid parameters for Tokyo Datum */
#define FT  (1./299.152813)
#define EET (FT*(2.-FT))
#define US  ( 0.1463)       /* Datum Shift parameters after Kanazawa(1988) */
#define VS  (-0.5071)
#define WS  (-0.6810)

void xw84t(double wlat, double wlon, double walt,
             double *tlat, double *tlon, double *talt)
{   double rlt, rln, h, cwlat, swlat, cwlon, swlon;
    double bw, xw, uw, vw, ww, ut, vt, wt, r, re;
    double hb, s, srlt;
    rlt=wlat/RAD; rln=wlon/RAD; h=walt/HU;
    cwlat=cos(rlt); swlat=sin(rlt); cwlon=cos(rln); swlon=sin(rln);
    bw=AW/sqrt(1.-EEW*swlat*swlat); xw=(bw+h)*cwlat;
    uw=xw*cwlon; vw=xw*swlon; ww=(bw*(1.-EEW)+h)*swlat;
    ut=uw+US; vt=vw+VS; wt=ww+WS; r=sqrt(ut*ut+vt*vt); re=r*(1.-EET);
    *tlon=atan2(vt,ut)*RAD;
    do {
        hb=h; s=wt-EET*h*sin(rlt); rlt=atan(s/re);
        srlt=sin(rlt); h=r/cos(rlt)-AT/sqrt(1.-EET*srlt*srlt);
    } while (fabs(h-hb) > 0.000001);
    *tlat=rlt*RAD; *talt=h*HU;
}

void xtw84(double tlat, double tlon, double talt,
            double *wlat, double *wlon, double *walt)
{   double rlt, rln, h, ctlat, stlat, ctlon, stlon;
    double bt, xt, ut, vt, wt, uw, vw, ww, r, re;
    double hb, s, srlt;
    rlt=tlat/RAD; rln=tlon/RAD; h=talt/HU;
    ctlat=cos(rlt); stlat=sin(rlt); ctlon=cos(rln); stlon=sin(rln);
    bt=AT/sqrt(1.-EET*stlat*stlat); xt=(bt+h)*ctlat;
    ut=xt*ctlon; vt=xt*stlon; wt=(bt*(1.-EET)+h)*stlat;
    uw=ut-US; vw=vt-VS; ww=wt-WS; r=sqrt(uw*uw+vw*vw); re=r*(1.-EEW);
    *wlon=atan2(vw,uw)*RAD;
    do {
        hb=h; s=ww-EEW*h*sin(rlt); rlt=atan(s/re);
        srlt=sin(rlt); h=r/cos(rlt)-AW/sqrt(1.-EEW*srlt*srlt);
    } while (fabs(h-hb) > 0.000001);
    *wlat=rlt*RAD; *walt=h*HU;
}

void xw84t_(float *wlat, float *wlon, float *walt,
             float *tlat, float *tlon, float *talt)
{   double lat, lon, alt;
    xw84t((double)*wlat, (double)*wlon, (double)*walt, &lat, &lon, &alt);
    *tlat = (float)lat; *tlon = (float)lon; *talt = (float)alt;
}

void xtw84_(float *tlat, float *tlon, float *talt,
             float *wlat, float *wlon, float *walt)
{   double lat, lon, alt;
    xtw84((double)*tlat, (double)*tlon, (double)*talt, &lat, &lon, &alt);
    *wlat = (float)lat; *wlon = (float)lon; *walt = (float)alt;
}

void xw84td_(double *wlat, double *wlon, double *walt,
             double *tlat, double *tlon, double *talt)
{   xw84t(*wlat, *wlon, *walt, tlat, tlon, talt); }

void xtw84d_(double *tlat, double *tlon, double *talt,
             double *wlat, double *wlon, double *walt)
{   xtw84(*tlat, *tlon, *talt, wlat, wlon, walt); }