import numpy as np

from pyhdf.SD import SD, SDC

DEGS_IN_RAD = 360.0 / (2.0 * np.pi)

COLS = 3712
LINES = 3712

COFF = 1856
LOFF = 1856
CFAC = 781648343
LFAC = 781648343

c = np.arange(1, COLS + 1)
l = np.arange(1, LINES + 1)

x = (c - COFF) / (float(CFAC) / (1 << 16))
y = (l - LOFF) / (float(LFAC) / (1 << 16))

y, x = np.meshgrid(y, x)

s_d = np.sqrt((42164.0 * np.cos(x) * np.cos(y)) ** 2 -
              (np.cos(y) ** 2 + 1.006803 * np.sin(y) ** 2) * 1737121856.0)
s_n = ((42164.0 * np.cos(x) * np.cos(y) - s_d) /
       (np.cos(y) ** 2 + 1.006803 * np.sin(y) ** 2))
s_1 = 42164.0 - s_n * np.cos(x) * np.cos(y)
s_2 = s_n * np.sin(x) * np.cos(y)
s_3 = -s_n * np.sin(y)
s_xy = np.sqrt(s_1 ** 2 + s_2 ** 2)

lat = -np.arctan(1.006803 * s_3 / s_xy) * DEGS_IN_RAD
lon = -np.arctan(s_2 / s_1) * DEGS_IN_RAD

mask = np.isnan(s_d)
lat[mask] = np.nan
lon[mask] = np.nan

# sd = SD('nav.hdf', SDC.WRITE | SDC.CREATE | SDC.TRUNC)
# sds = sd.create('Latitude', SDC.FLOAT64, lat.shape)
# sds[:] = lat
# sds = sd.create('Longitude', SDC.FLOAT64, lon.shape)
# sds[:] = lon

sd = SD('nav_geocat2.hdf', SDC.WRITE | SDC.CREATE | SDC.TRUNC)
sds = sd.create('pixel_latitude', SDC.FLOAT64, lat.shape)
sds[:] = lat[::-1,::-1].T
sds = sd.create('pixel_longitude', SDC.FLOAT64, lon.shape)
sds[:] = lon[::-1,::-1].T