#!/usr/bin/env python
'Grid and Ellipsoidal Conversion'

# Created 17 January 2011, 20:37
# Copyright 2011, Sean B. Palmer, infomesh.net
# Apache License 2.0

# OSGB36 is a registered trademark of Ordnance Survey

from math import sqrt, radians, degrees, sin, cos, tan

def sec(n): 
   return 1. / cos(n)

# These algorithms are from Ordanance Survey
# Cf. Transformations and OSGM02 user guide.pdf

# Airy 1830 ellipsoid constants
a = 6377563.396 # Semi-major axis in metres
b = 6356256.910 # Semi-minor axis in metres

# Ellipsoid squared eccentricity constant
e2 = ((a**2) - (b**2)) / (a**2) 
e = sqrt(e2)

# Projection constants
N0 = -100000. # Northing of true origin
E0 = 400000. # Easting of true origin

# National Grid projection constants
F0 = 0.9996012717 # Scale factor on central meridian
phi0 = radians(49) # Latitude of true origin, i.e. 49 deg N
lambda0 = radians(-2) # Longitude of true origin, i.e. 2 deg W

def B2(a, b): 
   return (a - b) / (a + b)
n = B2(a, b)

def B3(a, F0, e, phi): 
   return a * F0 * ((1. - (e**2) * (sin(phi)**2)) ** -0.5)

def B4(a, F0, e, phi): 
   return a * F0 * ((1. - (e**2)) * (1. - (e**2) * (sin(phi)**2)) ** -1.5)

def B5(nu, rho): 
   return (nu / rho) - 1.

def B6(b, F0, n, phid, phi0): 
   return b * F0 * (
    ( (1. + n + (5./4.)*(n**2) 
     + (5./4.)*(n**3)) * (phid - phi0) ) -
    ( (3.*n + 3.*(n**2) + (21./8.)*(n**3)) 
     * sin(phid - phi0) * cos(phid + phi0) ) + 
    ( ((15./8.)*(n**2) + (15./8.)*(n**3)) * 
     sin(2.*(phid - phi0)) * cos(2.*(phid + phi0)) ) - 
    ( (35./24.)*(n**3) * sin(3.*(phid - phi0)) 
     * cos(3.*(phid + phi0)) ) )

def value(k, v): 
   if value.debug: 
      print k, v
value.debug = False

def osgb36(lat, lon): 
   'E.g. osgb36(52.65757, 1.71792) -> Easting, Northing'

   phi = radians(lat)
   lambd = radians(lon)

   nu = B3(a, F0, e, phi)
   rho = B4(a, F0, e, phi)
   eta2 = B5(nu, rho)

   value('nu', nu)
   value('rho', rho)
   value('eta2', eta2)

   M = B6(b, F0, n, phi, phi0)
   value('M', M)

   I = M + N0
   II = (nu/2.) * sin(phi) * cos(phi)
   III = ((nu/24.) * sin(phi) * (cos(phi)**3) 
      * (5. - (tan(phi)**2) + 9. * eta2) )
   IIIA = ((nu/720.) * sin(phi) * (cos(phi)**5)
      * (61. - 58. * (tan(phi)**2) + (tan(phi)**4)) )
   IV = nu * cos(phi)
   V = (nu / 6.) * (cos(phi)**3) * ((nu / rho) - (tan(phi)**2))
   VI = ( (nu / 120.) * (cos(phi)**5) * (5 - 18 * (tan(phi)**2)
      + (tan(phi)**4) + 14 * eta2 - 58 * (tan(phi)**2) * eta2) )

   value('I', I)
   value('II', II)
   value('III', III)
   value('IIIA', IIIA)
   value('IV', IV)
   value('V', V)
   value('VI', VI)

   N = (I + II * ((lambd - lambda0)**2) + III * ((lambd - lambda0)**4)
      + IIIA * ((lambd - lambda0)**6))
   E = (E0 + IV * (lambd - lambda0) + V * ((lambd - lambda0)**3)
      + VI * ((lambd - lambda0)**5))

   return E, N

def etrs89(E, N): 
   'E.g. etrs89(651409.903, 313177.270) -> lat, lon in degrees'

   phid = ((N - N0) / (a * F0)) + phi0
   M = B6(b, F0, n, phid, phi0)

   value('phid', phid)
   value('M', M)

   while abs(N - N0 - M) >= (0.0001 * 0.01): 
      phid = ((N - N0 - M) / (a * F0)) + phid
      M = B6(b, F0, n, phid, phi0)
      value('phid', phid)
      value('M', M)

   nu = B3(a, F0, e, phid)
   rho = B4(a, F0, e, phid)
   eta2 = B5(nu, rho)

   value('nu', nu)
   value('rho', rho)
   value('eta2', eta2)

   VII = tan(phid) / (2 * rho * nu)
   VIII = ( tan(phid) / (24. * rho * (nu**3))
      * (5. + 3. * (tan(phid)**2) + eta2 - (9. * (tan(phid)**2) * eta2) ) )
   IX = ( tan(phid) / (720. * rho * (nu**5))
      * (61. + 90. * (tan(phid)**2) + 45. * (tan(phid)**4)) )
   X = sec(phid) / nu
   XI = sec(phid) / (6. * (nu**3)) * ((nu / rho) + 2. * (tan(phid)**2))
   XII = ( sec(phid) / (120. * (nu**5))
      * (5. + 28. * (tan(phid)**2) + 24. * (tan(phid)**4)) )
   XIIA = ( sec(phid) / (5040. * (nu**7)) 
      * (61. + 662. * (tan(phid)**2)
        + 1320. * (tan(phid)**4) + 720. * (tan(phid)**6) ) )

   value('VII', VII)
   value('VIII', VIII)
   value('IX', IX)
   value('X', X)
   value('XI', XI)
   value('XII', XII)
   value('XIIA', XIIA)

   phi = (phid - VII * ((E - E0)**2) + VIII * ((E - E0)**4) 
      - IX * ((E - E0)**6))
   lambd = (lambda0 + X * (E - E0) - XI * ((E - E0)**3)
      + XII * ((E - E0)**5) - XIIA * ((E - E0)**7) )

   value('phi', phi)
   value('lambd', lambd)

   return degrees(phi), degrees(lambd)

def deg(d): 
   a = d // 1
   etc = d % 1
   b = etc // (1. / 60.)
   etc = etc % (1. / 60.)
   c = etc / (1. / 3600.)
   return a, b, c

def osgb36_test(): 
   lat = 52 + (39. / 60.) + (27.2531 / 3600.)
   lon = 1 + (43. / 60.) + (4.5177 / 3600.)

   value.debug = True
   E, N = osgb36(lat, lon)
   print 'E', E
   print 'N', N

   assert round(E, 3) == 651409.903
   assert round(N, 3) == 313177.270

def etrs89_test(): 
   E = 651409.903
   N = 313177.270

   value.debug = True
   phi, lambd = etrs89(E, N)
   print 'N', deg(phi)
   print 'E', deg(lambd)

   assert round(phi, 5) == 52.65757
   assert round(lambd, 5) == 1.71792

def test(): 
   print 'Testing ETRS89 to OSGB36 conversion'
   osgb36_test()
   print 

   print 'Testing OSGB36 to ETRS89 conversion'
   etrs89_test()

if __name__ == '__main__': 
   print __doc__