def getdRAdY(rvec, rvecdot, R, delta, t, t0):
    rvec = list(rvec)
    rvec1 = rvec[:]
    rvec2 = rvec[:]
    rvec1[1] = rvec1[1] + delta
    rvec2[1] = rvec2[1] - delta
    OE = babyOD2(rvec1, rvecdot, t)
    dec1, ra1 = ephemeris2(OE[0], OE[1], OE[2], OE[3], OE[4], OE[5], t0, t, R)
    OE = babyOD2(rvec2, rvecdot, t)
    dec2, ra2 = ephemeris2(OE[0], OE[1], OE[2], OE[3], OE[4], OE[5], t0, t, R)
    return (ra1 - ra2) / (2 * delta), (dec1 - dec2) / (2 * delta)
def getdRAdZdot(rvec, rvecdot, R, delta, t, t0):
    rvecdot = list(rvecdot)
    rvecdot1 = rvecdot[:]
    rvecdot2 = rvecdot[:]
    rvecdot1[2] = rvecdot1[2] + delta
    rvecdot2[2] = rvecdot2[2] - delta
    OE = babyOD2(rvec, rvecdot1, t)
    dec1, ra1 = ephemeris2(OE[0], OE[1], OE[2], OE[3], OE[4], OE[5], t0, t, R)
    OE = babyOD2(rvec, rvecdot2, t)
    dec2, ra2 = ephemeris2(OE[0], OE[1], OE[2], OE[3], OE[4], OE[5], t0, t, R)
    return (ra1 - ra2) / (2 * delta), (dec1 - dec2) / (2 * delta)
def correct(rvec, rvecdot, R, delta, t, t0):
    rvecdot = list(rvecdot)
    rvec = list(rvec)
    for i in range(len(t)):
        OE = babyOD2(rvec, rvecdot, t[i])

        DECfit, RAfit = ephemeris2(OE[0], OE[1], OE[2], OE[3], OE[4], OE[5],
                                   t0, t[i], R[i])

        parX.extend(getdRAdX(rvec, rvecdot, R[i], delta, t[i], t0))
        parY.extend(getdRAdY(rvec, rvecdot, R[i], delta, t[i], t0))
        parZ.extend(getdRAdZ(rvec, rvecdot, R[i], delta, t[i], t0))
        parXdot.extend(getdRAdXdot(rvec, rvecdot, R[i], delta, t[i], t0))
        parYdot.extend(getdRAdYdot(rvec, rvecdot, R[i], delta, t[i], t0))
        parZdot.extend(getdRAdZdot(rvec, rvecdot, R[i], delta, t[i], t0))

        deltas.extend([abs(ra[i] - RAfit), abs(dec[i] - DECfit)])

    masterArr = [[parX, parY, parZ, parXdot, parYdot, parZdot],
                 [parX, parY, parZ, parXdot, parYdot, parZdot],
                 [parX, parY, parZ, parXdot, parYdot, parZdot],
                 [parX, parY, parZ, parXdot, parYdot, parZdot],
                 [parX, parY, parZ, parXdot, parYdot, parZdot],
                 [parX, parY, parZ, parXdot, parYdot, parZdot]]

    partialArr = [[parX], [parY], [parZ], [parXdot], [parYdot], [parZdot]]
    for i in range(6):
        for j in range(6):
            masterArr[i][j] = np.dot(masterArr[i][j], partialArr[i][0])
    for i in range(6):
        partialArr[i][0] = np.dot(partialArr[i][0], deltas)

    xyzArr = np.matmul(np.linalg.pinv(masterArr), partialArr)
    return xyzArr
def RMS(r2, r2dot, R, t, t0):
    deltalist = []
    r2 = list(r2)
    r2dot = list(r2dot)
    for i in range(len(t)):
        OE = babyOD2(r2, r2dot, t[i])
        a, e, I, o, w, M, MP = babyOD3(r2, r2dot, t[i])
        DECfit, RAfit = ephemeris2(OE[0], OE[1], OE[2], OE[3], OE[4], OE[5],
                                   t0, t[i], R[i])
        deltalist.extend([abs(ra[i] - RAfit), abs(dec[i] - DECfit)])
    return sqrt(np.dot(deltalist, deltalist) / (len(t) * 2 - 6))