#include "yin_cpp.h"
 
// Takes in 32 bit samples from the ADC until a window of 1800 samples is filled.
// Then executes the YIN algorithm over the window and produces an output phase increment for the Xilix DDS IP block
 
void yin(udata_t *y, data_t x)
{
    const data_t max_val = 8388607;
     
    // Constants for DDS conversion
    const scale_t b_theta = 4294967295.0; // 2^32 - 1
    const freq_t sys_clock = 100000000.0;
    const frac_t root_thresh = 0.5;
    const ufrac_t pinc_scale = b_theta / sys_clock;
 
    ufrac_t tau;
    int i;
    static int count = 0;
 
    frac_t x_reg[WINDOW_LENGTH];
 
    ufrac_t freq;
    ufrac_t d_tsum = 0;
    ufrac_t d_t = 0;
    ufrac_t d_tprime[TMAX];
 
    static udata_t phase_inc = 0;
 
    if(count < WINDOW_LENGTH)
    {
        x_reg[count] = (frac_t) ((temp_t) x / max_val);
        count++;
    }
 
    if(count >= WINDOW_LENGTH)
    {
        // Once the window is full, calculate the result
        tau_loop : for(tau = TMIN; tau < TMAX; tau++)
        {
            d_t = 0;
 
            // Calculate d_t(tau) = (x(i) - x(i+tau))^2
            i_loop : for(i = 0; i < W; i++)
            {
                d_t += (ufrac_t) ((x_reg[i] - x_reg[i+tau]) * (x_reg[i] - x_reg[i+tau]));
            }
 
            // Accumulate values of d_t up to the current tau
            d_tsum += d_t;
 
            // Calculate d_t'
            d_tprime[tau] = (ufrac_t) (d_t / ((ufrac_t) d_tsum / tau));
 
            /* Peak Detection
             * The first value that crosses the threshold is the root value
             * Must be a valley so it avoids the initial dip near tau = 1 */
            if(tau > TMIN + 2)
            {
                // Check threshold
                if(d_tprime[tau-1] < root_thresh)
                {
                    // Check for a valley: d[tau-2] > d[tau-1] < d[tau]
                    if((d_tprime[tau-2] > d_tprime[tau-1]) && (d_tprime[tau] > d_tprime[tau-1]))
                    {
                        // Convert tau value into phase increment for DDS block
                        freq = ((ufrac_t) SAMPLE_FREQ) / ((ufrac_t) tau);
 
                        phase_inc = (udata_t) (NUM_CH * freq * pinc_scale);
 
                        // Don't need to calculate any more values so exit the loop
                        count = 0;
                        break;
                    }
                }
            }
        }
    }
 
    // Always output the value for the sequencing unit
    *y = phase_inc;
 
}