/*
 * PositionMixerv2.h
 *
 *  Created on: 20 июл. 2020 г.
 *      Author: user
 */

#ifndef UMLIBRARY_DRIVER_POSITIONMIXERV2_H_
#define UMLIBRARY_DRIVER_POSITIONMIXERV2_H_

#include "IncrementCounter.hh"
#include "IResolver.hh"
#include "HiperfacePositionData.hh"

#include <cmath>

namespace driver {

class PositionMixer_v2 {
private:
    //Источники данных о положении:
    IncrementCounter increment_counter;
    driver::IResolver & resolver;

    const unsigned short step_per_turn;     //!< Битов абсолютной (цифровой) позиции в механическом обороте.

    const float resolver_to_rad;            //!< Коэффициент преобразования координаты по резольверу в абсолютную.

    const unsigned long line_in_turn;
    const unsigned long max_turn_value;

    const unsigned long long max_angle_code;

    mutable bool phase_error_flag = false;          //!< Несоответствие угла по резольверу и по счетчику.
    bool scrt_valid = false;

    PositionComponents last_position;
    bool new_code_after_resolver_fault = false;

    bool is_sector_synchronized = false;
    unsigned long counter_before_synchronized = 0;

    short sector_offset:2;

    const float sector_deviation;

    const unsigned short resolver_hyst;             //! Количество подряд валидных сигналов синуса и косинуса.
    const unsigned short counter_hyst;              //! Количество подряд невалидных сигналов синуса и косинуса.
    unsigned short resolver_valid_value_count = 0;
    unsigned short resolver_invalid_value_count = 0;

    const float half_increment;     //!<Половина дистанции между отсчетами, рад.

    PositionCode incSinCosPeriod(PositionCode code);
    PositionCode decSinCosPeriod(PositionCode code);

public:
    /// Битов абсолютной (цифровой) позиции в периоде синуса/косинуса. Всегда.
    static const unsigned short periods_in_turn_power = 5;

    /// Количество отсчетов инкрементального энкодера в одном периоде синуса/косинуса, степень двойки. Всегда.
    static const unsigned short line_in_period_power = 2;

    /// Проверка источников позиции - потеря квадратурного сигнала - отказ.
    bool isValid() const    { return increment_counter.isValid(); }

    bool isPhaseError() const {
        bool error = phase_error_flag;
        if(phase_error_flag) phase_error_flag = false;
        return error;
    }

    /// Признак использования данных от СКВТ.
    bool isScrtValid() const { return scrt_valid and is_sector_synchronized; }

    /// Сброс счетчиков.
    void reset();

    //!Предоставляет смикшированные данные в конечном виде.
    PositionData calculatePosition();

    /// Предоставляет отдельные данные о положении со всех источников.
    PositionComponents getPositionComponents() const;

    /// Установка абсолютного положения в отсчетах абсолютного энкодера.
    void setAbsolutePosition(unsigned long absolute_position);

    PositionMixer_v2( peripheral::ICounter & counter, unsigned long sin_period_resolution, unsigned long max_turn_value,
                      driver::IResolver & resolver, unsigned short resolver_hyst, unsigned short counter_hyst, float sector_deviation );

private:
    PositionCode cross_correction(const PositionCode & position, const float period_angle);
    void sector_synchronization(const PositionCode & position, const float period_angle);

    void sector_resynchronizarion();
};

} /* namespace driver */

#endif /* UMLIBRARY_DRIVER_POSITIONMIXERV2_H_ */