//
// Created by sozonov on 10.11.2021.
//

#include "SignalIntegrator.hh"


schematic::SignalIntegrator::SignalIntegrator( Value & _inp ) :
        i_input( _inp )
        , input ( 0.0f )
        , output( 0.0f )
        , dt( 0.0f )
        , init_state ( 0.0f )
        , wrap_value( 0.0f , 0.0f )
        , saturation( HUGE_VALF, -HUGE_VALF )
        , saturation_upper_reached ( false )
        , saturation_down_reached ( false )
        , delta_wrap (  0.0f )
        , reset_to(output)
{

}

schematic::SignalIntegrator::SignalIntegrator( Value & _inp, float _wrap_up, float _wrap_down, float _pos_sat, float _neg_sat, float _init_state ) :
        i_input( _inp )
        , input ( 0.0f )
        , output( _init_state )
        , dt( 0.0f )
        , init_state ( _init_state )
        , wrap_value( _wrap_up > init_state ? ( _wrap_up >= _wrap_down ? _wrap_up : _wrap_down ) : 0,
                      _wrap_down < init_state ? ( _wrap_up >= _wrap_down ? _wrap_down  : _wrap_up ) : 0 )

        , saturation( _pos_sat, _neg_sat )
        , saturation_upper_reached ( false )
        , saturation_down_reached ( false )
        , delta_wrap ( wrap_value.first - wrap_value.second )
        , reset_to(output)
{

}

schematic::SignalIntegrator::Setting::Setting() :
     init_state ( HUGE_VALF ),
     wrap_up ( HUGE_VALF ),
     wrap_down ( -HUGE_VALF ),
     positive_saturation ( -HUGE_VALF ),
     negative_saturation ( HUGE_VALF ) {}


bool schematic::SignalIntegrator::Setting::isValid() {

    using namespace std;

    const Setting non_valid;

    if( !isfinite( init_state ) ) {
        init_state = non_valid.init_state;
    }

    if( !isfinite( wrap_up ) ) {
        wrap_up = non_valid.wrap_up;
    }

    if( !isfinite( wrap_down ) ) {
        wrap_down = non_valid.wrap_down;
    }


    if( positive_saturation < 0 ) {
        positive_saturation = non_valid.positive_saturation;
    }

    if( negative_saturation > 0 ) {
        negative_saturation = non_valid.negative_saturation;
    }


    return init_state != non_valid.init_state and wrap_up != non_valid.wrap_up
           and wrap_down != non_valid.wrap_down
           and positive_saturation != non_valid.positive_saturation
           and negative_saturation != non_valid.negative_saturation;
}

bool schematic::SignalIntegrator::configure( Setting & config ) {

    init_state = config.init_state;

    output = init_state;

    set_wrap_value( config.wrap_up, config.wrap_down);
    set_saturation( config.positive_saturation, config.negative_saturation );

    return true;

}

void schematic::SignalIntegrator::set_input(float _in_value) {
    input = _in_value;
}

float schematic::SignalIntegrator::get_output_value() const {
    return output;
}

void schematic::SignalIntegrator::process() {

    value_update();

    integration();
    check_saturation();
    wrap_state();

}

void schematic::SignalIntegrator::integration() {
    output += input * dt;
}

void schematic::SignalIntegrator::set_wrap_value( float _wr_val_up, float _wr_val_down ) {
    wrap_value.first = _wr_val_up > init_state ?
                       ( _wr_val_up >= _wr_val_down ? _wr_val_up : _wr_val_down ) : 0;
    wrap_value.second = _wr_val_down < init_state ?
                        ( _wr_val_up >= _wr_val_down ? _wr_val_down  : _wr_val_up ) : 0 ;

    delta_wrap = wrap_value.first - wrap_value.second;
}

void schematic::SignalIntegrator::wrap_state() {

    if( output > wrap_value.first ){
        output -= delta_wrap;
    }
    if( output < wrap_value.second ){
        output += delta_wrap;
    }


}

void schematic::SignalIntegrator::set_saturation( float _positive_saturation, float _negative_saturation ) {
    saturation.first = _positive_saturation > 0 ? _positive_saturation : 0;
    saturation.second = _negative_saturation < 0 ? _negative_saturation : 0;
}

void schematic::SignalIntegrator::check_saturation() {

    saturation_down_reached = false;
    saturation_upper_reached = false;

    if( output >= saturation.first ) {
        saturation_upper_reached = true;
        output = saturation.first;
    }

    if( output <= saturation.second ) {
        saturation_down_reached = true;
        output = saturation.second;
    }

}

void schematic::SignalIntegrator::set_dt(float _dt) {
    if( _dt > 0){
        dt = _dt;
    }
}

const bool &schematic::SignalIntegrator::get_saturation_upper_reached() {
    return saturation_upper_reached;
}

const bool &schematic::SignalIntegrator::get_saturation_down_reached() {
    return saturation_down_reached;
}

void schematic::SignalIntegrator::value_update() {

    input = i_input;

}

void schematic::SignalIntegrator::reset() {
    output = init_state;
}