// WGS2GK.CPP : C++ version of Ottmar Labonde's WGSDHDN3.PAS with CPU time measurement
// compile with MS Visual C++ version 6.0 or do the necessary modifications for your compiler
//
// Conversion of WGS84 lat and lon to DHDN- (Deutsches Haupt-Dreiecksnetz),
// aka "Potsdam-Datum" and R & H (Gauss-Krueger Rechtswert and Hochwert).

#include "stdafx.h"
#include <math.h>

#define Pi 3.1415926535897932384626433832795028841971693993751058209749445923078164

const double awgs = 6378137.0;        // WGS84 Semi-Major Axis = Equatorial Radius in meters
const double bwgs = 6356752.314;      // WGS84 Semi-Minor Axis = Polar Radius in meters
const double abes = 6377397.155;      // Bessel Semi-Major Axis = Equatorial Radius in meters
const double bbes = 6356078.962;      // Bessel Semi-Minor Axis = Polar Radius in meters
const double cbes = 111120.6196;      // Bessel latitude to Gauss-Krueger meters
const double dx   = -585.7;           // Translation Parameter 1
const double dy   = -87.0;            // Translation Parameter 2 
const double dz   = -409.2;           // Translation Parameter 3
const double rotx = 2.540423689E-6;   // Rotation Parameter 1
const double roty = 7.514612057E-7;   // Rotation Parameter 2
const double rotz = -1.368144208E-5;  // Rotation Parameter 3
// const double sc   = 1/0.99999122;     // Scaling Factor wrong!
// Maik Stoeckmann reported this error on Nov 12th 2002. Thank you, Maik!
const double sc   = 0.99999122;       // Scaling Factor

double l1;
double b1;
double l2;
double b2;
double h1;
double h2;
double R;
double H;
double a;
double b;
double eq;
double N;
double Xq;
double Yq;
double Zq;
double X;
double Y;
double Z;

// For performance measurement
static LARGE_INTEGER pm_freq;
static LARGE_INTEGER pm_start;
LARGE_INTEGER pm_end;
double pm_executiontime;

// Prototypes
void HelmertTransformation(double,double,double,double&,double&,double&);
void BesselBLnachGaussKrueger(double,double,double&,double&);
void BLRauenberg (double,double,double,double&,double&,double&);
double neuF(double,double,double,double);
double round(double);

int main(int argc, char* argv[])
{
  printf("WGS84 to Gauss-Krueger Transformation Test\n");
  printf("Written by Walter Piechulla, Regensburg University\n");
  printf("Thanks to Ottmar Labonde, http://user.baden-online.de/~olabonde/\n");
  printf("This should work for Germany with an error of +- 1 m\n");

  printf("\nPlease type in WGS-Latitude (decimal)> ");
  scanf("%lf",&b1);

  printf("\nPlease type in WGS-Longitude (decimal)> ");
  scanf("%lf",&l1);

  printf("\nPlease type in WGS-Height over ground (meters)> ");
  scanf("%lf",&h1);

  QueryPerformanceFrequency(&pm_freq);      // get frequency
  QueryPerformanceCounter(&pm_start);       // store actual counter
    
  l1=Pi*l1/180;
  b1=Pi*b1/180;

  a=awgs; 
  b=bwgs;
  
  eq=(a*a-b*b)/(a*a);
  N=a/sqrt(1-eq*sin(b1)*sin(b1));
  Xq=(N+h1)*cos(b1)*cos(l1);
  Yq=(N+h1)*cos(b1)*sin(l1);
  Zq=((1-eq)*N+h1)*sin(b1);

  HelmertTransformation(Xq,Yq,Zq,X,Y,Z);
  
  a=abes;
  b=bbes;

  eq=(a*a-b*b)/(a*a);

  BLRauenberg(X,Y,Z,b2,l2,h2);
  
  BesselBLnachGaussKrueger(b2,l2,R,H);
  
  b2=b2*180/Pi;
  l2=l2*180/Pi;

  QueryPerformanceCounter(&pm_end); 
  // executiontime is difference of counters divided by the frequency
  pm_executiontime=(double)(pm_end.QuadPart - pm_start.QuadPart)/(double)pm_freq.QuadPart;

  printf("\nPotsdam-Breite: %lf\n",b2);
  printf("Potsdam-Laenge: %lf\n",l2);
  printf("Potsdam-Hoehe: %lf\n",h2);
  
  printf("\nGauss-Krueger Koordinaten:\n\n");
  printf("R = %lf\n\n",R);
  printf("H = %lf\n\n",H);

  printf("Execution time in ms = %5.3lf\n", pm_executiontime*1000);

  return(0);
}

void HelmertTransformation(double x,double y,double z,double& xo,double& yo,double& zo)
{
  xo=dx+(sc*(1*x+rotz*y-roty*z));
  yo=dy+(sc*(-rotz*x+1*y+rotx*z));
  zo=dz+(sc*(roty*x-rotx*y+1*z));
}                            

void BesselBLnachGaussKrueger(double b,double ll,double& Re,double& Ho)
{
  double l;
  double l0;
  double bg;
  double lng;
  double Ng;
  double k;
  double t;
  double eq;
  double Vq;
  double v;
  double nk;
  double X;
  double gg;
  double SS;
  double Y;
  double kk;
  double Pii;
  double RVV;

  bg=180*b/Pi;
  lng=180*ll/Pi;
  l0=3*round((180*ll/Pi)/3);
  l0=Pi*l0/180;
  l=ll-l0;
  k=cos(b);
  t=sin(b)/k;
  eq=(abes*abes-bbes*bbes)/(bbes*bbes);
  Vq=1+eq*k*k;
  v=sqrt(Vq);
  Ng=abes*abes/(bbes*v);
  nk=(abes-bbes)/(abes+bbes);
  X=((Ng*t*k*k*l*l)/2)+((Ng*t*(9*Vq-t*t-4)*k*k*k*k*l*l*l*l)/24);

  gg=b+(((-3*nk/2)+(9*nk*nk*nk/16))*sin(2*b)+15*nk*nk*sin(4*b)/16-35*nk*nk*nk*sin(6*b)/48);
  SS=gg*180*cbes/Pi;
  Ho=(SS+X);
  Y=Ng*k*l+Ng*(Vq-t*t)*k*k*k*l*l*l/6+Ng*(5-18*t*t+t*t*t*t)*k*k*k*k*k*l*l*l*l*l/120;
  kk=500000;
  Pii=Pi;
  RVV=round((180*ll/Pii)/3);
  Re=RVV*1000000+kk+Y;
}

void BLRauenberg (double x,double y,double z,double& b,double& l,double& h)
{
  double f;
  double f1;
  double f2;
  double ft;
  double p;

  f=Pi*50/180;
  p=Z/sqrt(x*x+y*y);
  
  do
  {
    f1=neuF(f,x,y,p);
    f2=f;
    f=f1;
    ft=180*f1/Pi;
  }
  while(!(fabs(f2-f1)<10E-10));
  
  b=f;
  l=atan(y/x);
  h=sqrt(x*x+y*y)/cos(f1)-a/sqrt(1-eq*sin(f1)*sin(f1));
}

double neuF(double f,double x,double y,double p)
{
  double zw;
  double nnq;

  zw=a/sqrt(1-eq*sin(f)*sin(f));
  nnq=1-eq*zw/(sqrt(x*x+y*y)/cos(f));
  return(atan(p/nnq));
}

double round(double src)
{
  double theInteger;
  double theFraction;
  double criterion = 0.5;

  theFraction = modf(src,&theInteger);

  if (!(theFraction < criterion))
  {
    theInteger += 1; 
  } 

  return theInteger;
}

// Outline for measurement of used CPU time with Windows 95/98/NT
//
// QueryPerformanceFrequency(&freq);      // get frequency
// QueryPerformanceCounter(&start);       // store actual counter
//...
// execute code to measure
//...
// QueryPerformanceCounter(&end); // store actual counter again
//
// executiontime is difference of counters divided by the frequency
// executiontime=(double)(end.QuadPart - start.QuadPart)/(double)freq.QuadPart;
//
// printf("execution in ms = %5.3lf\n", executiontime*1000);