CLK/Storage/Disk/DPLL/DigitalPhaseLockedLoop.hpp

//
//  DigitalPhaseLockedLoop.hpp
//  Clock Signal
//
//  Created by Thomas Harte on 11/07/2016.
//  Copyright 2016 Thomas Harte. All rights reserved.
//

#ifndef DigitalPhaseLockedLoop_hpp
#define DigitalPhaseLockedLoop_hpp

#include <array>
#include <cassert>
#include <memory>
#include <vector>

#include "../../../ClockReceiver/ClockReceiver.hpp"

namespace Storage {

/*!
	Template parameters:

	@c bit_handler A class that must implement a method, digital_phase_locked_loop_output_bit(int) for receving bits from the DPLL.
	@c length_of_history The number of historic pulses to consider in locking to phase.
*/
template <typename BitHandler, size_t length_of_history = 3> class DigitalPhaseLockedLoop {
	public:
		/*!
			Instantiates a @c DigitalPhaseLockedLoop.

			@param clocks_per_bit The expected number of cycles between each bit of input.
		*/
		DigitalPhaseLockedLoop(int clocks_per_bit, BitHandler &handler) :
			bit_handler_(handler), window_length_(clocks_per_bit), clocks_per_bit_(clocks_per_bit) {}

		/*!
			Changes the expected window length.
		*/
		void set_clocks_per_bit(int clocks_per_bit) {
			window_length_ = clocks_per_bit_ = clocks_per_bit;
		}

		/*!
			Runs the loop, impliedly posting no pulses during that period.

			@c number_of_cycles The time to run the loop for.
		*/
		void run_for(const Cycles cycles) {
			offset_ += cycles.as_integral();
			phase_ += cycles.as_integral();
			if(phase_ >= window_length_) {
				auto windows_crossed = phase_ / window_length_;

				// Check whether this triggers any 0s.
				if(window_was_filled_) --windows_crossed;
				for(int c = 0; c < windows_crossed; c++)
					bit_handler_.digital_phase_locked_loop_output_bit(0);

				window_was_filled_ = false;
				phase_ %= window_length_;
			}
		}

		/*!
			Announces a pulse at the current time.
		*/
		void add_pulse() {
			if(!window_was_filled_) {
				bit_handler_.digital_phase_locked_loop_output_bit(1);
				window_was_filled_ = true;
				post_phase_offset(phase_, offset_);
				offset_ = 0;
			}
		}

	private:
		BitHandler &bit_handler_;

		void post_phase_offset(Cycles::IntType new_phase, Cycles::IntType new_offset) {
			// Erase the effect of whatever is currently in this slot.
			total_divisor_ -= offset_history_[offset_history_pointer_].divisor;
			total_spacing_ -= offset_history_[offset_history_pointer_].spacing;

			// Fill in the new fields.
			const auto multiple = std::max((new_offset + (clocks_per_bit_ >> 1)) / clocks_per_bit_, Cycles::IntType(1));
			offset_history_[offset_history_pointer_].divisor = multiple;
			offset_history_[offset_history_pointer_].spacing = new_offset;

			// Add in the new values;
			total_divisor_ += offset_history_[offset_history_pointer_].divisor;
			total_spacing_ += offset_history_[offset_history_pointer_].spacing;

			// Advance the write slot.
			offset_history_pointer_ = (offset_history_pointer_ + 1) % offset_history_.size();

#ifndef NDEBUG
			Cycles::IntType td = 0, ts = 0;
			for(auto offset: offset_history_) {
				td += offset.divisor;
				ts += offset.spacing;
			}
			assert(ts == total_spacing_);
			assert(td == total_divisor_);
#endif

			// In net: use an unweighted average of the stored offsets to compute current window size,
			// bucketing them by rounding to the nearest multiple of the base clocks per bit
			window_length_ = std::max(total_spacing_ / total_divisor_, Cycles::IntType(1));

			// Also apply a difference to phase, use a simple spring mechanism as a lowpass filter.
			const auto error = new_phase - (window_length_ >> 1);
			phase_ -= (error + 1) >> 1;
		}

		struct LoggedOffset {
			Cycles::IntType divisor = 1, spacing = 1;
		};
		std::array<LoggedOffset, length_of_history> offset_history_;
		std::size_t offset_history_pointer_ = 0;

		Cycles::IntType total_spacing_ = length_of_history;
		Cycles::IntType total_divisor_ = length_of_history;

		Cycles::IntType phase_ = 0;
		Cycles::IntType window_length_ = 0;

		Cycles::IntType offset_ = 0;
		bool window_was_filled_ = false;

		int clocks_per_bit_ = 0;
};

}

#endif /* DigitalPhaseLockedLoop_hpp */