import math
import numpy as np

#define data
data = [6.224, 6.665, 6.241, 5.302, 5.073, 5.127, 4.994, 5.012, 5.108, 5.377, 5.510, 6.372]
m = len(data)
b = np.reshape(data,(m,1))
time = [0.0, 1/12, 2/12, 3/12, 4/12, 5/12, 6/12, 7/12, 8/12, 9/12, 10/12, 11/12]

#Create the A matrix from the time
Amatrix = []
n = 0
while n < m:
    Amatrix.append([1.0, round(math.cos(2*math.pi*time[n]), 12), round(math.sin(2*math.pi*time[n]), 12),])
    n = n + 1
    pass
Amatrix = np.array(Amatrix)
print(Amatrix)

#Create and use Transpose
TransposeMatrix = Amatrix.transpose()
print(TransposeMatrix)
Cmatrix = np.matmul(TransposeMatrix, Amatrix)
print(Cmatrix)

#Fix error when ATranspose times A has a 0 on the diagonal, may cause problems if not careful
f = 0
while f < 3:
    if Cmatrix[f,f] == 0:
        Cmatrix[f,f] = .0000000000000001
        pass
    f = f + 1
    pass

TransposeTimesb = np.matmul(TransposeMatrix, b)
print(TransposeTimesb)
C = np.linalg.solve(Cmatrix,TransposeTimesb)
print(C)

#Calculate Error
r = np.subtract(b, np.matmul(Amatrix, C))
k = 0
SE = 0
while k < m:
    SE = SE + (r.item(k))**2

    k = k + 1
    pass
RMSE = math.sqrt(SE/m)

print("F3(t) = ", C.item(0), " + ", C.item(1), "* cos(2(pi)t) + ", C.item(2), "* sin(2(pi)t)")
print("RMSE = ", RMSE)
