/*
 * FieldCalculation.cpp
 *
 *  Created on: 16 мар. 2021 г.
 *      Author: titov
 */

#include "FieldCalculation.hh"

processing::FieldCalculation::FieldCalculation(systemic::ISignal & position,
                                               systemic::ISignal & speed ) : position(position), speed(speed),
                                               est_theta(0.f), est_omega(0.f), _Np(NAN), _ThetaZero(0.f) {}

void processing::FieldCalculation::configure( const Setting & config ) {

    _ThetaZero = config.ThetaZero;
    _Np = config.Np;

}

const float & processing::FieldCalculation::theta() const {

    return est_theta;

}

const float & processing::FieldCalculation::omega() const {

    return est_omega;

}

void processing::FieldCalculation::process() {

    using namespace std;

    est_theta = fmodf( position * _Np + _ThetaZero, math::constants::pi2 );
    est_omega = speed * _Np;

}