/*
 * EncoderMultiturnWraper.h
 *
 *  Created on: 20 сент. 2021 г.
 *      Author: Kruglij
 */

#ifndef UMLIBRARY_DRIVER_ENCODERMULTITURNWRAPER_H_
#define UMLIBRARY_DRIVER_ENCODERMULTITURNWRAPER_H_

#include <cmath>
#include <stdint.h>

#include "../common/Counter.hpp"
#include "../common/DoubleBuffer.hpp"
#include "../math/math_inc.hh"

#include "../systemic/IProcess.hh"

#include "IEncoder.hh"

namespace driver { namespace detail {

class EncoderMultiturnWraper : public IEncoder, public systemic::IProcess {
private:

    IEncoder & single_turn_encoder;

    Counter<unsigned short> error_counter;

    DoubleBuffer< std::pair<Turn, Angle> > data;

    

    float prev_angle = NAN;
    float max_angle_delta = NAN;

public:
    struct TurnCounter {
        unsigned short max_count = 0;
        unsigned short num_count = 0;

        operator float();

        TurnCounter & operator++();
        TurnCounter & operator--();
        TurnCounter & operator=(unsigned short new_num_count);

        void setMaxCount(unsigned short new_max_count);
    } turn_counter;

    struct Setting {
        float max_angle_delta;              //!<Изменение между двумя значениями угла больше данного порога считаются ошибкой.
        uint16_t num_turn;                  //!<Количество оборотов.

        bool isValid() const {
            return max_angle_delta < math::constants::pi2 and num_turn;
        }
    };

    bool configure( Setting & config );
    void reset();

    virtual float getTurn() const override { return data.read().first; }
    virtual float getAngle() const override { return data.read().second; }
    virtual std::pair<Turn, Angle> getPosition() const override { return data.read(); }

    bool isValidData() const { return !( error_counter.value() ); }
    bool isFailure() const { return static_cast<bool>(error_counter); }

    void process();

    EncoderMultiturnWraper( IEncoder & encoder, unsigned short max_error );
};

} /* namespace detail */
} /* namespace driver */

#endif /* UMLIBRARY_DRIVER_ENCODERMULTITURNWRAPER_H_ */