#include <cstdint>
#include <tuple>
#include <optional>
#include <cassert>

// C++ port of RPLL from https://github.com/quartiq/idsp
// MIT or Apache-2.0 license

/// Reciprocal PLL.
///
/// Consumes noisy, quantized timestamps of a reference signal and reconstructs
/// the phase and frequency of the update() invocations with respect to (and in units of
/// 1 << 32 of) that reference.
/// In other words, `update()` rate ralative to reference frequency,
/// `u32::MAX` corresponding to both being equal.
class RPLL {
    private:
        uint32_t dt2;  // 1 << dt2 is the counter rate to update() rate ratio
        int32_t x;     // previous timestamp
        uint32_t ff;   // current frequency estimate from frequency loop
        uint32_t f;    // current frequency estimate from both frequency and phase loop
        int32_t y;     // current phase estimate
    public:
        RPLL(uint32_t dt2);
        std::tuple<int32_t, uint32_t> update(std::optional<int32_t>, uint32_t, uint32_t);
        int32_t phase();
        uint32_t frequency();
};

/// Create a new RPLL instance.
///
/// Args:
/// * dt2: inverse update() rate. 1 << dt2 is the counter rate to update() rate ratio.
///
/// Returns:
/// Initialized RPLL instance.
RPLL::RPLL(uint32_t dt2):
    dt2(dt2),
    x(0),
    ff(0),
    f(0),
    y(0)
{}

/// Advance the RPLL and optionally supply a new timestamp.
///
/// Args:
/// * input: Optional new timestamp (wrapping around at the i32 boundary).
///   There can be at most one timestamp per `update()` cycle (1 << dt2 counter cycles).
/// * shift_frequency: Frequency lock settling time. 1 << shift_frequency is
///   frequency lock settling time in counter periods. The settling time must be larger
///   than the signal period to lock to.
/// * shift_phase: Phase lock settling time. Usually one less than
///   `shift_frequency` (see there).
///
/// Returns:
/// A tuple containing the current phase (wrapping at the i32 boundary, pi) and
/// frequency.
std::tuple<int32_t, uint32_t> RPLL::update(std::optional<int32_t> input,
                                           uint32_t shift_frequency,
                                           uint32_t shift_phase)
{
    assert(shift_frequency >= dt2);
    assert(shift_phase >= dt2);
    // Advance phase
    y += f;
    if(input.has_value()) {
        // Reference period in counter cycles
        int32_t dx = *input - x;
        // Store timestamp for next time
        x = *input;
        // Phase using the current frequency estimate
        uint64_t p_sig_64 = (uint64_t)ff * (uint64_t)dx;
        // Add half-up rounding bias and apply gain/attenuation
        uint32_t p_sig =
            (p_sig_64 + (1 << (shift_frequency - 1))) >> shift_frequency;
        // Reference phase (1 << dt2 full turns) with gain/attenuation applied
        uint32_t p_ref = 1 << (32 + dt2 - shift_frequency);
        // Update frequency lock
        ff += p_ref - p_sig;
        // Time in counter cycles between timestamp and "now"
        uint32_t dt = -x & ((1 << dt2) - 1);
        // Reference phase estimate "now"
        int32_t y_ref = (f >> dt2)*dt;
        // Phase error with gain
        int32_t dy = (y_ref - y) >> (shift_phase - dt2);
        // Current frequency estimate from frequency lock and phase error
        f = ff + dy;
    }
    return { y, f };
}

/// Return the current phase estimate
int32_t RPLL::phase()
{
    return y;
}

/// Return the current frequency estimate
uint32_t RPLL::frequency()
{
    return f;
}