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

#ifndef SOURCE_PROCESSING_TRACKINGPOSITIONSCRT_H_
#define SOURCE_PROCESSING_TRACKINGPOSITIONSCRT_H_

#include "../driver/IResolver.hh"

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

namespace processing {

//class Observer_FirstOrder {
//private:
//    float est_dx;
//    float est_x;
//
//    const float est_range;
//};

//!Класс обработки путевой информации из данных от резольвера.
/*!Модуль использует интерфейс резольвера для получения исходных данных.
 * Исходными данными является квадратурный сигнал позиции.
 * Восстанавливает скорость, позицию из квадратурного сигнала.
 */
class TrackingPositionScrt {
public:
    typedef driver::IResolver TrackerInterface;

    struct Setting {
        float L1;               //!<Коэффициент следящей системы, установить 1000.
        float L2;               //!<Коэффициент следящей системы, установить1000^2/2.
        float ThetaNull;        //!<Угол коммутации, рад.
        unsigned long Np;       //!<Количество пар полюсов двигателя.

        float scrt_power_max;   //!<Максимальное значение суммы квадратов для синус-косинусного вращающегося трансформатора.
        float scrt_power_min;   //!<Минимальное значение суммы квадратов для синус-косинусного вращающегося трансформатора.
        float scrt_speed_max;   //!<Максимальная скорость синус-косинусного вращающегося трансформатора, рад/с.

        unsigned long Nr;       //!<Количество пар полюсов резольвера.

        bool isValid();

        Setting() : L1(0.0f), L2(0.0f), ThetaNull(NAN), Np(0xFFFFu),
                scrt_power_max(0.0f), scrt_power_min(0.0f), scrt_speed_max(0.0f), Nr(0xFFFFu) {}

    };


private:
    driver::IResolver & scrt;

    //Модель датчика.
    float est_sens_speed;
    float est_sens_position_pi2;
    float est_sens_turn;
    float est_sens_error;

    //todo: Вынести представления в отдельные модули.

    //Представление в электрических координатах.
    float est_theta;
    float est_omega;

    //Представление в физических координатах.
    float est_position;
    float est_speed;

    unsigned int error_counter; //!<Счетчик ошибки.

private:
    float _Ts;

    //Данные расчета ошибки:
    float _power_max;   //!<Максимально-допустимая сумма квадратов сигналов СКВТ.
    float _power_min;   //todo Проверить почему здесь!
    float _Tun; //!<Допустимое время без данных.
    unsigned int _Nerr;
    float prev_scrt_sin;
    float prev_scrt_cos;
    float prev_error;

    //Настройки наблюдателя позиции.
    float _L1Base;
    float _L2Base;
    float _L1Ts;
    float _L2Ts;

    //Настройки предстваления в электрических координатах.
    float _ThetaZero;
    float _Np_Nr;

    //Настройки представления в физических координатах.
    float _1_Nr;
    float _pi2_Nr;

    void bad_data_handler();

    ~TrackingPositionScrt() = default;
public:
    TrackingPositionScrt( driver::IResolver & resolver );

    void configure( const Setting & setting );
    void setSampleTime( float Ts );

    void process();

    //!Функция позволяет получить текущую позицию.
    const float & position()    { return est_position;  }
    //!Функция позволяет получить текущую скорость.
    const float & speed()       { return est_speed; }

    //!Функция позволяет получить текущий электрический угол.
    const float & theta()       { return est_theta;     }
    //!Функция позволяет получить текущий электрическую скорость.
    const float & omega()       { return est_omega;     }

    const float & pos_error()   { return est_sens_error;     }

    bool isError() const        { return error_counter == _Nerr; }
};


}



#endif /* SOURCE_PROCESSING_TRACKINGPOSITIONSCRT_H_ */