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

#ifndef UMLIBRARY_DRIVER_INCREMENTCOUNTER_HH_
#define UMLIBRARY_DRIVER_INCREMENTCOUNTER_HH_

#include "../peripheral/ICounter.hh"

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

namespace driver {

//!Код позиции.
struct PositionCode {
    long angle_data;        //!< Положение внутри оборта в отсчетах.
    unsigned long turn;     //!< Номер оборота.

    //
    static PositionCode applyOffset(const PositionCode & pos, const PositionCode & offset,
                                    const long line_in_turn, const unsigned long max_turn_value) {

        long temp_angle_data = pos.angle_data + offset.angle_data;

        long temp_turn = temp_angle_data / line_in_turn;

        temp_angle_data = temp_angle_data - temp_turn * line_in_turn;

        if( temp_angle_data < 0 ) {
            temp_angle_data += line_in_turn;
            temp_turn -= 1;
        }

        temp_turn += pos.turn + offset.turn;
        temp_turn -= ( temp_turn / max_turn_value ) * max_turn_value;

        return { temp_angle_data, temp_turn };
    }
};

//!Класс реализует инкрементальный датчик положения, на основе модуля квадратурного энкодера.
class IncrementCounter {
private:
    peripheral::ICounter & counter;

    /// Количество отсчетов в одном механическом обороте датчика.
    const unsigned long line_in_turn;

    /// Количество механических оборотов многооборотного датчика.
    const unsigned long max_turn_value;

    /// Коэффициент преобразования кода угла в угловое положение.
    const float _1_line_to_rad = math::constants::pi2 / line_in_turn;

    PositionCode pos_code = { .angle_data = 0, .turn = 0 };

public:

    float position_to_rad(unsigned long position) const                     { return _1_line_to_rad*position; }

    unsigned long position_to_increments(const PositionCode & code) const   { return code.turn*line_in_turn + code.angle_data; }

    /// Обновление показаний датчика.
    operator PositionCode();

    PositionCode toCorrectPositionCode(PositionCode code) {
        if (code.turn >= max_turn_value) {
            code.turn -= static_cast<int>(code.turn/max_turn_value)*max_turn_value;
        }

        return { code.angle_data, code.turn };
    }

    bool isValid() const            { return not counter.isError(); }

    void resetCounter()             { pos_code.angle_data = pos_code.turn = 0; counter.resetError(); }

    void setCounter(const PositionCode & position_code) { pos_code = position_code; }

    IncrementCounter(peripheral::ICounter & counter, unsigned long line_in_turn, unsigned long max_turn_value)
    : counter(counter), line_in_turn(line_in_turn), max_turn_value(max_turn_value) {}

};

} /* namespace driver */

#endif /* UMLIBRARY_DRIVER_INCREMENTCOUNTER_HH_ */