/*
 * Amc1210Analizer.cpp
 *
 *  Created on: 16 мар. 2018 г.
 *      Author: titov
 */

#include "Amc1210Analizer.hh"

#include "../../math/math_inc.hh"

float driver::detail::Amc1210Analizer::getSinChannel() const {
    return raw.sin;
}

float driver::detail::Amc1210Analizer::getCosChannel() const {
    return raw.cos;
}

void driver::detail::Amc1210Analizer::setSampleTime( float Ts ) {

    zero_crossing.max_delta = max_speed * Ts;
    non_update.max_delta = max_speed * Ts;
    spike.max_delta = max_speed * Ts;

}

driver::detail::Amc1210Analizer::Amc1210Analizer(
        driver::IResolver & scrt_amc1210) : amc_scrt(scrt_amc1210),
                code(0.0f, 0.0f), prev(0.0f, 0.0f), prev_delta(0.0f, 0.0f), old(0.0f, 0.0f),
                raw(0.0f, 0.0f), valid_signal(false), max_speed(0.0f), count_error(0), failure_amc1210(false),
                zero_crossing(), non_update(), overflow(), spike() {}

bool driver::detail::Amc1210Analizer::configure( Setting & config ) {

    max_speed = config.max_frequency * config.max_amplitude * math::constants::pi2;

    zero_crossing.max_value = config.max_value;
    non_update.min_delta = config.max_noise;
    overflow.max_value = config.max_value;

    return true;
}

void driver::detail::Amc1210Analizer::process() {

    code = Quad( amc_scrt.getSinChannel(), amc_scrt.getCosChannel() );

    Quad correct_1 = zero_crossing.correct( code, prev );

    Quad cur_delta = correct_1 - prev;

    Quad correct_2 = non_update.correct( correct_1, cur_delta, prev_delta );

    bool valid = true;

    valid = overflow.validate(  correct_2 )                 && valid;
    valid = spike.validate( correct_2, cur_delta, old )     && valid;

    valid_signal = valid;

    if( !valid || raw == correct_2 ) {
        if( count_error < max_error )
            count_error++;
        else
            failure_amc1210 = true;
    } else {
        count_error = 0;
        failure_amc1210 = false;
    }

    old = prev;
    prev = correct_1;
    raw = correct_2;
    prev_delta = cur_delta;

}

driver::detail::Amc1210Analizer::Quad driver::detail::Amc1210Analizer::ZeroCrossingCorrection::correct(
        const Quad & current, const Quad & previous ) const {

    //Обработка неудачного перехода через 0.
    if(     ( previous.sin <= 0 && previous.sin > - max_delta )
        ||  ( previous.sin >= 0 && previous.sin <   max_delta ) ) {

        if( current.sin < ( - max_value ) )
            return Quad( current.sin + 32767, current.cos );

        if( current.sin > (   max_value ) )
            return Quad( current.sin - 32768, current.cos );

    }

    //Обработка неудачного перехода через 0.
    if(     ( previous.cos <= 0 && previous.cos > - max_delta )
        ||  ( previous.cos >= 0 && previous.cos <   max_delta ) ) {

        if( current.cos < ( - max_value ) )
            return Quad( current.sin, current.cos + 32767 );

        if( current.cos > (   max_value ) )
            return Quad( current.sin, current.cos - 32768 );

    }

    return Quad( current.sin, current.cos );
}

driver::detail::Amc1210Analizer::Quad driver::detail::Amc1210Analizer::NonUpdateValues::correct(
        const Quad & current, const Quad & current_delta, const Quad & previous_delta ) const {

    const float prev_dsin = fabsf( previous_delta.sin );

    //Проверка на обновление данных в движении.
    if( ( prev_dsin > min_delta )
            && ( prev_dsin < max_delta )
            && ( current_delta.sin == 0.0f ) )

        return Quad( current.sin + previous_delta.sin * 0.5f, current.cos );

    const float prev_dcos = fabsf( previous_delta.cos );

    //Проверка на обновление данных в движении.
    if( ( prev_dcos > min_delta )
            && ( prev_dcos < max_delta )
            && ( current_delta.cos == 0.0f ) )

        return Quad( current.sin, current.cos + previous_delta.cos * 0.5f );

    return Quad( current.sin, current.cos );
}

bool driver::detail::Amc1210Analizer::Overflow::validate( Quad & current ) {

    using namespace std;

    bool result( true );

    //Проверка на переполнение и невозможные данные.
    if( fabsf( current.sin ) > max_value ) {

        current.sin = NAN;

        result = false;
    }

    //Проверка на переполнение и невозможные данные.
    if( fabsf( current.cos ) > max_value ) {

        current.cos = NAN;

        result = false;
    }

    //Проверка на ноль.
    if( current.sin == 0.0f && current.cos == 0.0f ) {

        current.sin = current.cos = NAN;

        result = false;
    }

    //Проверка на -1.
    if( current.sin == -1.0f && current.cos == -1.0f ) {

        current.sin = current.cos = NAN;

        result = false;
    }


    return result;
}

bool driver::detail::Amc1210Analizer::Spike::validate(
        Quad & current, const Quad & current_delta, const Quad & old ) {

    bool result( true );

    const float delta_data_0 = fabsf( current_delta.sin );
    const float old_delta_data_0 = fabsf( current.sin - old.sin );

    if( delta_data_0 > max_delta
            && old_delta_data_0 > max_delta ) {

        current.sin = NAN;

        result = false;

    }

    const float delta_data_1 = fabsf( current_delta.cos );
    const float old_delta_data_1 = fabsf( current.cos - old.cos );

    if( delta_data_1 > max_delta
            && old_delta_data_1 > max_delta ) {

        current.cos = NAN;

        result = false;

    }

    return result;
}

float driver::detail::Amc1210Analizer::getSquareSumChannel() const {

    if( valid_signal )
        return raw.sin * raw.sin + raw.cos * raw.cos;
    else
        return NAN;

}

bool driver::detail::Amc1210Analizer::Setting::isValid() {

    using namespace std;

    const Setting not_valid;

    if( !isfinite( max_frequency ) || max_frequency <= 0.0f )
        max_frequency = not_valid.max_frequency;

    if( !isfinite( max_amplitude ) || max_amplitude <= 0.0f )
        max_amplitude = not_valid.max_amplitude;

    if( !isfinite( max_value ) || max_value <= 0.0f )
        max_value = not_valid.max_value;

    if( !isfinite( max_noise ) || max_noise <= 0.0f )
        max_noise = not_valid.max_noise;

    if( max_noise > max_value )
        max_noise = not_valid.max_noise;

    if( max_amplitude > max_value )
        max_amplitude = not_valid.max_amplitude;

    if( max_amplitude < max_noise )
        max_noise = not_valid.max_noise;

    return max_frequency != not_valid.max_frequency
            and max_amplitude != not_valid.max_amplitude
            and max_value != not_valid.max_value
            and max_noise != not_valid.max_noise;

}

driver::detail::Amc1210Analizer::Setting::Setting() :
                max_frequency(NAN), max_amplitude(NAN),
                max_value(NAN), max_noise(NAN) {}