import numpy as np
import matplotlib.pyplot as plt


class fundamental_freq_meas(object):
	"""docstring for fft_meas"""
	def __init__(self, fs=50e3, tn=10):
		self.fs = fs
		self.ts = 1/self.fs
		self.tn = tn
		self.time = 0
		self.y_old = 0
		self.tn_start_time = 0
		self.tn_end_time = 0
		self.zerocross_count = 0
		# Calculated Frequency
		self.freq = 50

		self.t_zc = 0

	def step(self, y, zerocross_flag):

		# Integrate time
		self.time += self.ts

		if zerocross_flag:
			denom = (y - self.y_old)
			if denom != 0.0:  # avoid divide-by-zero
				# linear interpolation to find zero-crossing time
				frac = -self.y_old / denom
				self.t_zc = (self.time - self.ts) + frac * self.ts

				# store first crossing time
				if self.zerocross_count == 0:
					self.tn_start_time = self.t_zc

				self.zerocross_count += 1
				self.tn_end_time = self.t_zc  # always keep last crossing time in this window

		# window finished (independent of whether a crossing happened exactly here)
		if self.time >= self.tn:
			if self.zerocross_count >= 2 and self.tn_end_time > self.tn_start_time:
				self.freq = (self.zerocross_count - 1) / (self.tn_end_time - self.tn_start_time)

			# reset window state
			self.zerocross_count = 0
			self.tn_start_time = 0.0
			self.tn_end_time = 0.0
			self.time -= self.tn  # avoids drift vs. self.time = 0

		self.y_old = y