/* -*- c++ -*- */
/*
 * Copyright 2011,2013,2018 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 <gnuradio/blocks/control_loop.h>
#include <gnuradio/math.h>
#include <stdexcept>

namespace gr {
namespace blocks {

#define M_TWOPI (2.0f * GR_M_PI)

control_loop::control_loop(float loop_bw, float max_freq, float min_freq)
    : d_phase(0), d_freq(0), d_max_freq(max_freq), d_min_freq(min_freq)
{
    // Set the damping factor for a critically damped system
    d_damping = sqrtf(2.0f) / 2.0f;

    // Set the bandwidth, which will then call update_gains()
    set_loop_bandwidth(loop_bw);
}

control_loop::~control_loop() {}

void control_loop::update_gains()
{
    float denom = (1.0 + 2.0 * d_damping * d_loop_bw + d_loop_bw * d_loop_bw);
    d_alpha = (4 * d_damping * d_loop_bw) / denom;
    d_beta = (4 * d_loop_bw * d_loop_bw) / denom;
}

void control_loop::advance_loop(float error)
{
    d_freq = d_freq + d_beta * error;
    d_phase = d_phase + d_freq + d_alpha * error;
}

void control_loop::phase_wrap()
{
    while (d_phase > M_TWOPI)
        d_phase -= M_TWOPI;
    while (d_phase < -M_TWOPI)
        d_phase += M_TWOPI;
}

void control_loop::frequency_limit()
{
    if (d_freq > d_max_freq)
        d_freq = d_max_freq;
    else if (d_freq < d_min_freq)
        d_freq = d_min_freq;
}

/*******************************************************************
 * SET FUNCTIONS
 *******************************************************************/

void control_loop::set_loop_bandwidth(float bw)
{
    if (bw < 0) {
        throw std::out_of_range("control_loop: invalid bandwidth. Must be >= 0.");
    }

    d_loop_bw = bw;
    update_gains();
}

void control_loop::set_damping_factor(float df)
{
    if (df <= 0) {
        throw std::out_of_range("control_loop: invalid damping factor. Must be > 0.");
    }

    d_damping = df;
    update_gains();
}

void control_loop::set_alpha(float alpha)
{
    if (alpha < 0 || alpha > 1.0) {
        throw std::out_of_range("control_loop: invalid alpha. Must be in [0,1].");
    }
    d_alpha = alpha;
}

void control_loop::set_beta(float beta)
{
    if (beta < 0 || beta > 1.0) {
        throw std::out_of_range("control_loop: invalid beta. Must be in [0,1].");
    }
    d_beta = beta;
}

void control_loop::set_frequency(float freq)
{
    if (freq > d_max_freq)
        d_freq = d_min_freq;
    else if (freq < d_min_freq)
        d_freq = d_max_freq;
    else
        d_freq = freq;
}

void control_loop::set_phase(float phase)
{
    d_phase = phase;
    while (d_phase > M_TWOPI)
        d_phase -= M_TWOPI;
    while (d_phase < -M_TWOPI)
        d_phase += M_TWOPI;
}

void control_loop::set_max_freq(float freq) { d_max_freq = freq; }

void control_loop::set_min_freq(float freq) { d_min_freq = freq; }

/*******************************************************************
 * GET FUNCTIONS
 *******************************************************************/

float control_loop::get_loop_bandwidth() const { return d_loop_bw; }

float control_loop::get_damping_factor() const { return d_damping; }

float control_loop::get_alpha() const { return d_alpha; }

float control_loop::get_beta() const { return d_beta; }

float control_loop::get_frequency() const { return d_freq; }

float control_loop::get_phase() const { return d_phase; }

float control_loop::get_max_freq() const { return d_max_freq; }

float control_loop::get_min_freq() const { return d_min_freq; }

} /* namespace blocks */
} /* namespace gr */
