import os, sys, struct
import math
import Blender
from Blender.Mathutils import *

def insertRotatedCamera(cam, nframes=36):
    assert(cam.type == 'Camera')

    Blender.Set('curframe',1)
    morig = Matrix(cam.matrix)

    print morig
    
    for i in xrange(0, nframes):
        Blender.Set('curframe', i+1);
        angle = 2.0*math.pi*(float(i)/nframes)

        m = RotationMatrix(angle*180/math.pi, 4, 'z')
        cam.setMatrix(morig*m)
        cam.insertIpoKey(Blender.Object.LOCROT)
        Blender.Window.Redraw()





def singleCamera(cam, nframes=36, dist=7.0, height=2.0):
    for i in xrange(0, nframes):
        Blender.Set('curframe', i+1)
        angle = 2.0*math.pi*(float(i)/nframes)
        
        m = RotationMatrix(angle*180/math.pi, 4, 'z') * RotationMatrix(-math.atan2(dist, height)*180/math.pi, 4, 'x') * TranslationMatrix(Vector([0,-1,-dist]));
        m = m.invert()
        #same as this:
        #loc = Vector([cos(angle)*dist,sin(angle)*dist,height])
        #RotationMatrix(atan2(dist, height)*180/pi, 4, 'x')*RotationMatrix(angle*180/pi+90, 4, 'z')*TranslationMatrix(loc)
        cam.setMatrix(m)
        cam.insertIpoKey(Blender.Object.LOCROT)
        Blender.Window.Redraw()

def cameraToCalib(o, w, h):
    assert(o.type == 'Camera')
    aspect = w/h
    cam=o.getData()
    mat=o.getMatrix()
    m=mat.copy()
    
    minv=mat.copy()
    minv.invert()
    
    mgl=Blender.Mathutils.Matrix([1,0,0,0],[0,0,-1,0],[0,1,0,0],[0,0,0,1])
    mgl.invert()
    
    extr=mgl*minv*RotationMatrix(180.0,4,'x')
    print extr
    lens=cam.getLens()
    tan=math.tan(cam.angle*math.pi/180.0/2)
    f=h/tan/2
    f=f*aspect #w.t.f blender, this is x fov?
    
    print f
    print tan
    print cam.angle

    intr=[f,0.0,w/2.0,0.0,f,h/2.0,0.0,0.0,1.0]
    
    str='CLB '+struct.pack('I',3*8+(9+16+12)*8)
    str+=('INTR'+struct.pack('I',9*8))
    str+=(struct.pack('9d',intr[0],intr[1],intr[2],intr[3],intr[4],intr[5],intr[6],intr[7],intr[8]))
    str+=('EXTR'+struct.pack('I',16*8))
    for i in xrange(4):
        str+=(struct.pack('4d',extr[0][i],extr[1][i],extr[2][i],extr[3][i]))
        
    str+=('PROJ'+struct.pack('I',12*8))
    proj=[[1.0,0.0,0.0,0.0],[0.0,1.0,0.0,0.0],[0.0,0.0,1.0,0.0]]
    for i in xrange(3):
        str+=(struct.pack('4d',proj[i][0],proj[i][1],proj[i][2],proj[i][3]))
    print len(str),3*8+(9+16+12)*8+8

    return str;
    #cfile=file('/tmp/blen/image-%04d.png.clb'%cnt,'wb')
    #cfile.write(str)
    #cfile.close()