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

#ifndef SOURCE_PROCESSING_TRACKINGPOSITIONENCODER_H_
#define SOURCE_PROCESSING_TRACKINGPOSITIONENCODER_H_

#include "../driver/IEncoder.hh"

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

#include <stdint.h>

namespace processing {

//!Модуль осуществляет восстановление путевой информации по данным полученным от цифрового энкодера.
/*В основе алгоритма восстановления лежит наблюдатель первого порядка.
 * Производиться учет таких факторов как:
 *  * Дискретизация данных энкодера по уровню;
 *  * Квантование данных энкодера по времени;
 *  * Пропуск данных и исправление единичных сбоев энкодера;
 *  * Смещение "нулевого" оборота данных для работы в комфортной шкале позиции.
 */
class TrackingPositionEncoder {
public:
    typedef driver::IEncoder TrackerInterface;

	struct Setting {
		float L1;               //!<Коэффициент следящей системы, коррекция позиции.
		float L2;               //!<Коэффициент следящей системы, коррекция скорости.
		float max_speed;		//!<Максимально допустимая скорость энкодера.
		float quant_error;	    //!<Квантование ошибки следящей системы.
		float error_demping;    //!<Коэффициент демпфирования системы.
		float turn_offset;      //!<Смещение оборотов системы координат.

		bool isValid() {

		    using namespace std;

		    const Setting notValidSetting;

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

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

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

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

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

            if( error_demping < 0.0f or error_demping > 1.0f )
                error_demping = notValidSetting.error_demping;

            if( not isfinite( turn_offset ) )
                turn_offset = notValidSetting.turn_offset;

		    bool result =
		                L1           != notValidSetting.L1
		            and L2           != notValidSetting.L2
		            and max_speed     != notValidSetting.max_speed
		            and quant_error  != notValidSetting.quant_error
		            and error_demping != notValidSetting.error_demping
		            and turn_offset != notValidSetting.turn_offset;

		    return result;

		}

		Setting() : L1(NAN), L2(NAN), turn_offset(NAN),
		        max_speed(NAN), quant_error(-1.0f), error_demping(-1.0f) {}

	};

private:
	driver::IEncoder & encoder;

	float prev_n1_encoder_angle;
	float prev_n2_encoder_angle;

	float position_error;
	float est_position;
	float est_speed;
	float est_angle;
	float est_turn;

	float set_turn_offset;
	float turn_offset;

	float _Ts;
	float _1_Ts;
	float _L1Ts;
	float _L2Ts;
	float _Tun;     //!<Допустимое время без данных.
	float _DTs;	    //!<Максимальное приращение угла за цикл.
	float _Qe;	    //!<Кванование ошибки следящей системы.
	float _1_Qe;	//!<Кванование ошибки следящей системы.
	float _demping; //!<

	unsigned int maxcount_error;
	unsigned int count_error;

	unsigned int repeat_counter;
    unsigned int max_repeat;

    float prev_angle;           //!<Предыдущее значение угла от энкодера.
    float prev_turn;            //!<Предыдущее значение оборотов от энкодера.
    bool prev_sample_actual;    //!<Предыдущее измерение актуально.

	uint32_t max_turn;
	uint32_t half_turn;

    float turnover(float raw_turn);
    void miss(const float demping);
    void angle_tuning(float raw_angle, float raw_turn, float &encoder_angle,
                      float &encoder_turn);

    const bool tuning;

public:
	TrackingPositionEncoder( driver::IEncoder & encoder, uint32_t max_turn, bool tuning = true );
	~TrackingPositionEncoder() = default;

	void configure( const Setting & setting );

	void setSampleTime( float Ts );
	void process();

	//!Метод позволяет установить новое смещение нулевого оборота.
	void setTurnOffset( float turn_offset );

	//!Текущая оценка позиции.
	const float & position()    { return est_position;  }
	//!Текущая оценка скорости
	const float & speed()       { return est_speed;     }

	const float & pos_error() 	{ return position_error; }

	//!Оценка оборотов.
    const float & turn()        { return est_turn;  }

    //!Оценка угла.
    const float & angle()       { return est_angle;  }

	//!Функция возращает признак отказа в обработке положения/скорости.
	bool isFailure() const 		{ return count_error == maxcount_error; }

};

} /* namespace processing */

#endif /* SOURCE_PROCESSING_TRACKINGPOSITIONENCODER_H_ */