from __future__ import division, with_statement
import math, copy, csv
from OD_LIB import *
from OD3 import *
from OD2 import *
from Cramer import *

def RRdot(dat, sun, tiny):
    k = 0.017202098947
    c = 2.99792458*10**8
    rtemp = [10**12,10**12, 10**12]
    T1 = k*(dat[0][0]-dat[1][0])
    T3 = k*(dat[2][0]-dat[1][0])
    T0 = T3-T1
    D0 = tripleProduct(dat[0][1], dat[1][1], dat[2][1])
    if D0 == 0:
        raise Exception("reeeee")
    D1 = [tripleProduct(dat[2][1],r[1],dat[1][1]) for r in sun]
    D2 = [tripleProduct(dat[2][1],dat[0][1],r[1]) for r in sun]
    D3 = [tripleProduct(dat[0][1],dat[1][1],r[1]) for r in sun]

    D=[D1, D2, D3]
    A1 = T3/T0
    A3 = -T1/T0
    A2 = -1
    A = [A1, A2, A3]
    rho = [(A[0]*D[i][0]+A[1]*D[i][1]+A[2]*D[i][2])/(A[i]*D0) for i in range(len(D))]
    r = [[i*f-d for f, d in zip(j[1],k[1])] for i, j, k in zip(rho, dat, sun)]
    rold = copy.deepcopy(r[1])
    rdotold = ([(a-b)/T0 for a, b in zip(r[2],r[0])])
    itermax = 100000
    counter = 0
#     print rold, rdotold
#     for l in range(1000):
    while sum([(x-y)**2 for x, y in zip(rold, rtemp)])>tiny:
        if counter>itermax:
            raise RuntimeError("Too many iterations.")
        k=0.017202098947

        t1 = k*((dat[0][0]-rho[0]/c)-(dat[1][0]-rho[1]/c))
        t3 = k*((dat[2][0]-rho[2]/c)-(dat[1][0]-rho[1]/c))
        t0 = t3-t1
        f = []
        g = []
        for s in [t1, t3]:
            f.append(1-(s)**2/(2*(mag(rold)**3))+dot(rold, rdotold)*(s)**3/(2*mag(rold)**5)+((s)**4/(24*mag(rold)**3))*(3*(dot(rdotold, rdotold)/mag(rold)**2-(1/mag(rold)**3))-15*(dot(rold, rdotold)/mag(rold)**2)**2+1/mag(rold)**3))
            g.append((s)-(s)**3/(6*mag(rold)**3)+dot(rold, rdotold)*(s)**4/(4*mag(rold)**5))
#         print f, g
        a1 = g[1]/(f[0]*g[1]-g[0]*f[1])
        a3 = g[0]/(f[1]*g[0]-g[1]*f[0])
        A = [a1, -1, a3]
        b1 = f[1]/(f[1]*g[0]-g[1]*f[0])
        b3 = f[0]/(f[0]*g[1]-g[0]*f[1])
        B = [b1, -1, b3]
        rho = [(A[0]*D[i][0]+A[1]*D[i][1]+A[2]*D[i][2])/(A[i]*D0) for i in range(len(D))]
        r = [[i*f-d for f, d in zip(j[1],k[1])] for i, j, k in zip(rho, dat, sun)]
#         rold = [a*A[0] + b*A[2] for a, b in zip(r[0], r[2])] #Shaheen method
        rtemp = rold
        rold = r[1] #Flynn method
        rdotold = [a*B[0] + b*B[2] for a, b in zip(r[0], r[2])]
        counter += 1
#         print "dasfads,", rold, rdotold
#         break
    return rold, rdotold




if __name__ == "__main__":
    data = []
    with open("testdate.csv","rb") as fil:
        read = csv.reader(fil)
        for l in read:
            data.append([float(l[0]),[float(l[2]),float(l[3]),float(l[4])]])
    for i in data:
        i[1] = normalize(i[1])

    suns = []
    with open("sun.csv","rb") as fil:
        read = csv.reader(fil)
        for l in read:
            suns.append([float(l[0]),[float(l[2]),float(l[3]),float(l[4])]])

    suna = []
    with open("sunast.csv","rb") as fil:
        read = csv.reader(fil)
        for l in read:
            suna.append([float(l[0]),[float(l[2]),float(l[3]),float(l[4])]])

    answers = RRdot(data, suns, 0.00000000000000000000000000001)
    print answers
    print [(i-j)/(j) for i, j in zip(answers[0], suna[1][1])]

### 1000 iterations
# shaheen [-1.7999830616466656e-07, -2.0041305492683978e-07, 2.0346651712492786e-06]
#dumbass [-1.7999829075524797e-07, -2.0041303449284424e-07, 2.0346649638512806e-06]

##10 iterations
# dumb = [0.0011511359310530176, 0.001333089510249852, -0.013556117030343401]
# shaheen = [0.0011511359310401766, 0.00133308951023212, -0.013556117030162917]