/************************************************************************
*
* File:                 Utm.cpp
* RCS:                  $Header: /cvsroot/stelvio/stelvio/NavStar/Utm.cpp,v 1.1 2001/03/18 20:07:03 steve_l Exp $
* Author:               Steve Loughran
* Created:              2001
* Language:             C++
* Package:              
* Status:               Experimental
* @doc
*
************************************************************************/

/*
        This is code to do UTM conversion.

  I took this code from Jason Bevins' GPS thing which blagged the VB algorithms
    from the Mapping Datum Transformation Software (MADTRAN) program,
    written in PowerBasic.  To get the source code for MADTRAN, go to:

      http://164.214.2.59/publications/guides/MADTRAN/index.html
    
        this version retains the core algorithms as static functions

*/

//#include "stdafx.h"
//#include "Common.h"
//#include "osgb.h"
#include "utm.h"
#include <math.h>

// Some constants used by these functions.
static const double fe = 500000.0;
static const double ok = 0.9996;

// An array containing each vertical UTM zone.
static char cArray[] = "CDEFGHJKLMNPQRSTUVWX";


/////////////////////////////////////////////////////////////////////////////
// Miscellaneous functions for these UTM conversion formulas.

double CalculateESquared (double a, double b)
{
        return ((a * a) - (b * b)) / (a * a);
}


double CalculateE2Squared (double a, double b)
{
        return ((a * a) - (b * b)) / (b * b);
}


double denom (double es, double sphi)
{
        double sinSphi = sin (sphi);
        return sqrt (1.0 - es * (sinSphi * sinSphi));
}


double sphsr (double a, double es, double sphi)
{
        double dn = denom (es, sphi);
        return a * (1.0 - es) / (dn * dn * dn);
}


double sphsn (double a, double es, double sphi)
{
        double sinSphi = sin (sphi);
        return a / sqrt (1.0 - es * (sinSphi * sinSphi));
}


double sphtmd (double ap, double bp, double cp, double dp, double ep,
        double sphi)
{
        return (ap * sphi) - (bp * sin (2.0 * sphi)) + (cp * sin (4.0 * sphi))
                - (dp * sin (6.0 * sphi)) + (ep * sin (8.0 * sphi));
}


//=======================================================================
// Purpose:
//  This function converts the specified lat/lon coordinate to a UTM
//  coordinate.
// Parameters:
//  double a:
//      Ellipsoid semi-major axis, in meters. (For WGS84 datum, use 6378137.0)
//  double f:
//      Ellipsoid flattening. (For WGS84 datum, use 1 / 298.257223563)
//  int& utmXZone:
//      Upon exit, this parameter will contain the hotizontal zone number of
//      the UTM coordinate.  The returned value for this parameter is a number
//      within the range 1 to 60, inclusive.
//  char& utmYZone:
//      Upon exit, this parameter will contain the zone letter of the UTM
//      coordinate.  The returned value for this parameter will be one of:
//      CDEFGHJKLMNPQRSTUVWX.
//  double& easting:
//      Upon exit, this parameter will contain the UTM easting, in meters.
//  double& northing:
//      Upon exit, this parameter will contain the UTM northing, in meters.
//  double lat, double lon:
//      The lat/lon coordinate to convert.
// Notes:
//  - The code in this function is a C conversion of some of the source code
//    from the Mapping Datum Transformation Software (MADTRAN) program,
//    written in PowerBasic.  To get the source code for MADTRAN, go to:
//
//      http://164.214.2.59/publications/guides/MADTRAN/index.html
//    
//    and download MADTRAN.ZIP
//  - If the UTM zone is out of range, the y-zone character is set to the
//    asterisk character ('*').
//=======================================================================

void LatLonToUtm (double a, double f, int* utmXZone, char* utmYZone,
        double* easting, double* northing, double lat, double lon) 
{
        double recf;
        double b;
        double eSquared;
        double e2Squared;
        double tn;
        double ap;
        double bp;
        double cp;
        double dp;
        double ep;
        double olam;
        double dlam;
        double s;
        double c;
        double t;
        double eta;
        double sn;
        double tmd;
        double t1, t2, t3, t6, t7;
        double nfn;

        if (lon <= 0.0) {
                *utmXZone = 30 + (int)(lon / 6.0);
        } else {
                *utmXZone = 31 + (int)(lon / 6.0);
        }
        if (lat < 84.0 && lat >= 72.0) {
                // Special case: zone X is 12 degrees from north to south, not 8.
                *utmYZone = cArray[19];
        } else {
                *utmYZone = cArray[(int)((lat + 80.0) / 8.0)];
        }
        if (lat >= 84.0 || lat < -80.0) {
                // Invalid coordinate; the vertical zone is set to the invalid
                // character.
                *utmYZone = '*';
        }

        double latRad = lat * (M_PI/180);
        double lonRad = lon * (M_PI/180);
//      double latRad = lat * deg2rad;
//      double lonRad = lon * deg2rad;
        recf = 1.0 / f;
        b = a * (recf - 1.0) / recf;
        eSquared = CalculateESquared (a, b);
        e2Squared = CalculateE2Squared (a, b);
        tn = (a - b) / (a + b);
        ap = a * (1.0 - tn + 5.0 * ((tn * tn) - (tn * tn * tn)) / 4.0 + 81.0 *
                ((tn * tn * tn * tn) - (tn * tn * tn * tn * tn)) / 64.0);
        bp = 3.0 * a * (tn - (tn * tn) + 7.0 * ((tn * tn * tn)
                - (tn * tn * tn * tn)) / 8.0 + 55.0 * (tn * tn * tn * tn * tn) / 64.0)
                / 2.0;
        cp = 15.0 * a * ((tn * tn) - (tn * tn * tn) + 3.0 * ((tn * tn * tn * tn)
                - (tn * tn * tn * tn * tn)) / 4.0) / 16.0;
        dp = 35.0 * a * ((tn * tn * tn) - (tn * tn * tn * tn) + 11.0
                * (tn * tn * tn * tn * tn) / 16.0) / 48.0;
        ep = 315.0 * a * ((tn * tn * tn * tn) - (tn * tn * tn * tn * tn)) / 512.0;
        olam = (*utmXZone * 6 - 183) * (M_PI/180);
        dlam = lonRad - olam;
        s = sin (latRad);
        c = cos (latRad);
        t = s / c;
        eta = e2Squared * (c * c);
        sn = sphsn (a, eSquared, latRad);
        tmd = sphtmd (ap, bp, cp, dp, ep, latRad);
        t1 = tmd * ok;
        t2 = sn * s * c * ok / 2.0;
        t3 = sn * s * (c * c * c) * ok * (5.0 - (t * t) + 9.0 * eta + 4.0
                * (eta * eta)) / 24.0;
        if (latRad < 0.0) nfn = 10000000.0; else nfn = 0;
        *northing = nfn + t1 + (dlam * dlam) * t2 + (dlam * dlam * dlam
                * dlam) * t3 + (dlam * dlam * dlam * dlam * dlam * dlam) + 0.5;
        t6 = sn * c * ok;
        t7 = sn * (c * c * c) * (1.0 - (t * t) + eta) / 6.0;
        *easting = fe + dlam * t6 + (dlam * dlam * dlam) * t7 + 0.5;
        if (*northing >= 9999999.0) *northing = 9999999.0;
}

//=======================================================================
// Purpose:
//  This function converts the specified lat/lon coordinate to a UTM
//  coordinate in the WGS84 datum.  (See the comment block for the
//  LatLonToUtm() member function.)
//=======================================================================

void LatLonToUtmWGS84 (int* utmXZone, char* utmYZone,
        double* easting, double* northing, double lat, double lon) 
{
        LatLonToUtm (6378137.0, 1 / 298.257223563, utmXZone, utmYZone,
                easting, northing, lat, lon);
}