/*!
 * \file tracking_FLL_PLL_filter.cc
 * \brief Implementation of a hybrid FLL and PLL filter for tracking carrier loop
 * \author Javier Arribas, 2011. jarribas(at)cttc.es
 *
 * Class that implements hybrid FLL and PLL filter for tracking carrier loop
 * Filter design (Kaplan 2nd ed., Pag. 181 Fig. 181)
 *
 * -------------------------------------------------------------------------
 *
 * Copyright (C) 2010-2011  (see AUTHORS file for a list of contributors)
 *
 * GNSS-SDR is a software defined Global Navigation
 *          Satellite Systems receiver
 *
 * This file is part of GNSS-SDR.
 *
 * GNSS-SDR 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 of the License, or
 * at your option) any later version.
 *
 * GNSS-SDR 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 GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
 *
 * -------------------------------------------------------------------------
 */

#include "tracking_FLL_PLL_filter.h"
#include <iostream>

void tracking_FLL_PLL_filter::set_params(float fll_bw_hz,float pll_bw_hz, int order)
{
    /*
     * Filter design (Kaplan 2nd ed., Pag. 181 Fig. 181)
     */
    d_order=order;
    if (d_order==3)
        {
            /*
             *  3rd order PLL with 2nd order FLL assist
             */
            d_pll_b3 = 2.400;
            d_pll_a3 = 1.100;
            d_pll_a2 = 1.414;
            d_pll_w0p = pll_bw_hz/0.7845;
            d_pll_w0p2 = d_pll_w0p*d_pll_w0p;
            d_pll_w0p3 = d_pll_w0p2*d_pll_w0p;

            d_pll_w0f = fll_bw_hz/0.53;
            d_pll_w0f2 = d_pll_w0f*d_pll_w0f;
        }else
            {
                /*
                 * 2nd order PLL with 1st order FLL assist
                 */
                d_pll_a2 = 1.414;
                d_pll_w0p = pll_bw_hz/0.53;
                d_pll_w0p2 = d_pll_w0p*d_pll_w0p;
                d_pll_w0f = fll_bw_hz/0.25;
            }

}
void tracking_FLL_PLL_filter::initialize(float d_acq_carrier_doppler_hz)
{
    if (d_order==3)
        {
            d_pll_x 		= 2.0*d_acq_carrier_doppler_hz;
            d_pll_w	= 0;
        }else{
                d_pll_w 		= d_acq_carrier_doppler_hz;
                d_pll_x 		= 0;
        }
}

float tracking_FLL_PLL_filter::get_carrier_error(float FLL_discriminator, float PLL_discriminator, float correlation_time_s)
{
    float carrier_error_hz;
    if (d_order==3)
        {
            /*
             *  3rd order PLL with 2nd order FLL assist
             */
            d_pll_w = d_pll_w + correlation_time_s * (d_pll_w0p3 * PLL_discriminator + d_pll_w0f2 * FLL_discriminator);
            d_pll_x = d_pll_x + correlation_time_s * (0.5*d_pll_w + d_pll_a2 * d_pll_w0f * FLL_discriminator + d_pll_a3 * d_pll_w0p2 * PLL_discriminator);
            carrier_error_hz  = 0.5*d_pll_x + d_pll_b3 * d_pll_w0p * PLL_discriminator;
        }else
            {
                /*
                 * 2nd order PLL with 1st order FLL assist
                 */
                float pll_w_new;
                pll_w_new = d_pll_w + PLL_discriminator*d_pll_w0p2*correlation_time_s + FLL_discriminator*d_pll_w0f*correlation_time_s ;
                carrier_error_hz = 0.5*(pll_w_new + d_pll_w)+d_pll_a2 * d_pll_w0p*PLL_discriminator;
                d_pll_w =pll_w_new;
                /*std::cout<<" d_pll_w = "<<carrier_error_hz<<
			   ", pll_w_new = "<<pll_w_new
			   <<", PLL_discriminator=" <<PLL_discriminator
			   <<" FLL_discriminator ="<<FLL_discriminator
			   <<" correlation_time_s = "<<correlation_time_s<<"\r\n";*/
            }

    return carrier_error_hz;

}

tracking_FLL_PLL_filter::tracking_FLL_PLL_filter ()
{
}

tracking_FLL_PLL_filter::~tracking_FLL_PLL_filter ()
{

}