/*
 * PositionMixer.h
 *
 *  Created on: 2 мар. 2020 г.
 *      Author: user
 */

#ifndef UMLIBRARY_DRIVER_POSITIONMIXER_HH_
#define UMLIBRARY_DRIVER_POSITIONMIXER_HH_

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

#include <cmath>

namespace driver {

//!Класс реализует вычисление углового положения на основе инкрементального счетчика и резольвера.
class PositionMixer {
private:
    //Источники данных о положении:
    IncrementCounter increment_counter;
    driver::IResolver & resolver;

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

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

    //Это значение должно быть гарантировано больше чем значение в радианах, когда компараторы еще не переключились,
    //а угол по резольверу перешел через логическую границу переключения.
    const float mistrust_angle;

    //Константы для корректировки углового положения:
    //Ближайшая точка от нуля справа.
    const float plus_zero_point_rad;

    //Ближайшая точка от нуля слева.
    const float minus_zero_point_rad;

    //Ноль радиан слева.
    const float minus_zero_rad;

//    unsigned long getTurnFromAbsolutePosition(unsigned long absolute_position) const  {
//        return absolute_position >> step_per_turn;
//    }
//
//    unsigned long getAngleFromAbsolutePosition(unsigned long absolute_position) const {
//        return absolute_position & static_cast<unsigned long>( ( 1 << step_per_turn ) - 1 );
//    }

    //Код угла, в рамках которого происходит уточнение положения по резольверу.
    short pos_code = 0;

    //Показывает несоответсвие кода угла сигналам от резольвера.
    bool phase_error_flag = false;

public:
    /// Разрядность датчика.
    //    const unsigned short total_number_of_step_power = 24;

    /// Битов абсолютной (цифровой) позиции в периоде синуса/косинуса. Всегда.
    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(); }

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

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

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

    void setAbsolutePosition(unsigned long absolute_position);

    PositionMixer(peripheral::ICounter & counter, unsigned long sin_period_resolution, unsigned long max_turn_value, driver::IResolver & resolver, float mistrust_angle)
    : increment_counter(counter, 1 << (sin_period_resolution + line_in_period_power), max_turn_value), resolver(resolver),
      resolver_to_rad(1.f/(1 << sin_period_resolution)), step_per_turn(sin_period_resolution + periods_in_turn_power),
      mistrust_angle(mistrust_angle), plus_zero_point_rad(mistrust_angle*resolver_to_rad), minus_zero_point_rad((math::constants::pi_2 - mistrust_angle)*resolver_to_rad),
      minus_zero_rad(math::constants::pi_2*resolver_to_rad) {}

};

} /* namespace driver */

#endif /* UMLIBRARY_DRIVER_POSITIONMIXER_HH_ */