import math
import matplotlib.pyplot as plt
import numpy as np

def goertzel_sinusoid(freq, duration, sample_rate, amplitude):
    n = int(duration * sample_rate)
    omega = (2.0 * math.pi * freq) / sample_rate
    coeff = 2.0 * math.cos(omega)
    
    # Initialize state variables
    q1 = 1.0
    q2 = 1.0
    result = []
    
    for i in range(n):
        sample = coeff * q1 - q2
        q2 = q1
        q1 = sample
        result.append(sample * amplitude)
    
    return result

def sinusoid(n, cycles=1):
    result = []
    for i in range(1, n+1):
        sample = math.cos(2.0 * cycles * math.pi * (i/n))
        result.append(sample)
    return result

def main():
    cycles = 2
    signal = goertzel_sinusoid(cycles, 1, 1024, 1)
    reference = np.array(sinusoid(1024, cycles))
    error = reference - signal
    print("max error:", error.max())
    x = np.array(range(0, 1024))
    plt.plot(x, signal)
    plt.plot(x, reference)
    plt.plot(x, error)
    #plt.plot(x, error*1000/3)
    plt.show()

if __name__ == "__main__":
    main()