// spll2.cpp -- software PLL
// (C) 2000, C. Bond. All rights reserved. 
//
// These PLL simulators are based on the methods and 'structograms'
// presented in Chapter 5 of the text, "Phase-Locked Loops, Theory,
// Design and Applications", by Roland E. Best, McGraw-Hill, Inc.,
// 1993.
//
#include <float.h>
#include <math.h>
#include <iostream.h>

static float ud[2],uc[2],uf[2],u2[3],phi2[3],Q[2],tm[2],t2[2];
static float a1,b0,b1,w0,K0,Kd,T,phi0,tau1,tau2,Ub,u10;


/*
 * This version of a linear PLL simulator operates on signal
 * vectors. Given a signal, 'u1', vector of length, 'len',
 * and appropriate parameter values, it calculates and
 * returns corresponding vectors for the output values of
 * the phase detector 'ud', the filter 'uf', the
 * DCO 'u2' and the phase accumulator 'phi2'.
 */
void lpll(int len,float *u1,float *u2,float *ud,float *uf,
    float *phi2,float a1,float b0,float b1,float T,float w0,
    float K0,float Kd)
{
    int n;

// Initialize
    ud[0] = 0.0;
    uf[0] = 0.0;
    u2[1] = 0.0;
    phi2[1] = 0.0;
    n = 1;
// Loop
    while (n < (len-1)) {
        ud[n] = Kd * u1[n] * u2[n];
        uf[n] = -a1*uf[n-1] + b0*ud[n] + b1*ud[n-1];
        phi2[n+1] = phi2[n] + (w0 + K0*uf[n]) * T;

        if (phi2[n+1] > M_PI)
            phi2[n+1] -= 2*M_PI;
        if (phi2[n+1] >= 0.0)
            u2[n+1] = 1.0;
        else
            u2[n+1] = -1.0;
// Shift
        n++;
    }
}
/*
 * This routine sets up global (static) variables for use
 * by the reentrant version of the linear PLL simulator,
 * 'lpll2'.
 *
 * It can be called at any time to reset the operating
 * conditions of the PLL.
 */
void initlpll2(float detector,float filter,float dco,float phase,
    float a1coeff,float b0coeff,float b1coeff,float omega0,
    float gainK0,float gainKd, float sampleT){

    ud[0] = detector;
    uf[0] = filter;
    u2[0] = 0.0;
    u2[1] = dco;
    phi2[0] = 0.0;
    phi2[1] = phase;
    a1 = a1coeff;
    b0 = b0coeff;
    b1 = b1coeff;
    w0 = omega0;
    K0 = gainK0;
    Kd = gainKd;
    T = sampleT;
}
/*
 * This version of the linear PLL simulator processes a single
 * input event. It accepts a value for the signal and returns
 * the updated values for the phase detector, filter, DCO and
 * phase accumulator.
 *
 * The routine is reentrant, as it saves critical values in
 * static variables so they are carried forward from one call
 * to the next.
 *
 * It is a requirement that the static variables be initialized
 * by 'initpll2' prior to the first call to lpll2.
 */
void lpll2(float sig,float *detector,float *filter,float *dco,float *phase)
{
// Calculate new quantities
    ud[1] = Kd * sig * u2[1];
    uf[1] = -a1*uf[0] + b0*ud[1] + b1*ud[0];
    phi2[2] = phi2[1] + (w0 + K0*uf[1]) * T;

// Collapse phase and determine dco
    if (phi2[2] > M_PI)
        phi2[2] -= 2.0*M_PI;
    if (phi2[2] >= 0.0)
        u2[2] = 1.0;
    else
        u2[2] = -1.0;

// Shift
    ud[0] = ud[1];
    uf[0] = uf[1];
    phi2[1] = phi2[2];
    u2[1] = u2[2];

// Set return values
    *detector = ud[0];
    *filter = uf[0];
    *dco = u2[1];
    *phase = phi2[1];
}

/*
 * This version of a digital PLL simulator operates on vectors. The
 * primary difference between this PLL and the linear PLL is
 * the replacement of a multiplier type phase detector in the linear
 * PLL with a digital phase/frequency detector.
 *
 * In the linear PLL, the input signal is assumed to be sampled at
 * regular (T) intervals. For the digital PLL, a signal vector, 'u1',
 * and a time vector, 't1', must be provided. This simulates operation
 * in an interrupt driven system where the time vector takes the
 * place of a 'time stamp' captured at the start of an interrupt
 * triggered by edges of the input signal.
 *
 */
void dpll(int len,float *u1,float *t1,float *t2,float *Q,float *uc,
    float *uf,float *phi2,float tau1,float tau2,float w0,float K0,float Ub)
{
    int n,idx;
    float Tn,tcross,tp,tm,oldphi,newphi;
    float tmp1,tmp2;

// Initialize
    uc[0] = 0.0;
    uf[0] = 0.0;
    Q[0] = 0.0;
    phi2[0] = 0.0;
    t2[0] = 0.0;
    oldphi = newphi = 0.0;

    n = 1;
    idx = 1;
    while (n < len) {
        Tn = t1[n] - t1[n-1];

// Calculate and correct phi2
        phi2[n] = phi2[n-1] + (w0 + K0 * uf[n-1]) * Tn;
        if (phi2[n] > 2.0 * M_PI) {
            phi2[n] -= 2.0 * M_PI;
            phi2[n-1] -= 2.0 * M_PI;
        }
// Old code for updating t2...
        newphi = oldphi + (w0 + K0 * uf[n-1]) * Tn;
        if (newphi > M_PI) {
            t2[idx++] = t1[n-1] + (M_PI - oldphi)/(newphi-oldphi)*Tn;
            newphi -= M_PI;
        }

        oldphi = newphi;

// Calculate tcross
        if ((phi2[n] >= 0.0) && (phi2[n-1] < 0.0))
            tcross = -phi2[n-1]*Tn/(phi2[n]-phi2[n-1]);
        else
            tcross = 0.0;

// Calculate tp, tm, Q[n]
        if (Q[n-1] == +1.0) {
            if (tcross == 0.0) {
                tp = Tn;
                tm = 0.0;
                Q[n] = 1.0;
            }
            else {
                tp = tcross;
                tm = 0.0;
                Q[n] = 0.0;
            }
        }
        else if (Q[n-1] == 0.0) {
            if (u1[n-1] == 1.0) {
                if (tcross == 0.0) {
                    tp = Tn;
                    tm = 0.0;
                    Q[n] = 1.0;
                }
                else {
                    tp = tcross;
                    tm = 0.0;
                    Q[n] = 0.0;
                }
            }
            else {
                if (tcross == 0.0) {
                    tp = 0.0;
                    tm = 0.0;
                    Q[n] = 0.0;
                }
                else {
                    tp = 0.0;
                    tm = Tn - tcross;
                    Q[n] = -1.0;
                }
            }
        }
        else {      // (Q[n-1] == -1)
            if (u1[n-1] == 1.0) {
                if (tcross == 0.0) {
                    tp = 0.0;
                    tm = 0.0;
                    Q[n] = 0.0;
                }
                else {
                    tp = 0.0;
                    tm = Tn - tcross;
                    Q[n] = -1.0;
                }
            }
            else {
                tp = 0.0;
                tm = Tn;
                Q[n] = -1.0;
            }
        }
// Update filter
        if (tp > 0.0) {
            uc[n] = uc[n-1] + (Ub-uc[n-1])*tp/(tau1+tau2);
            uf[n] = uc[n] + (tp/Tn)*(tau2/(tau1+tau2))*(Ub-uc[n]);
        }
        else {
            if (tm > 0.0) {
                uc[n] = uc[n-1] * (1.0 - tm/(tau1+tau2));
                uf[n] = uc[n] * (1.0 - (tau2/(tau1+tau2))*(tm/Tn));
            }                  
            else {
                uc[n] = uc[n-1];
                uf[n] = uc[n];
            }
        }
        n++;
    }
}
void initdpll2(float cap,float filter,float Qstate,float phase,
    float phi,float gainK0,float gainKd,float omega0,float t0,
    float sig0,float tK1,float tK2,float Vc)
{
    uc[0] = cap;
    uf[0] = filter;
    Q[0] = Qstate;
    T = t0;
    phi2[0] = phase;
    K0 = gainK0;
    Kd = gainKd;
    w0 = omega0;
    phi0 = phi;
    u10 = sig0;
    tau1 = tK1;
    tau2 = tK2;
    Ub = Vc;
   
}

void dpll2(float sig,float timing,float *t2,float *filter,
    float *phase)
{
    int idx;
    float Tn,tcross,tp,tm,newphi;

    Tn = timing - T;

// Calculate and correct phi2
    phi2[1] = phi2[0] + (w0 + K0 * uf[0]) * Tn;
    if (phi2[1] > 2.0 * M_PI) {
    phi2[1] -= 2.0 * M_PI;
    phi2[0] -= 2.0 * M_PI;
    }
    newphi = phi0 + (w0 + K0 * uf[0]) * Tn;
    if (newphi > M_PI) {
        *t2 = T + (M_PI - phi0)/(newphi-phi0)*Tn;
        newphi -= M_PI;
        idx++;
    }
    phi0 = newphi;

// Calculate tcross
    if ((phi2[1] >= 0.0) && (phi2[0] < 0.0))
        tcross = -phi2[0]*Tn/(phi2[1]-phi2[0]);
    else
        tcross = 0.0;

// Calculate tp, tm, Q[1]
    if (Q[0] == +1.0) {
        if (tcross == 0.0) {
            tp = Tn;
            tm = 0.0;
            Q[1] = 1.0;
        }
        else {
            tp = tcross;
            tm = 0.0;
            Q[1] = 0.0;
        }
    }
    else if (Q[0] == 0.0) {
        if (u10 == 1.0) {
            if (tcross == 0.0) {
                tp = Tn;
                tm = 0.0;
                Q[1] = 1.0;
            }
            else {
                tp = tcross;
                tm = 0.0;
                Q[1] = 0.0;
            }
        }
        else {
            if (tcross == 0.0) {
                tp = 0.0;
                tm = 0.0;
                Q[1] = 0.0;
            }
            else {
                tp = 0.0;
                tm = Tn - tcross;
                Q[1] = -1.0;
            }
        }
    }
    else {      // (Q[0] == -1)
        if (u10 == 1.0) {
            if (tcross == 0.0) {
                tp = 0.0;
                tm = 0.0;
                Q[1] = 0.0;
            }
            else {
                tp = 0.0;
                tm = Tn - tcross;
                Q[1] = -1.0;
            }
        }
        else {
            tp = 0.0;
            tm = Tn;
            Q[1] = -1.0;
        }
    }
// Update filter
    if (tp > 0.0) {
        uc[1] = uc[0] + (Ub - uc[0]) * tp / (tau1+tau2);
        uf[1] = uc[1] + (tp/Tn) * (tau2/(tau1+tau2)) * (Ub-uc[1]);
    }
    else {
        if (tm > 0.0) {
            uc[1] = uc[0] * (1.0 - tm/(tau1+tau2));
            uf[1] = uc[1] * (1.0 - (tau2/(tau1+tau2))*(tm/Tn));
        }                  
        else {
            uc[1] = uc[0];
            uf[1] = uc[1];
        }
    }
// Shift variables
}