/* -*- c++ -*- */
/*
 * Copyright 2006,2010 Free Software Foundation, Inc.
 *
 * This file is part of GNU Radio
 *
 * GNU Radio is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3, or (at your option)
 * any later version.
 *
 * GNU Radio is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with GNU Radio; see the file COPYING.  If not, write to
 * the Free Software Foundation, Inc., 51 Franklin Street,
 * Boston, MA 02110-1301, USA.
 */

#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

#include <atsc_fpll.h>
#include <gr_io_signature.h>
#include <atsc_consts.h>
#include <algorithm>
#include "fpll_btloop_coupling.h"
#include <gr_math.h>

atsc_fpll_sptr
atsc_make_fpll()
{
  return gnuradio::get_initial_sptr(new atsc_fpll());
}


/*
 * I strongly suggest that you not mess with these...
 *
 * They are strongly coupled into the symbol timing code and
 * their value also sets the level of the symbols going
 * into the equalizer and viterbi decoder.
 */
static const float FPLL_AGC_REFERENCE = 2.5 * FPLL_BTLOOP_COUPLING_CONST;
static const float FPLL_AGC_RATE = 0.25e-6;



atsc_fpll::atsc_fpll()
  : gr_sync_block("atsc_fpll",
		  gr_make_io_signature(1, 1, sizeof(float)),
		  gr_make_io_signature(1, 1, sizeof(float))),
		  initial_phase(0)
{
  initial_freq = 5.75e6 - 3e6 + 0.31e6 + 5e3; // a_initial_freq;
  agc.set_rate (FPLL_AGC_RATE);
  agc.set_reference (FPLL_AGC_REFERENCE);
  initialize();
}


void
atsc_fpll::initialize ()
{
  float Fs = 19.2e6;

  float alpha = 1 - exp(-1.0 / Fs / 5e-6);

  afci.set_taps (alpha);
  afcq.set_taps (alpha);

  printf("Setting initial_freq: %f\n",initial_freq);
  nco.set_freq (initial_freq / Fs * 2 * M_PI);
  nco.set_phase (initial_phase);
}


int
atsc_fpll::work (int noutput_items,
		       gr_vector_const_void_star &input_items,
		       gr_vector_void_star &output_items)
{
  const float *in = (const float *) input_items[0];
  float *out = (float *) output_items[0];

  for (int k = 0; k < noutput_items; k++){

    float a_cos, a_sin;

    float input = agc.scale (in[k]);

    nco.step ();                // increment phase
    nco.sincos (&a_sin, &a_cos);  // compute cos and sin

    float I = input * a_sin;
    float Q = input * a_cos;

    out[k] = I;

    float filtered_I = afci.filter (I);
    float filtered_Q = afcq.filter (Q);

    // phase detector

    // float x = atan2 (filtered_Q, filtered_I);
    float x = gr_fast_atan2f(filtered_Q, filtered_I);

    // avoid slamming filter with big transitions

    static const float limit = M_PI / 2;

    if (x > limit)
      x = limit;
    else if (x < -limit)
      x = -limit;

    // static const float alpha = 0.037;   // Max value
    // static const float alpha = 0.005;   // takes about 5k samples to pull in, stddev = 323
    // static const float alpha = 0.002;   // takes about 15k samples to pull in, stddev =  69
                                           //  or about 120k samples on noisy data,
    static const float alpha = 0.001;
    static const float beta = alpha * alpha / 4;

    nco.adjust_phase (alpha * x);
    nco.adjust_freq (beta * x);

  }

  return noutput_items;
}