/************************************************************************
*
* 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 * 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) * deg2rad;
        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);
}




//=======================================================================
// Purpose:
//  This function converts the specified UTM coordinate to a lat/lon
//  coordinate.
// Pre:
//  - utmXZone must be between 1 and 60, inclusive.
//  - utmYZone must be one of: CDEFGHJKLMNPQRSTUVWX
// 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:
//      The horizontal zone number of the UTM coordinate.
//  char utmYZone:
//      The vertical zone letter of the UTM coordinate.
//  double easting, double northing:
//      The UTM coordinate to convert.
//  double& lat:
//      Upon exit, lat contains the latitude.
//  double& lon:
//      Upon exit, lon contains the longitude.
// 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
//=======================================================================

void UtmToLatLon (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 nfn;
        double tmd;
        double sr;
        double sn;
        double ftphi;
        double s;
        double c;
        double t;
        double eta;
        double de;
        double dlam;
        double olam;

        recf = 1.0 / f;
        b = a * (recf - 1) / 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;
        if ((utmYZone <= 'M' && utmYZone >= 'C')
                || (utmYZone <= 'm' && utmYZone >= 'c')) { 
                nfn = 10000000.0;
        } else {
                nfn = 0;
        }
        tmd = (northing - nfn) / ok;
        sr = sphsr (a, eSquared, 0.0);
        ftphi = tmd / sr;
        double t10, t11, t14, t15;
        for (int i = 0; i < 5; i++) {
                t10 = sphtmd (ap, bp, cp, dp, ep, ftphi);
                sr = sphsr (a, eSquared, ftphi);
                ftphi = ftphi + (tmd - t10) / sr;
        }
        sr = sphsr (a, eSquared, ftphi);
        sn = sphsn (a, eSquared, ftphi);
        s = sin (ftphi);
        c = cos (ftphi);
        t = s / c;
        eta = e2Squared * (c * c);
        de = easting - fe;
        t10 = t / (2.0 * sr * sn * (ok * ok));
        t11 = t * (5.0 + 3.0 * (t * t) + eta - 4.0 * (eta * eta) - 9.0 * (t * t)
                * eta) / (24.0 * sr * (sn * sn * sn) * (ok * ok * ok * ok));
        lat = ftphi - (de * de) * t10 + (de * de * de * de) * t11;
        t14 = 1.0 / (sn * c * ok);
        t15 = (1.0 + 2.0 * (t * t) + eta) / (6 * (sn * sn * sn) * c 
                * (ok * ok * ok));
        dlam = de * t14 - (de * de * de) * t15;
        olam = (utmXZone * 6 - 183.0) * deg2rad;
        lon = olam + dlam;
        lon *= rad2deg;
        lat *= rad2deg;
}

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

void UtmToLatLonWGS84 (int utmXZone, char utmYZone, double easting,
                                           double northing, double& lat, double& lon) 
        {
        UtmToLatLon (6378137.0, 1 / 298.257223563, utmXZone, utmYZone,
                easting, northing, lat, lon);
        }

//=======================================================================
/**
@func Build a position string 
@parm   target. must be 30 characters or longer.
*/
//=======================================================================

void CUtmPoint::GetString(TCHAR *position) const
        {
        _stprintf(position,
                _T("%02d%c %06d %07d"),
                m_xzone, m_yzone,
                (int)m_easting,
                (int)m_northing);
        }

//=======================================================================
/**
@func get the position of a UTM point
@parm   point out
*/
//=======================================================================

void CUtmPoint::ToPosition(CPosition &pos)  const
        {
        double lat,lon;
        UtmToLatLonWGS84(m_xzone,m_yzone,m_easting,m_northing,
                lat,lon);
        pos.Clear();
        pos.SetLatitude(lat);
        pos.SetLongitude(lon);
        }

//=======================================================================
/**
@func turn a position into a UTM point
@parm position
@rdesc  true if it was in range
*/
//=======================================================================

bool CUtmPoint::FromPosition(const  CPosition &pos)
        {
        Clear();
        if(!IsPositionInUtmSpace(pos))
                return false;
        LatLonToUtmWGS84(m_xzone,m_yzone,m_easting,m_northing,
                pos.GetLatitude(),
                pos.GetLongitude());
        return true;
        }