def getSeteliteParameters(dane, dist):

    numerSatelity = dane[0]
    print "wyniki dla satelity %d" % numerSatelity
    a0 = dane[7]
    a1 = dane[8]
    a2 = dane[9]
    A = dane[17]
    dn = dane[12]
    M0 = dane[13]
    e = dane[15]
    w = dane[24]
    idot = dane[26]
    I0 = dane[22]
    omega0 = dane[20]
    omegaDot = dane[25]
    toe = dane[18]
    print "toe: %f" % toe


    cus = dane[16]
    crs = dane[11]
    cuc = dane[14]

    cic = dane[19]
    cis = dane[21]
    crc = dane[23]

    dt = tsv - toe

    print "dt: %f" % dt

    tprop = dist / c
    print "tprop: %f" % tprop


    dts = a0 + a1*dt + a2*dt
    print "dts: %f" % dts

    t = tsv - dts - tprop
    print "t: %f" % t

    u = 3.986005E+14
    Oe = 7.2921151467*pow(10,-5)

    A = pow(A,2)

    print "A: %f" % A

    n0 = math.sqrt(u/pow(A,3))
    print "n0: %f" % n0

    tk = t - toe

    print "tk: %f" % tk

    n = n0 + dn
    print "n: %f" % n

    Mk = M0+n*tk

    print "Mk: %f" % Mk

    Ek = 1
    Ek0 = Mk
    while abs(Ek0 - Ek) > 1E-15:
        Ek = Ek0
        Ek0 = Mk + e*math.sin(Ek)

    print "Ek0: %f" % Ek0
    Ek = Ek0

    vk = math.atan2((math.sqrt(1-pow(e,2))*math.sin(Ek)/(1-e*math.cos(Ek))),((math.cos(Ek)-e)/(1-e*math.cos(Ek))))

    print "vk: %f" % vk

    Ek = math.acos((e+math.cos(vk))/(1+e*math.cos(vk)))

    Fik = vk + w

    print "Fik: %f" % Fik

    duk = cus*math.sin(2*Fik)+cuc*math.cos(2*Fik)
    drk = crs*math.sin(2*Fik)+crc*math.cos(2*Fik)
    dik = cis*math.sin(2*Fik)+cic*math.cos(2*Fik)

    print "duk: %f" % duk
    print "drk: %f" % drk
    print "dik: %f" % dik

    uk = Fik + duk

    print "uk: %f" % uk

    rk = A*(1-e*math.cos(Ek))+drk

    print "rk: %f" % rk

    ik = I0+dik+idot*tk

    print "ik: %f" % ik

    xkp = rk*math.cos(uk)
    ykp = rk*math.sin(uk)

    print "xkp: %f" % xkp
    print "ykp: %f" % ykp

    Ok = omega0+(omegaDot-Oe)*tk-Oe*toe

    print "Ok: %f" % Ok


    xk = xkp*math.cos(Ok) - ykp*math.cos(ik)*math.sin(Ok)
    yk = xkp*math.sin(Ok) + ykp*math.cos(ik)*math.cos(Ok)
    zk = ykp*math.sin(ik)

    print "xk: %f" % xk
    print "yk: %f" % yk
    print "zk: %f" % zk
    print "do pliku %d" % numerSatelity, dist, xk, yk, zk, "%f15" % dts
    return numerSatelity, xk, yk, zk, dts


def getL(dist, dt):
    return dist + dt*c

def getRo(X,Y,Z):
    return math.sqrt((X-X0) ** 2 + (Y-Y0) ** 2 + (Z-Z0) ** 2)

wsp2 = []
A =[]
L =[]

for (i, elem) in enumerate(data):
    print "i", i
    dist = distances[i]
    wynik = getSeteliteParameters(elem, dist)
    wsp2.append(wynik)
    X = wynik[1]
    Y = wynik[2]
    Z = wynik[3]
    dt = wynik[4]
    ro = getRo(X,Y,Z)

    print "ro", ro
    print "dist", dist

    elem1 = -(X-X0)/ro
    elem2 = -(Y-Y0)/ro
    elem3 = -(Z-Z0)/ro
    A.append([elem1, elem2, elem3, c])
    L.append(getL(dist,dt)-getRo(X,Y,Z))



At = np.transpose(A)
wynik = np.dot((np.linalg.inv(np.dot(At,A))),(np.dot(At,L)))
V = np.dot(A,wynik) - L

m = np.dot(np.transpose(V),V)/(len(A)-len(wynik))

print A
print L

for elem in wynik:
    print "%0.5f" % elem
print V

print "X0", X0+wynik[0]
print "Y0", Y0+wynik[1]
print "Z0", Z0+wynik[2]
print "dt %f15" % wynik[3]
print math.sqrt(m)