// PLL Simulation
// http://www.holmea.demon.co.uk/Frac2/Simulate.htm
// Copyright (c) Andrew Holme 2007

#include <stdlib.h>
#include <stdio.h>
#include <assert.h>
#include <string.h>
#include <math.h>

#include "fftw3.h"


// Sampling rate
#if 0
    const int fs = 10000000; // 10 MHz
    const double VCO_NOISE = 79.2e-6;
    #define LPF(v) (pz70 * pz74.Model(pz73.Model(pz72.Model(pz71.Model(v)))))
#else
    const int fs =  1000000; //  1 MHz
    const double VCO_NOISE = 25.1e-6;
    #define LPF(v) (pz60 * pz64.Model(pz63.Model(pz62.Model(pz61.Model(v)))))
#endif


// #define VCO_NOISE 0
#define __PFD__ AD9901


// Fractional frequency control (N.F)
const unsigned N = 160;
const unsigned F = 1;


// Constants
const int    fr = 100000;
const double Ts = 1.0/fs;
const double PI = 3.14159265358979323846;
const double kVco = 2e6;


struct POLEZERO {
    long double pole, zero, prevx, prevy;
    POLEZERO(long double p, long double z) : pole(p), zero(z), prevx(0), prevy(0) {}
    long double Model(long double x) {
        long double y = x + zero*prevx - pole*prevy;
        prevy = y;
        prevx = x;
        return y;
    }       
};


// Bilinear transform of loop filter
// Ts=1e-7
#define  pz70  1.3508159064426688e-010L
POLEZERO pz71(-0.99740290400416609L, 1L);
POLEZERO pz72(-0.9981498612243902L, -0.99983166399996226L);
POLEZERO pz73(-0.99546485257049411L, 1L);
POLEZERO pz74(-1L, 1L);
// Ts=1e-6
#define  pz60  1.2986891184199671e-007L
POLEZERO pz61(-0.97432903399994919L, 1L);
POLEZERO pz62(-0.98165136119408702L, -0.99831791283132709L);
POLEZERO pz63(-0.95555551981374376L, 1L);
POLEZERO pz64(-1L, 1L);


struct MASH {
    unsigned a[4];      // Accumulators
    unsigned c[4][4];   // Carry flip flops

    MASH() {
        // Initial values
        memset(c, 0, sizeof c);
        a[0] = 1;
        a[1] = 0;
        a[2] = 0;
        a[3] = 0;
    }

    int Clock(unsigned F);
};


int MASH::Clock(unsigned F) {
    register union {
        __int64  i64;
        unsigned i32[2];
    };

    // Carry flip-flops
    c[3][3] = c[3][2]; c[3][2] = c[3][1]; c[3][1] = c[3][0];
                       c[2][2] = c[2][1]; c[2][1] = c[2][0];
                                          c[1][1] = c[1][0];
    // Accumulators
    i64=a[0]; i64+=F;    a[0] = i32[0]; c[0][0] = i32[1];
    i64=a[1]; i64+=a[0]; a[1] = i32[0]; c[1][0] = i32[1];
    i64=a[2]; i64+=a[1]; a[2] = i32[0]; c[2][0] = i32[1];
    i64=a[3]; i64+=a[2]; a[3] = i32[0]; c[3][0] = i32[1];

    // Comment-out lines below to lower MASH order
    return  c[0][0]
        +   c[1][0] -   c[1][1]
        +   c[2][0] - 2*c[2][1] +   c[2][2]
        +   c[3][0] - 3*c[3][1] + 3*c[3][2] - c[3][3]
        ;
}


int vdd = 1;


struct NAND {
    static NAND *gates[];
    static int num_gates;
    
    int state, *out, *in[3];

    void Update() {
        state = !(*in[0] && *in[1] && *in[2]);
    }

    int Propagate() {
        if (*out == state) return 0;
        *out = state;
        return 1;
    }

    NAND() {
        gates[num_gates++] = this;
    }

    void Init(int& y, int& a, int& b, int& c = vdd) {
        out = &y, in[0] = &a, in[1] = &b, in[2] = &c;
    }

    static void Model() {
        int i, changes;
        do {
            for (i=0; i<num_gates; i++) gates[i]->Update();
            changes=0;
            for (i=0; i<num_gates; i++) changes += gates[i]->Propagate();
        } while(changes);
    }   
};


NAND *NAND::gates[100];
int NAND::num_gates;


struct DFF {    // D-type FF based on TTL 74LS74
    NAND nand[6];
    int net[4];

    DFF(int& q, int& qb, int& data, int& clk, int& setb, int& clrb) {
        nand[0].Init(net[0], setb, net[3], net[1]);
        nand[1].Init(net[1], net[0], clrb, clk);
        nand[2].Init(net[2], net[1], clk,  net[3]);
        nand[3].Init(net[3], net[2], clrb, data);
        nand[4].Init(q, setb, net[1], qb);
        nand[5].Init(qb, q, clrb, net[2]);

        // Prevent metastability
        q=0, qb=net[1]=1;
    }
};


struct TFF : DFF { // Toggling FF
    int qb;
    TFF(int& q, int& clk) : DFF(q, qb, qb, clk, vdd, vdd) {}
};


struct XOR {
    NAND nand[5];
    int net[4];

    XOR(int& out, int& a, int& b) {
        nand[0].Init(net[0], a, a);
        nand[1].Init(net[1], b, b);
        nand[2].Init(net[2], a, net[1]);
        nand[3].Init(net[3], b, net[0]);
        nand[4].Init(out, net[2], net[3]);
    }
};


struct AD9901 : XOR { // AD9901-type PFD
    TFF ref_tff, vco_tff;
    DFF ref_dff, vco_dff;
    NAND nand[2];

    int ref_tff_q, ref_dff_q, ref_dff_qb;
    int vco_tff_q, vco_dff_q, vco_dff_qb;
    int pfd_out, xout, nout;

    AD9901(int& ref_clk, int& vco_clk) :
        ref_tff(ref_tff_q, ref_clk),
        vco_tff(vco_tff_q, vco_clk),
        XOR(xout, ref_tff_q, vco_tff_q),
        ref_dff(ref_dff_q, ref_dff_qb, xout, ref_clk, vdd, vco_dff_q),
        vco_dff(vco_dff_q, vco_dff_qb, xout, vco_clk, ref_dff_qb, vdd) {
        nand[0].Init(nout, xout, vco_dff_q);
        nand[1].Init(pfd_out, ref_dff_qb, nout);
    }

    double Vout() {
        return 5*(pfd_out-0.5);
    }
};


struct TPFD { // Tri-state dual D-type PFD
    DFF ref_dff, vco_dff;
    NAND nand;

    int up, dn, nc[2], resetb;

    TPFD(int& ref_clk, int& vco_clk) :
        ref_dff(up, nc[0], vdd, ref_clk, vdd, resetb),
        vco_dff(dn, nc[1], vdd, vco_clk, vdd, resetb) {
        nand.Init(resetb, up, dn);
    }

    double Vout() {
        return 5*(up-dn);
    }
};


struct XPD : XOR { // Simple XOR gate PD
    int xout;
    XPD(int& ref_clk, int& vco_clk) : XOR(xout, ref_clk, vco_clk) {}
    double Vout() {
        return 5*(xout-0.5);
    }
};


struct PLL {
    MASH    mash;
    __PFD__ pfd;
    int     ref_clk, vco_clk;

    long double divisor, phi, omega;

    PLL() : 
        pfd(ref_clk, vco_clk), 
        divisor(0), phi(0), 
        ref_clk(0), vco_clk(0) {
        NAND::Model();
    }

    long double Interpolate(bool ref_edge, long double vco_edge);
    long double Interpolate(bool ref_edge);
    double Model();
};


long double PLL::Interpolate(bool ref_edge, long double vco_edge) {
    long double area=0;

    // Integrate from 0 to 0.5
    if (vco_edge<0.5) {
        area = pfd.Vout() * vco_edge;
        vco_clk=1;
        NAND::Model();
        area += pfd.Vout() * (0.5-vco_edge);
    } else
        area = pfd.Vout() * 0.5;

    // Timestep midpoint
    if (ref_edge) ref_clk=!ref_clk;
    if (vco_edge==0.5) vco_clk=1;
    if (vco_clk || ref_edge) NAND::Model();

    // Integrate from 0.5 to 1
    if (vco_edge>0.5) {
        area += pfd.Vout() * (vco_edge-0.5);
        vco_clk=1;
        NAND::Model();
        area += pfd.Vout() * (1-vco_edge);
    } else
        area += pfd.Vout() * 0.5;

    vco_clk=0;
    NAND::Model();

    return area;
}


long double PLL::Interpolate(bool ref_edge) {
    long double area = pfd.Vout() * 0.5;
    if (ref_edge) {
        ref_clk=!ref_clk;
        NAND::Model();
    }
    return area + pfd.Vout() * 0.5;
}


// Normally distributed (Gaussian) white noise with unity RMS amplitude
double Noise_1V_rms() {
    static double x1, x2;
    static int i;

    if (i=!i) {
        // x1,x2 = Uniformly distributed random pair in (0,1]
        x1 = (rand()+1)/32768.0;
        x2 = (rand()+1)/32768.0;

        // Box-Muller transform
        return sqrt(-2*log(x1))*cos(2*PI*x2);
    } else
        return sqrt(-2*log(x1))*sin(2*PI*x2);
}


double PLL::Model() {
    long double v;
    static int ref;

    // 100 KHz Reference frequency
    if (++ref==fs/fr/2)
        ref=0;

    // VCO Divider
    if (phi >= 2*PI*divisor) {
        phi -= 2*PI*divisor;
        divisor = N + mash.Clock(F);
        v = Interpolate(!ref, 1.0 - phi/omega/Ts); // PFD edge position
    } else
        v = Interpolate(!ref);

    // Loop filter
    v = LPF(v);
    
    // VCO
    v += VCO_NOISE * Noise_1V_rms();    // Inject VCO noise
    omega = 2*PI*(15e6 + kVco*v);       // Angular frequency
    phi += omega*Ts;                    // Output phase = Integral of freq.

    return v;
}


int main() {

    FILE *fpt = fopen("..\\time.txt", "w");
    FILE *fpf = fopen("..\\freq.txt", "w");

    PLL loop;
    int i;

    const int PRE = fs*2/100;   // Settling time allowed
    const int LEN = fs;         // FFT Length 1 second

    for (i=0; i<PRE; i++)
        fprintf(fpt, "%g, %.17g\n", i*1e6/fs, loop.Model());
    
    double       *time = (double       *) fftw_malloc(sizeof(double)       * LEN);
    fftw_complex *freq = (fftw_complex *) fftw_malloc(sizeof(fftw_complex) * LEN/2+1);

    for (i=0; i<LEN; i++)
        time[i] = loop.Model() * (1 - cos(2*PI*i/LEN)); // Hanning window

    fftw_plan t2f = fftw_plan_dft_r2c_1d(LEN, time, freq, FFTW_ESTIMATE);
    fftw_execute(t2f);
    fftw_destroy_plan(t2f);

    // VCO Phase noise at offset fm
    int prev=0;
    for (double f=1; f<LEN/2; f<10000?f++:f*=1.0002) {
        int fm;
        // Reference harmonics
        if (f>prev+fr) fm=(prev+=fr);
        else fm=int(f);

        // Deviation
        double fd =  kVco * sqrt(pow(freq[fm][0],2)+pow(freq[fm][1],2))/(LEN/2);

        // Modulation index
        double theta_p = fd/fm;

        // Power spectral density dBc/Hz
        fprintf(fpf, "%d, %0.5g\n", fm, 20*log10(theta_p/2));
    }
    
    fftw_free(time);
    fftw_free(freq);

    fclose(fpt);
    fclose(fpf);
    
    return 0;
}