package net.heartshield.filter;

import org.apache.commons.math3.complex.Complex;
import org.apache.commons.math3.util.FastMath;

/**
 * Low-level implementation of phase-locked loop.
 * Works with normalized frequency internally.
 *
 * @author David Madl (git@abanbytes.eu)
 * @date 2017-03-26
 */
class Pll {
    private double mPhase = 0;
    private double mFreq = 0;
    private double mAlpha, mBeta;
    private double mMaxFreq, mMinFreq;

    private static double TWO_PI = 2*Math.PI;

    /**
     * TODO convert freqs
     *
     * internal settings are in terms of radians per sample, not Hz.
     * see https://en.wikipedia.org/wiki/Normalized_frequency_%28unit%29
     *
     * @param bandwidth
     * @param minFreq
     * @param maxFreq
     */
    Pll(double bandwidth, double minFreq, double maxFreq) {
        if(bandwidth < 0.0)
            throw new IllegalArgumentException("bandwidth must be >= 0");
        if(maxFreq < 0.0)
            throw new IllegalArgumentException("maxFreq must be >= 0");
        if(minFreq < 0.0)
            throw new IllegalArgumentException("minFreq must be >= 0");
        if(maxFreq < minFreq)
            throw new IllegalArgumentException("maxFreq must be >= minFreq");

        mMaxFreq = maxFreq;
        mMinFreq = minFreq;

        double damping = Math.sqrt(2.0) / 2.0;
        double denom = (1.0 + 2.0 * damping * bandwidth + bandwidth * bandwidth);
        mAlpha = (4.0 * damping * bandwidth) / denom;
        mBeta = (4.0 * bandwidth * bandwidth) / denom;
    }

    void batch(Complex[] input, double[] output) {
        double err;
        for(int i = 0; i < output.length; i++) {
            output[i] = mFreq;

            Complex in = input[i];
            double phase = FastMath.atan2(in.getImaginary(), in.getReal());
            err = mod2pi(phase - mPhase);

            mFreq += mBeta * err;
            mPhase += mFreq + mAlpha * err;

            while(mPhase > TWO_PI)
                mPhase -= TWO_PI;
            while(mPhase < -TWO_PI)
                mPhase += TWO_PI;

            if(mFreq > mMaxFreq)
                mFreq = mMaxFreq;
            else if(mFreq < mMinFreq)
                mFreq = mMinFreq;
        }
    }

    private static double mod2pi(double in) {
        if(in > Math.PI)
            return in - TWO_PI;
        else if(in < -Math.PI)
            return in + TWO_PI;
        else
            return in;
    }
}
