import math
import cv2
import numpy

def get_maps(xres,yres):
    xlist=[]
    ylist=[]

    constant = 30./41.5*3

    # calculated from official spec: maximum angle (i.e., over the diagonals)
    # is 92 deg. -> angle over half a diagonal's length is 92/2 deg
    constant2= 1./(math.sqrt((1280/2)**2+(720/2)**2))*(92/2)/180*math.pi

    foo = math.tan( 92/2/math.sqrt(16**2+9**2)*16. ) * constant / math.tan(1280/2 * constant2)

    for y in xrange(0,yres):
        xtmp=[]
        ytmp=[]

        yy=(y-yres/2.)/xres # yes, xres.
        for x in xrange(0,xres):
            xx_=(x-xres/2.)/xres
            xx=math.tan(xx_*foo)/foo

            dist = math.sqrt(xx**2 + yy**2)
            
            if (dist != 0):
                xtmp=xtmp+[ 1280/2+  xx/dist/constant2*math.atan(constant*dist)  ]
                ytmp=ytmp+[ 720/2 +  yy/dist/constant2*math.atan(constant*dist)  ]
            else:
                xtmp=xtmp+[0+1280/2]
                ytmp=ytmp+[0+720/2]
            
        xlist=xlist+[xtmp]
        ylist=ylist+[ytmp]
        
        if y % 10 == 0:
            print y

    xmap=numpy.array(xlist).astype('float32')
    ymap=numpy.array(ylist).astype('float32')

    return xmap,ymap

xmap,ymap = get_maps(1600/2,900/2)

img=cv2.imread("cam42.png")
img2=cv2.remap(img, xmap, ymap, cv2.INTER_CUBIC)

cv2.imshow("orig", img)
cv2.imshow("remap", img2)

print img2.shape

while cv2.waitKey(50) & 256 != ord("q"):
    pass