#!/usr/bin/python

from Numeric import *
from LinearAlgebra import *
from math import *
from random import *
def make_vec(bearing,inclination):
    #convert to radians...
    bearing = bearing*pi/180
    inclination = inclination*pi/180
    return array([cos(bearing)*cos(inclination),
                  sin(bearing)*cos(inclination),
                  sin(inclination)])

def cross_product(a,b):
    return array([a[1] * b[2] - a[2] * b[1],
                  a[2] * b[0] - a[0] * b[2],
                  a[0] * b[1] - a[1] * b[0]])

def normalise(a):
    return a/sqrt(dot(a,a))

def mx(a,b):
    return matrixmultiply(a,b)

def get_device_transform(bearing,inclination,rotation):
    rotation = rotation*pi/180
    Dr = make_vec(bearing,inclination)
    O1 = normalise(cross_product(Dr,Gr))
    O2 = normalise(cross_product(Dr,O1))
    rotater = array([[1,0,0],
                     [0,cos(-rotation),-sin(-rotation)],
                     [0,sin(-rotation),cos(-rotation)]])
    return (mx(inverse(array([Dr,O1,O2])),rotater)) # transform into device coords

#demo...
dip = uniform(-80,80)

Mr = make_vec(0,dip)
Gr = make_vec(0,-90)


def MakeSensors(bear,incl,rot):
    Td = get_device_transform(bear,incl,rot)
    return(mx(Mr,Td), mx(Gr,Td))

def InventSensor(axis):
    theta = uniform(-5,5) * pi/180
    phi = uniform(-5,5) * pi/180
    result = array([0.,0.,0.])
    result[axis % 3] = cos(phi) * cos(theta)
    result[(axis+1) % 3] = sin(phi)
    result[(axis+2) % 3] = sin(theta)
    return result 

# main code....

# generate calibration matrices...
Mc = inverse(array([InventSensor(0),InventSensor(1),InventSensor(2)]))
Gc = inverse(array([InventSensor(0),InventSensor(1),InventSensor(2)]))
# Ms/Gs is scaling multipliers; takes into 10-bit form (with +256 approximately equal to 1)
Ms = array([uniform(240,272),uniform(240,272),uniform(240,272)])
Gs = array([uniform(240,272),uniform(240,272),uniform(240,272)])
# Mt/Gt are offsets... about 5% error max
Mt = array([uniform(-12,12),uniform(-12,12),uniform(-12,12)])
Gt = array([uniform(-12,12),uniform(-12,12),uniform(-12,12)])


#get number of outs; here we'll say 20
for x in range(60):
    bearing = uniform(0,360)
    inclination = uniform(-50,50)
    rotation = uniform(-180,180)
    (Md,Gd) = MakeSensors(bearing,inclination,rotation)
    Md = (mx(Md,Mc) * Ms) + Mt
    Gd = (mx(Gd,Gc) * Gs) + Gt
#    Md = Md * 256
#    Gd = Gd * 256
    print round(bearing),round(inclination),round(Md[0]),round(Md[1]),round(Md[2]),round(Gd[0]),round(Gd[1]),round(Gd[2])
#    print round(bearing),round(inclination),Md[0],Md[1],Md[2],Gd[0],Gd[1],Gd[2]