#!/usr/bin/env python

Vtrue =  [0,    1,    2,    5]
V =      [0.09, 1.05, 1.72, 4.15]
sigmaV = [0.1,  0.1,  0.1,  0.1]

def matvecmul2x2(M, rhs):
    return [M[0][0] * rhs[0] + M[1][0] * rhs[1], M[1][0] * rhs[0] + M[1][1] *
            rhs[1]]

# y = a * x + b
#
# V ^
#   |
#   |
#   +-------->
#            Vtrue

def calcStraightLine(xs, ys, sigma_ys):
    sx, sw = 0, 0
    for i in range(0, len(xs)):
        w = 1. / (sigma_ys[i] * sigma_ys[i])
        sx += w * xs[i]
        sw += w
    x0 = 0 # sx / sw

    mat = [[0, 0], [0, 0]]
    rhs = [0, 0]
    for i in range(0, len(xs)):
        w = 1. / (sigma_ys[i] * sigma_ys[i])
        dx = xs[i] - x0
        mat[0][0] += w * dx * dx
        mat[1][0] += w * dx
        mat[1][1] += w
        rhs[0] += w * ys[i] * dx
        rhs[1] += w * ys[i]

    det = mat[0][0] * mat[1][1] - mat[1][0] * mat[1][0]

    invmat = [[mat[1][1]/det, -mat[1][0] / det], [ -mat[1][0] / det, mat[0][0] / det]]

    sol = matvecmul2x2(invmat, rhs) 

    return x0, mat, rhs, invmat, sol

x0,mat, rhs, invmat, sol = calcStraightLine(Vtrue, V, sigmaV)
print(x0)
print(mat)
print(rhs)
print(invmat)
print(sol)
