/*
 * TrackingPositionEncoder.cpp
 *
 *  Created on: 22 мая 2018 г.
 *      Author: krugliy
 */

#include "TrackingPositionEncoder.hh"

processing::TrackingPositionEncoder::TrackingPositionEncoder(
		driver::IEncoder & enc, uint32_t max_turn, bool tuning_enable ) :
		encoder(enc), prev_n1_encoder_angle(NAN), prev_n2_encoder_angle(NAN),
		_Ts(1.0f), _1_Ts(1.0f), _Tun(0.0f), _DTs(0.0f),
		_L1Ts(0.0f), _L2Ts(0.0f),
		_Qe(0.0f), _1_Qe(0.0f),
		position_error(0.0f),
		est_position(NAN), est_angle(NAN), est_speed(NAN), est_turn(NAN),
		count_error(0), maxcount_error(1),
		max_turn(max_turn), half_turn(max_turn/2), repeat_counter(0), max_repeat(8),
		prev_angle(NAN), prev_turn(NAN), tuning(tuning_enable) {}

void processing::TrackingPositionEncoder::configure( const Setting & setting ) {

    _L1Ts = setting.L1 * _Ts,
    _L2Ts = setting.L2 * _Ts;

    _Tun = math::constants::pi_2 / setting.max_speed;
    _DTs = setting.max_speed * _Ts;

    _Qe = setting.quant_error;
    _1_Qe = setting.quant_error > 0.0f ? 1.0f / setting.quant_error : 0.0f;

    _demping = setting.error_demping;

    turn_offset = set_turn_offset = setting.turn_offset;

}

void processing::TrackingPositionEncoder::setSampleTime( float Ts ) {

	_L1Ts = _L1Ts * Ts / _Ts,
	_L2Ts = _L2Ts * Ts / _Ts;
	_DTs  = _DTs  * Ts / _Ts;
	maxcount_error = static_cast<unsigned int>( _Tun / Ts );
    _Ts = Ts;
    _1_Ts = 1.0f / Ts;

}

float processing::TrackingPositionEncoder::turnover(float raw_turn) {

    float encoder_turn;

    using namespace std;

    if( isfinite(raw_turn) ) {
        uint32_t turn = raw_turn + turn_offset;
        int32_t turnover = turn % max_turn;
        int32_t turnoff = turnover > half_turn ? turnover - max_turn : turnover;
        encoder_turn = turnoff;
    } else
        encoder_turn = NAN;

    return encoder_turn;

}

void processing::TrackingPositionEncoder::miss( const float demping ) {

    if (maxcount_error > count_error) {

        position_error = position_error * demping;
        count_error++;

    } else
        position_error = NAN;

}

void processing::TrackingPositionEncoder::angle_tuning(float raw_angle,
                                                       float raw_turn,
                                                       float & encoder_angle,
                                                       float & encoder_turn) {

    using namespace std;

    const unsigned int on_speed_repeat =
            isfinite(est_speed) ? ( (_Qe / fabsf(est_speed) ) * _1_Ts) : max_repeat;

    if( repeat_counter > on_speed_repeat)
        repeat_counter = 0;

    bool data_actual = not repeat_counter;

    if( data_actual and isfinite(raw_angle) and isfinite(raw_turn)
            and not prev_sample_actual ) {

        float turn_mismatch = prev_turn - raw_turn;
        float prev_angle_in_same_turn = prev_angle
                + turn_mismatch * math::constants::pi2;

        float average_angle = (raw_angle + prev_angle_in_same_turn) * 0.5f;

        if( average_angle > math::constants::pi2 ) {

            encoder_angle = average_angle - math::constants::pi2;
            encoder_turn = raw_turn + 1;

        } else if (average_angle < 0.0f) {

            encoder_angle = average_angle + math::constants::pi2;
            encoder_turn = raw_turn - 1;

        } else {

            encoder_angle = average_angle;
            encoder_turn = raw_turn;

        }

        prev_angle = raw_angle;
        prev_turn = raw_turn;

    }

    prev_sample_actual = data_actual;

}

void processing::TrackingPositionEncoder::process() {

    using namespace std;

    if( turn_offset != set_turn_offset ) {
        prev_turn = prev_turn + ( set_turn_offset - turn_offset );
        est_turn = est_turn + ( set_turn_offset - turn_offset );
        turn_offset = set_turn_offset;
    }

    driver::IEncoder::Position raw = encoder.getPosition();

    float raw_turn = turnover( raw.first );
    float raw_angle = raw.second;

    float encoder_angle = raw_angle;
    float encoder_turn  = raw_turn;

    if( raw_angle == prev_angle and repeat_counter < max_repeat )
        ++repeat_counter;
    else
        repeat_counter = 0;

    if( tuning )
        angle_tuning(raw_angle, raw_turn, encoder_angle, encoder_turn);

    bool data_actual = not repeat_counter;

    if( isfinite(encoder_angle) ) {
        if( isfinite( est_position ) ) {

            float delta_n1 = prev_n1_encoder_angle - encoder_angle;
            float delta_n2 = prev_n2_encoder_angle - encoder_angle;

            if(delta_n1 > math::constants::pi)
                delta_n1 = delta_n1 - math::constants::pi2;
            else if (delta_n1 <= -math::constants::pi)
                delta_n1 = delta_n1 + math::constants::pi2;

            if(delta_n2 > math::constants::pi)
                delta_n2 = delta_n2 - math::constants::pi2;
            else if (delta_n2 <= -math::constants::pi)
                delta_n2 = delta_n2 + math::constants::pi2;

            prev_n2_encoder_angle = prev_n1_encoder_angle;
            prev_n1_encoder_angle = encoder_angle;

            if( data_actual ) {

                if( not( fabsf( delta_n1 ) > _DTs && fabsf( delta_n2 ) > _DTs ) ) {

                    position_error = encoder_angle - est_angle;
                    count_error = 0;

                    if(position_error > math::constants::pi)
                        position_error = position_error - math::constants::pi2;
                    else if (position_error <= -math::constants::pi)
                        position_error = position_error + math::constants::pi2;

                } else miss( _demping );

            } else {

                position_error = position_error;
                count_error = 0;

            }

        } else if( isfinite(encoder_turn) ) {

            prev_n2_encoder_angle = prev_n1_encoder_angle = encoder_angle;

            est_turn = encoder_turn;
            est_angle = encoder_angle;
            est_position = est_angle + est_turn * math::constants::pi2;

            position_error = est_speed = 0.0f;

        }

    } else miss( _demping );

    est_speed = est_speed + position_error * _L2Ts;
    est_angle = est_angle + est_speed * _Ts + position_error * _L1Ts;

    if( est_angle < 0.0f ) {

        est_turn = est_turn - 1.0f;
        est_angle += math::constants::pi2;

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

        est_turn = est_turn + 1.0f;
        est_angle -= math::constants::pi2;

    }

    est_position = est_angle + est_turn * math::constants::pi2;

}

void processing::TrackingPositionEncoder::setTurnOffset(float turn) {

    set_turn_offset = turn;

}