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 # previous sample in sinusoid (1.0 for sample 1)
    q2 = 1.0 # previous previous sample (approximately 1.0 for sample 1)
    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():
    samp_per_cycle = 512
    cycles = 3
    len = cycles * samp_per_cycle
    signal = goertzel_sinusoid(1, cycles, samp_per_cycle, 1)
    reference = np.array(sinusoid(len, cycles))
    error = reference - signal
    print("max error:", error.max())
    x = np.array(range(0, len))
    plt.plot(x, signal)
    plt.plot(x, reference)
    plt.plot(x, error)
    #plt.plot(x, error*100)
    plt.show()

if __name__ == "__main__":
    main()