#!/usr/bin/python

from Numeric import *
from LinearAlgebra import *
from math import *
from sys import *
import copy

class dataline:
    def __init__(self,line):
        self.Md = array([0.,0.,0.,256.])
        self.Gd = array([0.,0.,0.,256.])
        (self.bearing,self.inclination,
         self.Md[0],self.Md[1],self.Md[2],
         self.Gd[0],self.Gd[1],self.Gd[2]) = map(float,line.split())
        self.Md = self.Md / 256
        self.Gd = self.Gd / 256
        
#note converts 4 vec into 3 vec
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]])
#note converts 4vec into 3 vec
def normalise(a):
    t = sqrt(a[0]*a[0] + a[1]*a[1] + a[2]*a[2])
    return array([a[0]/t,a[1]/t,a[2]/t])

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

def GetReadings(Md,Gd,Mc,Gc):
    down = normalise(mx(Gd,Gc)) * -1
    east = normalise(cross_product(down,mx(Md,Mc)))
    north = normalise(cross_product(east,down))
    Tr = inverse(array([north,east,down]))
    Dr = mx(array([1,0,0]),Tr)
    bout = atan2(Dr[1],Dr[0]) * 180/pi
    incout = atan2(Dr[2],hypot(Dr[0],Dr[1])) * 180/pi
    if bout < 0:
        bout += 360
    return (bout,incout)

def GetError(a1,a2,b1,b2):
    diffb = abs(a1-a2)
    diffi = abs(b1-b2)
    while diffb > 360:
        diffb -= 360
    if diffb > 180:
        diffb = 360 - diffb
    while diffi > 360:
        diffi -= 360
    if diffi > 180:
        diffi = 360 - diffi
    return (diffi*diffi)+(diffb*diffb)

def AssessMatrix(Mc,Gc,data,display):
    result = 0
    for d in data:
        (bearing,inclination) = GetReadings(d.Md,d.Gd,Mc,Gc)
        if (display):
            print '% 3.3f % 2.3f' % ( bearing - d.bearing, inclination - d.inclination)
        result += GetError(d.bearing,bearing,d.inclination,inclination)
    return result

def PrintComparisons(Mc,Gc,data):
    for d in data:
        (b,i) = GetReadings(d.Md,d.Gd,Mc,Gc)
        print b - d.bearing,i - d.inclination

    
def Teeter(Mc,Gc,i,k,delta,axis):
    if axis == 0:
        Mc[i][k] += delta
    elif axis == 1:
        Mc[i][k] -= delta
    elif axis == 2:
        Gc[i][k] += delta
    elif axis == 3:
        Gc[i][k] -= delta
    return
# main code....

def calibrate(data,iterations):
    delta = 1.0 #change to move each option by...
    bestM = identity(4,'d');
    bestG = identity(4,'d');
    bestR = AssessMatrix(bestM,bestG,data,0)
    oldR = bestR + 1
    for scale in range(iterations):
#        print scale
        while (oldR > bestR):
            oldM = bestM
            oldG = bestG
            oldR = bestR
            for i in range(4):
                for j in range(3):
                    for x in range(4):
                        (Mc,Gc) = (oldM.copy(),oldG.copy())
                        Teeter(Mc,Gc,i,j,delta,x)
                        result = AssessMatrix(Mc,Gc,data,0)
                        if result < bestR:
                            bestM = Mc
                            bestG = Gc
                            bestR = result
#                            print bestR/(len(data)*2)
        delta = delta / 2
        oldR = bestR +1
#    PrintComparisons(bestM,bestG,data)
#    print "Average angular deviation",sqrt(bestR/(len(data)*2))
    return (bestM,bestG)



#read in stdin to lists of dicts of bearing, inclination, Md, Gd
stuff = []
for s in stdin.readlines():
    stuff.append(dataline(s))
print "Results"
for f in range(10,35):
    data = stuff[:f]
    test = stuff[-20:]
    M = G = identity(4,'d')
    (M,G) = calibrate(data,11)
    print f,AssessMatrix(M,G,test,0)/(len(test)*2)


