/*
 * TrackingPositionScrt.cpp
 *
 *  Created on: 26 янв. 2018 г.
 *      Author: titov
 */

#include "TrackingPositionScrt.hh"

using processing::TrackingPositionScrt;

void TrackingPositionScrt::configure( const Setting & setting ) {

    using namespace std;

    _L1Base = setting.L1;
    _L2Base = setting.L2;

    _L1Ts = _L1Base * _Ts,
    _L2Ts = _L2Base * _Ts;

    _ThetaZero = setting.ThetaNull,
    _Np_Nr = setting.Np / setting.Nr;

    _power_max = setting.scrt_power_max * setting.scrt_power_max;
    _power_min = setting.scrt_power_min * setting.scrt_power_min;

    _Tun = math::constants::pi_2 / setting.scrt_speed_max;
    _Nerr = static_cast<unsigned int>( _Tun / _Ts );

    _1_Nr = 1.f / static_cast<float>( setting.Nr );
    _pi2_Nr = math::constants::pi2 / static_cast<float>( setting.Nr );

}

void TrackingPositionScrt::setSampleTime( float Ts ) {

    _L1Ts = _L1Base * Ts;
    _L2Ts = _L2Base * Ts;
    _Nerr = static_cast<unsigned int>( _Tun / Ts );
    _Ts = Ts;

}

processing::TrackingPositionScrt::TrackingPositionScrt(
        driver::IResolver & resolver) : scrt(resolver),
                _Ts(0.0f),
                _power_max(0.0f), _power_min(0.0f), _Tun(0.0f), _Nerr(0.0f),
                _L1Base(0.0f), _L2Base(0.0f),
                _L1Ts(0.0f), _L2Ts(0.0f),
                _ThetaZero(0.0f), _Np_Nr(0.0f),
                _1_Nr(0.0f), _pi2_Nr(0.0f),

                prev_error(0.0f), prev_scrt_sin(0.0f), prev_scrt_cos(0.0f),

                est_sens_speed(0.0f), est_sens_position_pi2(0.0f), est_sens_turn(0.0f), est_sens_error(0.0f),
                est_theta(0.0f), est_omega(0.0f),
                est_position(0.0f), est_speed(0.0f),
                error_counter(0) {}



void TrackingPositionScrt::bad_data_handler() {

    const float demping_k = 0.5f;

    if (error_counter < _Nerr)
        error_counter++;
    else
        est_sens_speed = est_sens_error = 0.0f;

    est_sens_error = est_sens_error * demping_k;
    prev_error = est_sens_error;
}

void TrackingPositionScrt::process() {

    using namespace std;

    //Обновление ошибки системы.
    {
        float scrt_sin = scrt.getSinChannel();
        float scrt_cos = scrt.getCosChannel();

        //В случае не валидных данных принимаем ошибку равной нулю.
        if( isfinite(scrt_sin) && isfinite(scrt_cos) ) {

            float sin_d2 = scrt_sin * scrt_sin;
            float cos_d2 = scrt_cos * scrt_cos;

            float amp_data = ( sin_d2 + cos_d2 );

            if( amp_data < _power_max && amp_data > _power_min ) {

                float est_sin, est_cos;
                math::function::sincos(est_sens_position_pi2, &est_sin, &est_cos);

                float _1_amp_data = math::function::q_rsqrt( amp_data );

                float error_0 = (   scrt_sin * est_cos      - scrt_cos * est_sin        )   * _1_amp_data;
                float error_1 = (   prev_scrt_sin * est_cos - scrt_cos * est_sin   )        * _1_amp_data;
                float error_2 = (   scrt_sin * est_cos      - prev_scrt_cos * est_sin   )   * _1_amp_data;

                float derror_0 = fabsf( prev_error - error_0 );
                float derror_1 = fabsf( prev_error - error_1 );
                float derror_2 = fabsf( prev_error - error_2 );

                est_sens_error =
                        derror_0 < derror_1 and derror_0 < derror_2 ?   error_0 :
                        derror_1 < derror_2 ?                           error_1 :
                                                                        error_2;

                if( error_counter && error_counter != _Nerr )
                    error_counter--;

                prev_error = est_sens_error;
                prev_scrt_sin = scrt_sin;
                prev_scrt_cos = scrt_cos;

            } else bad_data_handler();
        } else bad_data_handler();
    }

    //Сам наблюдатель:
    {
        est_sens_speed = est_sens_speed + est_sens_error * _L2Ts;
        est_sens_position_pi2 = est_sens_position_pi2 + est_sens_speed * _Ts + est_sens_error * _L1Ts;

        if( est_sens_position_pi2 < 0.0f ) {

            est_sens_turn = est_sens_turn - 1.0f;
            est_sens_position_pi2 += math::constants::pi2;

        } else if( est_sens_position_pi2 >= math::constants::pi2 ) {

            est_sens_turn = est_sens_turn + 1.0f;
            est_sens_position_pi2 -= math::constants::pi2;

        }
    }

    //Представление в электрических координатах.
    {
        est_theta = fmodf( est_sens_position_pi2 * _Np_Nr + _ThetaZero, math::constants::pi2 );
        est_omega = est_sens_speed * _Np_Nr;

        if( est_theta < 0.0f )
            est_theta = est_theta + math::constants::pi2;

        else if( est_theta >= math::constants::pi2 )
            est_theta = est_theta - math::constants::pi2;
    }

    //Представление в физических координатах.
    {
        est_speed = _1_Nr * est_sens_speed;
        est_position = _1_Nr * est_sens_position_pi2 + _pi2_Nr * est_sens_turn;
    }
}

bool processing::TrackingPositionScrt::Setting::isValid() {


    const Setting notValidSetting;

    if( L1 > L2 ) {
        L1 = notValidSetting.L1;
        L2 = notValidSetting.L2;
    }

    if( not Np )
        Np = notValidSetting.Np;

    if( not( ( - math::constants::pi2) < ThetaNull && ThetaNull < math::constants::pi2 ) )
        ThetaNull = notValidSetting.ThetaNull;

    if( scrt_power_max < 0.0f )
        scrt_power_max = notValidSetting.scrt_power_max;

    if( scrt_power_min < 0.0f )
        scrt_power_min = notValidSetting.scrt_power_min;

    if( scrt_power_min >= scrt_power_max ) {
        scrt_power_min = notValidSetting.scrt_power_min;
        scrt_power_max = notValidSetting.scrt_power_max;
    }

    if( scrt_speed_max <= 0.0f )
        scrt_speed_max = notValidSetting.scrt_speed_max;

    if( not Nr )
        Nr = notValidSetting.Nr;

    return      L1              != notValidSetting.L1
            and L2              != notValidSetting.L2
            and ThetaNull       != notValidSetting.ThetaNull
            and Np              != notValidSetting.Np
            and scrt_power_max  != notValidSetting.scrt_power_max
            and scrt_power_min  != notValidSetting.scrt_power_min
            and scrt_speed_max  != notValidSetting.scrt_speed_max
            and Nr              != notValidSetting.Nr;

}